Kalman Filter

Kalman Filter是一个高效的递归滤波器，它可以实现从一系列的噪声测量中，估 计动态系统的状态。广泛应用于包含Radar、计算机视觉在内的等工程应用领域，在控制理论和控制系统工程中也是一个非常重要的课题。连同线性均方规划，卡尔曼滤波器可以用于解决LQG(Linear-quadratic-Gaussian control)问题。卡尔曼滤波器，线性均方归化及线性均方高斯控制器，是大部分控制领域基础难题的主要解决途径。

目录

■    1     应用实例

■    2    命名和发展历史

■    3    基本动态系统模型

■    4    卡尔曼滤波器

            4.1    预测

            4.2    更新

            4.3    不变量

■    5    实例

■    6    推导

           6.1    后验估计协方差矩阵推导

           6.2    Kalman 增益推导

           6.3    后验误差协方差矩阵简化

■    7    信息滤波

■    8  非线性滤波器

           8.1   扩展Kalman 滤波

           8.2   Unscented Kalman filter

■    9   Kalman-Bucy滤波

■    10  应用

■    11  参见

■    12  参考文献

■    13  外部链接

■    1    应用实例

一个简单的应用是估计物体的位置和速度；简要描述如下：假设我们可以获取一个物体的包含噪声的一系列位置观测数据，我们可以获得此物体的精确速度和位置连续更新信息。

例如，对于雷达来说，我们关心的是跟踪目标，而目标的位置，速度，加速度的测量值是时刻含有误差的，卡尔曼滤波器利用目标的动态信息，去掉噪声影响，获取目标此刻好的位置估计(滤波)，将来位置估计(预测)，也可以是过去位置估计的(插值或平滑)

■    2    命名和发展历史

这个滤波器以它的发明者Rudolf.E.Kalman 而命名，但是在Kanlman之前，Thorvald Nicolai Thiele和Peter Swerling 已经提出了类似的算法。Stanley Schmidt 首次实现了Kalman滤波器。在一次对NASA Ames Research Center访问中，卡尔曼发现他的方法对于解决阿波罗计划的轨迹预测很有用，后来阿波罗飞船导航 电脑就使用了这种滤波器。这个滤波器可以追溯到Swerling(1958),Kalman(1960),Kalman和Bucy(1961)发表的论文。

这个滤波器有时叫做Stratonovich-Kalman-Bucy滤波器。因为更为一般的非线性滤波器最初由Ruslan L.Stratonovich发明，而Stratonovich-Kalman-Bucy滤波器只是非线性滤波器的一个特例。事实上，1960年夏季，Kalman和Stratonovich在一个Moscow召开的会议中相遇，而作为非线性特例的线性滤波方程，早已经由Stratonovich在此以前发表了。

 在控制领域，Kalman滤波被称为线性二次型估计，目前,卡尔曼滤波已经有很多不同的实现，有施密特扩展滤波器、信息滤波器以及一系列的Bierman和Thornton 发明的平方根滤波器等，而卡尔曼最初提出的形式现在称为简单卡尔曼滤波器。也许最常见的卡尔曼滤波器应用是锁相环，它在收音机、计算机和几乎全部视频或通讯设备中广泛存在。

 ■   3    基本动态系统模型

Kalman滤波基于时域描述的线性动态系统，它的模型是Markov Chain，而Markov Chain建立在一个被高斯噪声干扰的线性算子之上。系统的状态可以用一个元素为实数的向量表示。 随着离散时间的增加，这个线性算子就会作用到当前状态之上，产生一个新的状态，并且会带入一定的噪声，同时一些已知的控制信息也会加入。同时另外一个受噪声干扰的线性算子将产生这些隐含状态的可见输出。Kalman 滤波可以被看作为类似隐马尔科夫模型，它们的显著不同点在于：隐状态变量的取值空间是一个连续的空间，而离散状态空间则不是；另为，隐马尔科夫模型可以描述下一个状态的一个任意分布，这也与应用于Kalman滤波器中的高斯噪声模型相反。Kalman滤波器方程和隐马尔科夫方程之间有很大的二重性，关于Kalman 滤波方程和隐马尔科夫方程之间二重性参看Roweis and Ghahramani(1999)[4]。              

 为了从一系列的噪声观测中，应用Kalman滤波估计观测过程的内部状态。我们必须把这个过程在Kalman 滤波器的框架下建立模型， 这就意味着，对于

每一步k  我们要定义矩阵 、 、 、 、 如下：

Kalman Filter 假设k 时刻的真实状态是从k-1时刻演化而来，符合下式

这里

■ 是作用在前一状态的状态转移模型(状态转移矩阵)

■  是作用在控制向量 上的控制输入模型(输入输出矩阵)

■ 是过程噪声，假设是均值为0的白噪声，协方差为 则：

在k时刻，假设真实状态 的观测， 满足如下公式：

                                                   

其中 是观测模型(观测矩阵)，它把真实状态映射到观测空间， 是观测噪声，假设它是均值是0，方差是 的高斯白噪声：

Kalman Filter基本动态系统模型如图(1)所示，圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差在右下方标出。

初始状态以及每一时刻的噪声向量{x0, w1, ..., wk, v1 ... vk} 都为认为是互相独立的。实际中，真实世界中动态系统并不是严格的符合此模型。但是Kalman模型是设计在噪声过程工作的，一个近似的符合已经可以使这个滤波器非常有用了，更多复杂模型关于Kalman Filter模型的变种，将在下述中讨论：

                                                                     图(1)

■    4    卡尔曼滤波器

Kalman Filter 是一个递归的估计，即只要获知上一时刻的状态估计和当前状态的观测就可以计算出当前状态的估计，不同于其他的估计技术，Kalman 滤波器不需要观测或/和估计的历史记录，Kalman Filter 是一个纯粹的时域滤波器，而不像低通滤波器等频域滤波器那样，需要在频域中设计，然后转换到时域中应用。

下面，代表已知从m到n-1包括m时刻的观测在n时刻的估计值

卡尔曼滤波器的状态由以下两个变量表示：

      ■ 已知k时刻以前时刻观测值，k时刻的状态估计值

      ■ 误差协方差矩阵，度量状态估计的精度程度

Kalman 滤波包括两个阶段：预测和更新；在估计阶段，滤波器应用上一状态的估计做出对当前状态的估计。在更新阶段，滤波器利用在当前状态的观测值优化预测阶段的预测值，以获的一个更精确的当前状态的估计。

4.1    预测

状态预测：$$

■

估计协方差预测：

■

4.2    更新

新息或测量余量

■

新息协方差

■

Kalman 增益

■

状态估计更新

■

状态协方差更新

■

使用上述公式计算 仅在最优卡尔曼增益的时候有效。使用其他增益公式要复杂一些，看见推导

4.3    不变量

如果模型准确， 和 值将准确反映最初状态的分布，那么下面所有不变量保持不变，所有估计的误差均值为0：

■

■

这里 表示 的期望，而协方差矩阵则反映的估计的协方差

■

■

■

■ 5  实例

考虑在一个无摩擦、无限长的直轨道上的一辆小车，它的初始位置在0点，但是它会随机的受到冲击作用，我们每隔测量一次小车的位置，但是这些测量数据不是很精确。我们想建立一个关于小车位置和速度的模型，这里我们描述如何建立这个模型，以及从这个模型出发如何推导出Kalman 滤波器。

因为小车没有控制输入，我们可以忽略和。由于F，H，R和Q全是恒值，我们可以忽略时间下标。

小车的位置和速度用线性空间可以描述如下：

这里表示速度，也就是位置对时间的微分。

我们假设在时间间隔k-1和k之间，小车受到一个恒定的冲击 ， 服从均值为0，方差为的正态分布，根据Newton 动力学方程，可得到：

 

 其中  

 ，

我们发现：

在每一时刻,我们获取真实位置的.我们假设噪声服噪声干扰测量，假设测量噪声服从均值为0，标准差为正态分布。

  其中 H= [1  0]，

我们可以得到足够精度的初始状态数据，所以我们可以初始化 ，  

如果初始位置和速度不是精确的知道，那么协方差矩阵应该初始化为一个对角线元素B为适当大小的矩阵如下：

这样与模型中已有信息相比，滤波器更趋向于使用首次的测量数据信息。

■  6   推导

 6.1  后验估计协方差矩阵推导

首先开始不变量后验估计协方差矩阵的推导：  带入 定义，可得 ，代入 可得 代入 可得 整理误差向量可得， 由于误差向量 与其他不相关，所以 由协方差矩阵性质则 ：使用不变量Pk|k-1以及Rk的定义这一项可以写作 ，此公式(Joseph form)对任意增益Kk的都成立，如果Kk最优卡尔曼增益，则可以进一步简化，见下文。

6.2  Kalman 增益推导

Kalman 滤波器是一个最小均方误差估计器，先验状态误差估计可表示为 我们最小化这个矢量幅度平方的期望值 ，这等价于最小化后验估计协方差矩阵 的迹，通过展开合并 公式，可得

当矩阵导数为0时，矩阵的迹取最小值，

从这个式子解出Kalman增益

这个增益就是最优Kalman增益，应用它可以得到最小均方误差。

6.3    后验误差协方差矩阵简化

当应用上述最优Kalman增益时，后验误差协方差可以得到简化，在最优Kalman增益两边同时乘以 ，可得 ，参见后验误差协方差公式展开 ，带入上式，可得： 这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在最优卡尔曼增益时它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用上面导出的后验误差协方差公式。

 ■    7    信息滤波

在信息滤波器(逆方差滤波器)中，协方差估计和状态估计将会被信息矩阵和信息向量所取代，它们的定义如下：

类似的预测协方差和预测状态也有等价的信息形式，定义如下：

同样测量协方差和测量向量定义为：

信息更新现在变成一个加和形式：

信息滤波器的主要优点在于N和测量数据都可以用于滤波，简单的通过信息矩阵和信息向量的加和。

为了预测信息滤波器，信息矩阵和信息向量必须变换到它们的等价状态空间，或者应用下述信息空间更新：

 

这里F和Ｑ必须可逆。

■ 8  非线性滤波器

    8.1 扩展Kalman 滤波

　　估计过程

如以上所述，卡尔曼滤波器估计一个线性随机差分方程描述的离散时间过程的状态变量，但是如果被估计的过程和(或)观测变量与过程的关系不时线性关系。那该如何处理呢？一些很有趣和成功的Kalman滤波器应用就是处理这些情况的。将期望和方差线性化的卡尔曼滤波器称作扩展卡尔曼滤波器（Extended Kalman Filter），简称EKF。

同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方程的偏导来线性化并计算当前估计，为了实现这个目的，我们必须修改上面的一些描述，我们假设过程仍具有状态向量，但其状态方程已变为非线性随机差分方程的形式。

                                                      

观测变量 为：

                                             

这里随机变量 和 分别为过程噪声和观测噪声。差分方程式(1.1)中的非线性函数f 将过去k-1时刻状态与现在k时刻状态联系起来。在测量方程(2.2)中，输入函数uk和零均值过程噪声wk是它的参数。非线性函数h 反映了状态变量xk 和观测变量zk 的关系。

实际中我们并不知道每一时刻噪声wk 和vk 各自真实值，但是我们可以在假设他们不存在的前提下，近似估计状态向量和测量向量：

                                    

                                         

这里 是相对于前一时刻k的后验状态估计。

　　有一点非常重要，那就是扩展卡尔曼滤波器的一个基本缺陷：离散随机变量的分布(或连续随机变量的密度)在经过非线性系统转化后不再是正态的了。扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝叶斯决策的特殊状态估计器。[Julier96]中描述了一项有趣的研究，Julier设计了扩展卡尔曼滤波器的一种变体，使得通过非线性转换后的随机变量仍具有正态分布特性。

滤波器的计算原型

   为了估计一个具有非线性差分和量测关系的过程，我们先给出式1.3和式1.4的一个新的线性化表示：

                        

                          

 其中：

　　xk 和zk是状态向量和观测向量的真值；

　　 和 来自1.3式和1.4式，是状态向量和观测向量的近似值；

　  是k 时刻状态向量的后验估计；

　   随机变量wk 和vk 表示过程激励噪声和观测噪声。

       A是f 对x的偏导的雅可比矩阵：

　　　　

　   W 是f 对w 的偏导的雅可比矩阵：

　　  　　

       H 是h 对x 的偏导的雅可比矩阵：

　　   　　

      V 是h 对v 的偏导的雅可比矩阵：

　　   　

摘自：http://internetbuff.blog.163.com/blog/static/94251107200917113038144/