卡尔曼滤波原理及工具箱应用

在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！

卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

1、卡尔曼滤波

（Introduction to the Kalman Filter）

为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。

就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

2、卡尔曼滤波算法

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

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.从下面的图可以看出估计值即滤波与其真实值很接近，但是量测值与真实值相去甚远。
---------------------
作者：Particlefilter
来源：CSDN
原文：https://blog.csdn.net/u010545732/article/details/18989051
版权声明：本文为博主原创文章，转载请附上博文链接！

3、卡尔曼滤波工具箱

原文地址：http://www.cs.ubc.ca/~murphyk/Software/Kalman/kalman.html

Written by Kevin Murphy, 1998.

Last updated: 7 June 2004.

This toolbox supports filtering, smoothing and parameter estimation (using EM) for Linear Dynamical Systems.

Functions

kalman_filter
kalman_smoother - implements the RTS equations
learn_kalman - finds maximum likelihood estimates of the parameters using EM
sample_lds - generate random samples
AR_to_SS - convert Auto Regressive model of order k to State Space form
SS_to_AR
learn_AR - finds maximum likelihood estimates of the parameters using least squares

What is a Kalman filter?

For an excellent web site, see Welch/Bishop's KF page. For a brief intro, read on...

A Linear Dynamical System is a partially observed stochastic process with linear dynamics and linear observations, both subject to Gaussian noise. It can be defined as follows, where X(t) is the hidden state at time t, and Y(t) is the observation.

   x(t+1) = F*x(t) + w(t),  w ~ N(0, Q),  x(0) ~ N(X(0), V(0))
   y(t)   = H*x(t) + v(t),  v ~ N(0, R)

The Kalman filter is an algorithm for performing filtering on this model, i.e., computing P(X(t) | Y(1), ..., Y(t)).
The Rauch-Tung-Striebel (RTS) algorithm performs fixed-interval offline smoothing, i.e., computing P(X(t) | Y(1), ..., Y(T)), for t <= T.

Example of Kalman filtering

Here is a simple example. Consider a particle moving in the plane at constant velocity subject to random perturbations in its trajectory. The new position (x1, x2) is the old position plus the velocity (dx1, dx2) plus noise w.

[ x1(t)  ] =  [1 0 1 0] [ x1(t-1)  ] + [ wx1  ]
[ x2(t)  ]    [0 1 0 1] [ x2(t-1)  ]   [ wx2  ]
[ dx1(t) ]    [0 0 1 0] [ dx1(t-1) ]   [ wdx1 ]
[ dx2(t) ]    [0 0 0 1] [ dx2(t-1) ]   [ wdx2 ]

We assume we only observe the position of the particle.

[ y1(t) ] =  [1 0 0 0] [ x1(t)  ] + [ vx1 ]
[ y2(t) ]    [0 1 0 0] [ x2(t)  ]   [ vx2 ]
                       [ dx1(t) ] 
                       [ dx2(t) ]

Suppose we start out at position (10,10) moving to the right with velocity (1,0). We sampled a random trajectory of length 15. Below we show the filtered and smoothed trajectories.

The mean squared error of the filtered estimate is 4.9; for the smoothed estimate it is 3.2. Not only is the smoothed estimate better, but we know that it is better, as illustrated by the smaller uncertainty ellipses; this can help in e.g., data association problems. Note how the smoothed ellipses are larger at the ends, because these points have seen less data. Also, note how rapidly the filtered ellipses reach their steady-state (Ricatti) values. (Click here to see the code used to generate this picture, which illustrates how easy it is to use the toolkit.)

What about non-linear and non-Gaussian systems?

For non-linear systems, I highly recommend the ReBEL Matlab package, which implements the extended Kalman filter, the unscented Kalman filter, etc. (See Unscented filtering and nonlinear estimation, S Julier and J Uhlmann, Proc. IEEE, 92(3), 401-422, 2004. Also, a small correction.)

For systems with non-Gaussian noise, I recommend Particle filtering (PF), which is a popular sequential Monte Carlo technique. See also this discussion on pros/cons of particle filters. and the following tutorial: M. Arulampalam, S. Maskell, N. Gordon, T. Clapp, "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking," IEEE Transactions on Signal Processing, Volume 50, Number 2, February 2002, pp 174-189 (pdf cached here The EKF can be used as a proposal distribution for a PF. This method is better than either one alone. The Unscented Particle Filter, by R van der Merwe, A Doucet, JFG de Freitas and E Wan, May 2000.Matlab software for the UPF is also available.

Gatsby reading group on nonlinear dynamical systems

Other packages for Kalman filtering and state-space models

SSPIR package in R. The system identification toolbox from the Mathworks implements many classical algorithms.ARfit is an excellent package for autoregressive models.Zoubin Ghahramani has matlab code for EM in LDS's which is similar to mine, but is partially written in C.KBF, an implementation of the Kalman filter-smoother in Omatrix, a (supposedly faster) version of matlab.Le Sage's econometrics toolbox, contains lots of excellent matlab time series modelling functionsEconometric Links Econometrics Journal. Most of the software is either commercial or written in Gauss, which is similar to Matlab.SSF pack is a set of C routines for state-space filtering.Stamp is a commercial package for structural time series analysis. Statistical Time Series Analysis Toolbox O matrix Statistical Time Series Analysis Toolbox