To learn, to share, to debate, then comes progress.

1.算法背景

Kalman滤波算法的提出也是为了解决实际问题，斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。Kalman在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与Kalman and Bucy (1961)发表。

Kalman滤波经过几十年的发展，形成了适用于不同领域的算法，有线性滤波，非线性滤波，以及扩展的Kalman滤波等。这里只讨论简单的线性Kalman滤波。

2.算法基本原理

（1）信号模型

在对一个信号系统进行描述时，存在两种类型的信号，一种是反映系统状态变化的真实信号，一种是通过仪器测量等手段得到的观测信号，观测信号与真实之间往往不相等，因为存在仪器设计，测量操作等带来的误差，在Kalman滤波中，信号模型由两个方程决定：

状态方程：x(k)=A(k)x(k-1)+w(k-1)

量测方程：y(k)=C(k)x(k)+v(k)

其中，第一个方程反映系统状态随时间的变化，A(k)称为状态转移矩阵，其元素可随时间变化，也可以是一固定值，w为状态转移过程中可能存在的噪声。第二个方程为对系统状态的观测过程，C(k)称为量测矩阵，v为信号测量过程中存在的噪声。

（2）算法原理

对于一个信号系统，如果已知初始状态x(0)以及状态转移矩阵A(k)，那么便很容易得到任意时刻系统的状态信息，而这是在理想情况下，不受任何外界干扰时得到的推论，即这样得到的为理论值。在任一时刻，对系统进行某种手段的测量，也会得到一个值，称为观测值。

在实际应用中，是该利用理论值作为反映系统信息的信号，还是使用观测值，理论值是在严格的理论推倒下得到的系统的期望值，而观测值是实际得到的关于系统状态信息的反映。显然，理论值或观测值均存在各自的限制，将两者结合起来使用会得到更好的效果。

Kalman滤波算法中，先不考虑噪声的影响，得到状态变量与观测信号的估计值，再使用观测信号与期望信号之间的误差校正状态变量的估计值，再通过状态转移矩阵与量测方程获取更为准确的观测值。

定义变量：

x_d(k)  k时刻x的期望值

x_e(k)  k时刻x的估计值

x_c(k)  k时刻x的校正值

x_s(k)  k时刻x的估计误差 

y_e(k)  k时刻y的估计值

y_m(k)  k时刻y的观测值

y_s(k)   k时刻y的估计误差 

不考虑噪声影响时，状态变量与输出变量的估计值为：

x_e(k)=A(k)x_c(k-1)

y_e(k)=C(k)x_e(k)

输出信号的估计误差为：

y_s(k)=y_m(k)-y_e(k)

使用估计误差校正x的估计值，其中校正系数为Hk：

x_c(k)= A(k)x_c(k-1)+Hk*y_s(k)= A(k)x_c(k-1)+Hk* (y_m(k)-y_e(k))

= A(k)x_c(k-1)+Hk* (y_m(k)- C(k)x_e(k))

校正后x的误差为：

x_s(k)=x_e(k)-x_c(k)= x_e(k)-(A(k)x_c(k-1)+Hk* (y_m(k)- C(k)x_e(k)))

通过以上的推导，实现了使用观测数据对系统状态变量的校正，从而将理论值与观测值结合起来，得到更为准确的系统信息。

 

（3）Kalman滤波迭代公式

上边推导中使用误差校正状态值的系数Hk成为增益矩阵，实质为一个加权矩阵。这里使用最小均方误差作为滤波器的优化标准，将目标函数设为：

Min P(k)=E[x_s(k)^2]

得到满足要求的Hk后，即可得到最有滤波输出。

为了得到P(k)最小时的增益矩阵Hk，推导出如下的迭代公式：

x_c(k)= A(k)x_c(k-1)+Hk* (y_m(k)-y_e(k))

Hk=Pk_t*C^T(k)*(C(k)Pk_t C^T(k)+R(k))^-1

Pk_t=A(k)*Pk(k-1)* A^T(k)+Q(k-1)

P(k)=(I-HkCk)Pk_t

 

利用以上递推公式，即可实现对输入信号进行滤波，得到更为准确的状态信息。

关于Kalman滤波的算法原理，可以参考以下网站的介绍：

Prof Welch, University of North Carolina at Chapel Hill http://www.cs.unc.edu/~welch/kalman/

Wikipedia: https://en.wikipedia.org/wiki/Kalman_filter

一篇通俗易懂的博客：http://blog.csdn.net/xiahouzuoxin/article/details/39582483

 

3.算法应用与实现

以上对Kalman滤波的算法原理做了简单介绍，下面讨论如何利用Kalman滤波解决实际问题。

问题背景：一个物体在二维平面中做直线运动，x方向的速度为5，y方向的速度为10，系统状态转移过程中受到功率为0.1的噪声干扰，测量过程中两个方向上的噪声功率分布为10和12。利用Kalman滤波对该物体的运动轨迹进行处理，得到较为准确的轨迹。

问题分析 ：将物体在x方向与y方向上的运动视为相互独立的运动，并对其轨迹进行离散，得到每一时刻的状态信息。按照Kalman滤波算法的递推公式，设置初值，之后进行迭代，最后得到接近真实值的轨迹。

程序思路：生成理想轨迹---添加噪声，得到观测轨迹---设置变量初值---进行迭代---输出滤波结果---合成运动轨迹

4.结果与分析

http://s1/mw690/003qTDx1zy7bWxJf8w820&690

5.源代码

%this code applies the Kalman filtering on the motion tracking in 2D space

clear

close all

L=100;dt=1;

t=1:L;

Z=zeros(4,L);             %preallocate the array

ZZ=zeros(2,L);

X=zeros(4,L);

x0=100;y0=200;           %initial displacements and velocities 

vx0=5;vy0=10;

Z(1,:)=x0+vx0*(t-1);         %expectation values of motion

Z(2,:)=y0+vy0*(t-1);

Z(3,:)=vx0;

Z(4,:)=vy0;

Qx=1e-1;Qy=1e-1;     %variance of process noise and measurement noise

Qvx=1e-1;Qvy=1e-1;

Q=diag([Qx,Qy,Qvx,Qvy]);

Rx=12;Ry=20;    

R=diag([Rx,Ry]);

ZZ(1,:)=Z(1,:)+normrnd(0,Rx,1,L);     %measurement values    

ZZ(2,:)=Z(2,:)+normrnd(0,Ry,1,L);

% ZZ(3,:)=Z(3,:)+normrnd(0,Qvx,1,L);

% ZZ(4,:)=Z(4,:)+normrnd(0,Qvy,1,L);

A=[1 0 dt 0;                           %statte transform matrix

   0 1 0 dt;

   0 0 1 0;

   0 0 0 1];

H=[1 0 0 0;                             %measurement matrix

   0 1 0 0];

P=zeros(4,4,L);

P(:,:,1)=5*diag([Qx,Qy,Qvx,Qvy]);           %initial value of P and X

X(:,1)=[x0;y0;vx0;vy0];

E=eye(size(P(:,:,1)));

for i=2:L

    X_no=Q^0.5*normrnd(0,1,4,1);

    x_t=A*X(:,i-1)+X_no;

    P_t=A*P(:,:,i-1)*A'+Q;

    G=P_t*H'*inv((R+H*P_t*H'));

%    G=P_t/(P_t+sigma2);

    X(:,i)=x_t+G*(ZZ(1:2,i)-H*x_t);

    P(:,:,i)=(E-G*H)*P_t;

end

figure

plot(Z(1,:),Z(2,:),'k')

hold on

plot(ZZ(1,:),ZZ(2,:),'b')

hold on

plot(X(1,:),X(2,:),'r+')

 

 

legend('expectation value','measurement value','filter output',2)

title('Kalman filtering on a trajectory of 2D motion')