1、数学模型如下:
2、simulink建模
3、滤波结果如下图
4、系统仿真程序与滤波程序
%Plant
function [u]=kalman_sim(u1,u2,u3,u4)
%u1:输入加速度
dv=u1;
%u2:系统噪声W
W=u2;
%u3:测量噪声V
V=u3;
%u4:时间t
t=u4;
persistent v s Ts
if t==0
Ts=0.1;
s=0.0;
v=0.0;
z=s+V;
end
if t>0
s=s+v*Ts+0.5*Ts^2*(dv+W);
v=v+Ts*dv+Ts*W;
z=s+V;
end
u(1)=s;
u(2)=v;
u(3)=t;
u(4)=z;
%Plant
function [u]=kalman_sim(u1,u2,u3)
%u1:输入加速度
dv=u1;
%u2:测量位移
z=u2;
%u3:时间t
t=u3;
persistent x Ts Q R P C Jf B
if t==0
Ts=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 value
u=x; %Filtered signal
errcov=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;