matlab实现卡尔曼滤波(Kalman filter)

 很早以前就听说卡尔曼滤波，一直没有下功夫彻底弄懂过。一年前，听一个老师(很好的一个老师，讲得认真、负责，科研也不错)做过专讲，从而加深了对Kalman filter的理解和认识，现记录如下，与大家分享，希望对大家有用：

      美国学者卡尔曼在美国宇航局埃姆斯研究中心访问时，发现他所研究的卡尔曼滤波器对于解决阿波罗计划中的轨道预测问题很有用，随即同宇航局展开合作研究。后来阿波罗飞船的导航系统中使用了这种滤波器，这也足以说明卡尔曼滤波的有效性。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。卡尔曼滤波从一组有限的，包含噪声的观察序列中预测和估计出目标运动状态包括位移、速度、加速度。

卡尔曼滤波有两组基本的方程，状态运动方程和观测方程，这是需要预先给定的。

X(k)=A(k) X(k-1)+B U(k)+W(k)

再加上系统的测量值：

Z(k)=H (k)X(k)+V(k)

上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A称 为状态转移矩阵。可以看出状态运动方程反映了k-1时刻目标的状态与k时刻目标状态之间的关系，所以我们称之为状态运动方程。

Z(k)是k时刻的观测值，H是观测矩阵，表示人们对于k时刻状态进行观测时，获取观测量的机制。W(k)和V(k)分别表示状态方程和观测方程的噪声。这个方程是描述观测第k时刻目标状态的机理。在实际问题中，通常假设A(k)=A,H(k) = H.因此我们可以看出，在应用卡尔曼滤波时，首先必须要给出运动方程和观测方程，也就是说，要对目标运动及观测过程进行建模，并且模型是线性的，称之为 "Model-based" 。这是卡尔曼滤波最大的一个缺陷。

假设W和V为高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

首先我们要利用状态方程预测下一状态的系统。假设当前时刻为k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

X(k|k-1)=A X(k-1|k-1)+B U(k)  ………..                                 (1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。因为是利用状态方程对状态进行更新，反映了状态随时间演化的规律，因此，称之为时间更新。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance，即上面预测值相对于真实状态的误差方差阵更新公式：

P(k|k-1)=A P(k-1|k-1) A’+Q  ………                                       (2)

式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

当第k时刻的观测值Z(k)到达以后，我们希望利用它去修正第k个时刻的状态预测值。怎么去修正呢？当然是要用预测值中没有的信息去修正预测值了。因此Z(k)-H X(k|k-1)就是新的信息。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ………                  (3)

其中Kg为卡尔曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ………                    (4)

到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：

P(k|k)=（I-Kg(k) H）P(k|k-1) ………                                    (5)

其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这5个公式，可以很容易用计算机编程实现。

假设一个接近匀速运动的小球，初始位置为24，既然是“接近匀速”，必有误差，误差为0.04，状态转移矩阵是常数1。如果有一个仪器对位移进行测量，获得的是位移值，则观测矩阵也是1，测量一定有误差，其误差为0.25.从下面的图可以看出估计值即滤波与其真实值很接近，但是量测值与真实值相去甚远。

转载：https://blog.csdn.net/u010545732/article/details/18989051