用simulink两个函数模块完成,一个完成车辆动力学仿真,一个对仿真的结果进行卡尔曼滤波估算车辆质量。
链接:https://pan.baidu.com/s/1rCWkWanyAPRX4kWSyv9BQA
提取码:1234
理论可参考基于扩展卡尔曼滤波的车辆质量与道路坡度估计 - 百度文库(注意其系统方程没考虑旋转质量的影响)
车辆动力学模型,系统状态方程及其雅克比矩阵为:
系统仿真函数程序为:
(车辆参数为上一篇车辆动力性经济性计算的数据,注意W为引入的系统加速度噪声,V为引入的测量速度噪声,仿真中车辆质量为60000kg)
%Plant
function [u]=kalman_sim(u1,u2,u3,u4)
%u1:输入Ttq
Ttq=u1;
%u2:系统噪声W
W=u2;
%u3:测量噪声V
V=u3;
%u4:时间t
t=u4;
persistent v m i Ts ig i0 nn g f r CD A rou qq
if t==0
Ts=0.1;
ig=1.0;
i0=6.83;
nn=0.95*0.96;
g=9.8;
f=0.011;
r=0.502;
CD=0.8;%%%%%卡车取0.8 客车0.65
A=1.92*3.09;
rou=1.2258;
qq=1.027+0.000331*i0^2.*ig.^2;%%汽车质量转换系数
v=0.0;
m=60000.0;
i=0.0;
z=v+V;
end
if t>0
dv=1/qq*(Ttq*ig*i0*nn/(m*r)-g*f-CD*A*rou*v^2/(2*m)-g*i);
v=v+Ts*dv+Ts*W;
z=v+V;
end
u(1)=v;
u(2)=m;
u(3)=i;
u(4)=z;
卡尔曼滤波函数程序为:
(注意车辆质量初始值设定为30000kg,看卡尔曼滤波可否正确估算车辆的质量60000kg)
%Plant
function [u]=kalman_sim(u1,u2,u3)
%u1:输入Ttq
Ttq=u1;
%u2:系统噪声W
z=u2;
%u3:时间t
t=u3;
%persistent v m i
persistent x Ts ig i0 nn g f r CD A rou qq Q R P C Jf
if t==0
Ts=0.1;
ig=1.0;
i0=6.83;
nn=0.95*0.96;
g=9.8;
f=0.011;
r=0.502;
CD=0.8;%%%%%卡车取0.8 客车0.65
A=1.92*3.09;
rou=1.2258;
qq=1.027+0.000331*i0^2.*ig.^2;%%汽车质量转换系数
%系统方程:
C=[1.0,0.0,0.0];
%Covariances of w:
Q=diag([Ts*0.1*Ts',0,0]);
%Covariances of v:
R=[0.1];
%初始估计值:
x=[0.0;30000;0.0];
%初始估计误差协方差:
P=diag([Ts*0.1*Ts',90000000000,0.000001]);
end
%后验,Measurement update:
%根据估计误差协方差和测量噪声协方差计算卡尔曼增益:
Kk=P*C'/(C*P*C'+R);
%计算最优估计值:
%x=A*x+Mn*(yv-C*A*x);
x=x+Kk*(z-C*x);
%计算最优估计值和真实值之间的误差协方差矩阵,为下次递推做准备:
P=(eye(3)-Kk*C)*P;
%ye=C*x; %Filtered value
u=x; %Filtered signal
errcov=C*P*C'; %Covariance of estimation error
%先验,Time update:
%根据系统状态方程计算下一状态预测值:
%x=A*x+B*u1;
Jf=eye(3);
Jf(1,1)=1-CD*A*rou*x(1)*Ts/x(2);
Jf(1,2)=(CD*A*rou*x(1)^2*r-2*Ttq*ig*i0*nn)/(2*x(2)^2*r)*Ts;
Jf(1,3)=-g*Ts;
v=x(1);
m=x(2);
i=x(3);
dv=1/qq*(Ttq*ig*i0*nn/(m*r)-g*f-CD*A*rou*v^2/(2*m)-g*i);
v=v+Ts*dv;
x(1)=v;
x(2)=m;
x(3)=i;
%预测误差协方差:
P=Jf*P*Jf'+Q;
系统仿真的不含测量噪声的车速v(可认为是真实值)、含测量噪声的车速z(测量值)、卡尔曼滤波后的车速v_e(估算值):
实际的车辆质量m与坡度i,估算的车辆质量m与坡度i: