卡尔曼滤波simulink例子，位移和速度2变量估计

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;