卡尔曼滤波（一）

1、理论部分

卡尔曼滤波使用的准则是线性最小方差估计（LMMSE），因此，经典卡尔曼滤波适用于线性高斯系统，系统模型如下：

            

W和V分别代表过程噪声和量测噪声，数学期望为0，方差分别为Q和R，X代表系统状态。本文假定已有一定的线性系统基础，因此不对上图中公式做具体介绍。并且本文着重介绍公式的由来、公式为什么是这种形式，至于其中的物理意义之类的就不多说了。

首先给出卡尔曼滤波的5条公式：

                        

正式计算时每个循环中都顺序执行上述公式，即可实现卡尔曼滤波，下面说下由来。

首先，第四个公式代表卡尔曼滤波的校正，等式右边由两项组成，第一项代表由上一时刻的最优估计值递推估计的本时刻状态值，即：本时刻的经验估计值。第二项代表估计值与观测值之间的误差，两项相加，代表，由本时刻观测值与估计值之间的误差对估计值进行修正，K为修正作用的大小。这种形式，从物理意义上来说，是比较合理的，通过使用系统的经验估计值以及观测值，综合得出系统的估计值。

举个例子，上一时刻房间温度最优值为26度，由于温度变化缓慢，则可以根据经验估计本次温度也为26度，即四式右边第一项，而此时温度计读数为28度，那么本时刻温度应该是多少？不知道温度计精度的情况下，可以对两个数据求平均，为27度，即T=26+0.5*（28-26）=27。这里0.5就是本例子中的修正系数，那么假如温度计精度很高，将修正系数增大一些，比如增大到0.9，那么结果T=26+0.9*（28-26）=27.8，可以看到滤波结果很接近温度计读数，增大到1则滤波结果就是温度计读数，相反，如果温度计精度很差，那么减小修正系数，则可以使滤波结果更接近等式右边第一项得估计值。因此，合理调整K的大小，可以得到很好的滤波结果，卡尔曼的精髓就在于，能够动态调整K的大小，得到最优的估计解。

那么如何得到K呢？引入正交投影定理的概念 

            

用图表示更加形象一些

             

X为待估计状态，这里假设x的估计解有x=Az+b的形式，定理内容为：状态x在观测z上的正交投影就是x在z上的线性最小方差估计，由定理可以得出 

                                                                       

这里E为求数学期望。

由此可以得到K的求解公式  

                     

                 

全部带入即可求得修正系数K的表达式为

       

可以看到，K的求解又引入了P(k+1|k)项，继续计算 

                     

这里，又引入了P(k|k)项 

               

作变形 

                  

至此，卡尔曼滤波的公式推导完毕，过程看似全部都是密密麻麻的公式，实际上就是利用：过程噪声、观测噪声的数学期望为0，以及正交投影定理的两个公式，代入一步步写出即可，并不需要其他高端的知识。

公式中P代表方差，P(k+1|k)为k时刻转移至k+1时刻的方差，为“预测方差”，P(k|k)为估计方差，^为估计值，~为误差值。

2、实践部分

以一个例子说明卡尔曼滤波的用法。Kalman滤波在船舶GPS定位系统中的应用。

假定用GPS观测船舶的位置，可以观测到横向和纵向的位置值。船舶沿某方向匀速直线行驶，将由海风海浪引起的随机加速度认为是和观测不相关的随机白噪声，初始位置为（-100m，200m），水平速度2m/s，垂直速度20m/s，GPS观测周期为1秒，系统方程为

                   

代码：

function main

clc;

clear;

close all;

T=1;

N=80/T;

X=zeros(4,N);

X(:,1)=[-100,2,200,20];

Z=zeros(2,N);

Z(:,1)=[X(1,1),X(3,1)];

delta_w=1e-3;

Q=delta_w*diag([0.5,1,0.5,1]) ;

R=50*eye(2);

F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];

H=[1,0,0,0;0,0,1,0];

%------------------------------------------------------------------%

for t=2:N

    X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);

    Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);  

end

Xkf=zeros(4,N);

Xkf(:,1)=X(:,1);

% Xkf(:,1) = [-50,2,110,20];

P0=eye(4);

for i=2:N

    Xn=F*Xkf(:,i-1);

    P1=F*P0*F'+Q;

    K=P1*H'*inv(H*P1*H'+R);

    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);

    P0=(eye(4)-K*H)*P1;

end

for i=1:N

    Err_Observation(i)=RMS(X(:,i),Z(:,i));

    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));

end

figure

hold on;box on;

plot(X(1,:),X(3,:),'-g');

plot(Z(1,:),Z(2,:),'-b.');

plot(Xkf(1,:),Xkf(3,:),'-r+');

legend('真实轨迹','观测轨迹','滤波轨迹')

figure

hold on; box on;

plot(Err_Observation,'-ko','MarkerFace','g')

plot(Err_KalmanFilter,'-ks','MarkerFace','r')

legend('滤波前误差','滤波后误差')

%------------------------------------------------------------------%

function dist=RMS(X1,X2);

if length(X2)<=2

    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );

else

    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );

end

%------------------------------------------------------------------%