clear all;
close all;
clc;
%% 参数
ts=0.1;
t=30;
d=1.45;
p=-0.6;
%% 初始状态
x(1)=0;
y(1)=0;
xita(1)=0;
v=3;
w=0.2;
u=[v;w];
Z(:,1)=[-3;-3;0.2];
for k=1:1:t/ts
times(k+1)=k*ts;
X(:,k)=[cos(xita(k)) -d*sin(xita(k));sin(xita(k)) d*cos(xita(k));0 1]*u;
%% 状态更新
x(k+1)=x(k)+X(1,k)*ts;
y(k+1)=y(k)+X(2,k)*ts;
xita(k+1)=xita(k)+X(3,k)*ts;
Z_=p*Z(:,k);%每秒变化率
Z(:,k+1)=Z(:,k)+Z_*ts;%每步变化率
xc(k)=Z(1,k)+x(k);
yc(k)=Z(2,k)+y(k);
end
figure(1);
for k=1:1:length(xc)
plot(x,y,'r','Linewidth',2);
hold on;
plot(xc(k),yc(k),'sb','MarkerSize',10);
pause(0.1);
end
figure(2);
plot(times,Z(1,:),'r');
figure(3);
plot(times,Z(2,:),'r');
figure(4);
plot(times,Z(3,:),'r');