kalman滤波公式推导

 

 

 

 

kalman公式，算法步骤

首先给后验估计和误差协方差矩阵赋初值，

1，计算先验误差

2，计算先验误差协方差矩阵

3，计算卡尔曼增益

4，计算后验估计值

5，计算误差协方差矩阵

clear all;
close all;
clc;

   
ts=0.01;
%kalman赋初值
P=[0.03,0;0,0.03];
X_h=[0;0];
for i=1:1000
    
   times(i)=ts*i;
   if i==1
       
       X=[0;0];
       Z=[0;0];
       X_hxx=[0;0];
   else
       u=sin(times(i));
       X(:,i)=[1,1;0,1]*[X(1,i-1)*ts;X(2,i-1)]+[0;1]*ts*u+[0.03*rand();0.03*rand];
       Z(:,i)=[1,0;0,1]*X(:,i)+ts*[0.03*rand();0.03*rand];
       
       X_hx(:,i)=[1,1;0,1]*[X_h(1,i-1)*ts;X_h(2,i-1)]+[0;1]*ts*u;
       P_h=[1,1;0,1]*P*[1,1;0,1]'+[0.03,0;0,0.03];
       Kk=P_h*[1,0;0,1]'\([1,0;0,1]*P_h*[1,0;0,1]'+[0.03,0;0,0.03]);
       X_h(:,i)=X_hx(:,i)+Kk*(Z(:,i)-[1,0;0,1]*X_hx(:,i));
       P=([1,0;0,1]-Kk*[1,0;0,1])*P_h;
       
       X_hxx(:,i)=[1,1;0,1]*[X_hxx(1,i-1)*ts;X_hxx(2,i-1)]+[0;1]*ts*u;
   end 
    
end
plot(X_h(1,:));