1、数学模型如下：
2、simulink建模
3、滤波结果如下图
4、系统仿真程序与滤波程序
%Plantfunction [u]=kalman_sim(u1,u2,u3,u4)%u1:输入加速度dv=u1;%u2:系统噪声WW=u2;%u3:测量噪声VV=u3;%u4:时间tt=u4;persistent v s Tsif t==0 Ts=0.1;s=0.0;v=0.0;z=s+V;endif t>0s=s+v*Ts+0.5*Ts^2*(dv+W);v=v+Ts*dv+Ts*W;z=s+V;endu(1)=s;u(2)=v;u(3)=t;u(4)=z;
%Plantfunction [u]=kalman_sim(u1,u2,u3)%u1:输入加速度dv=u1;%u2:测量位移z=u2;%u3:时间tt=u3;persistent x Ts Q R P C Jf Bif t==0Ts=0.1;%系统方程：C=[1.0,0.0]; %Covariances of w:B=[0.5*Ts^2;Ts];Q=B*[0.1]*B';%Q=diag([Ts*0.1*Ts',0,0]);%Covariances of v:R=[0.1];%初始估计值：x=[0.0;0.0];%初始估计误差协方差：P=diag([0.0,0.0]);%P=diag([Ts*0.1*Ts',9000000000,0.000000]); 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(2)-Kk*C)*P;%ye=C*x;%Filtered valueu=x; %Filtered signalerrcov=C*P*C'; %Covariance of estimation error%先验，Time update:%根据系统状态方程计算下一状态预测值：%x=A*x+B*u1;Jf=eye(2);Jf(1,1)=1;Jf(1,2)=Ts;s=x(1);v=x(2);s=s+v*Ts+0.5*Ts^2*dv;v=v+Ts*dv;x(1)=s;x(2)=v;%预测误差协方差：P=Jf*P*Jf'+Q;
如果觉得《卡尔曼滤波simulink例子 位移和速度2变量估计》对你有帮助，请点赞、收藏，并留下你的观点哦！
