clear all;
clc;
m=1480;
Iz=2350;
lf=1.05;
lr=1.63;
Cf=67500;
Cr=47500;
v=25;
a1=-2*(Cf+Cr)/m;
a2=2*(Cr*lr-Cf*lf)/m;
a3=2*(Cr*lr-Cf*lf)/Iz;
a4=-2*(Cf*lf*lf+Cr*lr*lr)/Iz;
b1=2*Cf/m;
b2=2*Cf*lf/Iz;
%%%%%%%%%%%%前轮转角u,x1=y,x2=vy,x3=psi,x4=r
u(1)=0;
t(1)=0;
x1(1)=0;
x2(1)=10;
x3(1)=0.15;
x4(1)=2;
M(1)=0;
dt=0.001;
for i=1:3000
x1(i+1)=(x2(i)+v*x3(i))*dt+x1(i);
x2(i+1)=(a1/v*x2(i)+(-v+a2/v)*x4(i)+b1*u(i))*dt+x2(i);
x3(i+1)=x4(i)*dt+x3(i);
x4(i+1)=(a3/v*x2(i)+a4/v*x4(i)+b2*u(i)+M(i)/Iz)*dt+x4(i);
M(i+1)=[-3079200 -160900 -7453800 -165300]*[x1(i);x2(i);x3(i);x4(i)];
if t(i)>=0 && t(i)<=1;
u(i+1)=0.2*sin(pi*t(i));
else u(i+1)=0;
end
t(i+1)=t(i)+dt;
end
figure(1)
plot(t,x1,'b-','LineWidth',1);
title('XOY坐标下车辆横向位移');
xlabel('t(s)');
ylabel('m');
hold on
figure(2)
plot(t,x2,'b-','LineWidth',1);
title('车辆侧向速度');
xlabel('t(s)');
ylabel('m/s');
hold on
figure(3)
plot(t,x3,'b-','LineWidth',1);
title('车辆横摆角');
xlabel('t(s)');
ylabel('rad');
hold on
figure(4)
plot(t,x4,'b-','LineWidth',1);
title('横摆角速度');
xlabel('t(s)');
ylabel('rad/s');
hold on
figure(5)
plot(t,u,'b-','LineWidth',1);
title('前轮转角');
xlabel('t(s)');
ylabel('rad');
hold on
% figure(6)
% plot(v*t,x1,'b-','LineWidth',1);
% title('车辆运动轨迹');
% xlabel('m');
% ylabel('m');
% hold on
figure(7)
plot(t,M,'b-','LineWidth',1);
title('控制力矩');
xlabel('t(s)');
ylabel('Nm');
hold on