卡尔曼滤波

 

转载自http://chunqiu.blog.ustc.edu.cn/?p=223

之前选修自适应控制的时候看到过卡尔曼滤波器，没大看懂，感觉是一个很神奇的东西！这两天有兴趣把它搞懂，查了一些资料。

    有一个网站是专门讲kalman滤波器的， http://www.cs.unc.edu/~welch/kalman/，在Non-English Material中提到了有个约翰霍普金斯大学的华人博士翻译了 《An Introduction to the Kalman Filter》，通俗的介绍了kalman滤波器。另外还有matlab代码，点击后面的translated it into Chinese链接可以下载pdf文件和matlab代码，而在 http://www.scipy.org/Cookbook/KalmanFiltering这里有Scipy实现的相应的kalman代码。

    汉化的 《An Introduction to the Kalman Filter》讲蛮好的，有一些时间序列、矩阵概念的人很容易看懂，如果有控制理论的底子，那就更容易理解了。结合我的理解，做如下总结整理吧，公式我都试着推了，不难推，想深入理解的可以尝试推一推。

问题描述

    卡尔曼滤波器用于 估计离散时间过程的状态变量  x∈Rn  ，这个离散时间过程由以下 离散随机差分方程描述：

xk=Axk−1+Buk−1+wk−1

    定义观测变量  z∈Rm  ，得到 量测方程：

zk=Hxk+vk

wk  和  vk  分别表示过程激励噪声和观测噪声，假设它们是 相互独立，正态分布的白噪声 ，即满足

p(w)∼N(0,Q),

p(v)∼N(0,R).

其中  A  、  H  、  Q  、  R  都可能随时间变化，但在这里假设为常数。

    上面的差分过程可以如下图表示

    那么卡尔曼滤波器要解决什么问题呢？如上图， 蓝色标记的为已知的矩阵和信息， 卡尔曼滤波的目的就是利用这些已知的信息估计离散时间过程的状态变量  xk  。为什么是估计呢？因为有噪声  wk  、  vk  存在。而信息就这么多，我们要做的就是 利用这手头的信息尽量准确的估计出离散过程的状态变量，从而随时掌握系统的运行状态和变化。

    下图为带卡尔曼滤波器的系统方框图，图中的上半部分是实际的离散时间过程（有噪声的存在），下面部分是卡尔曼滤波器。 卡尔曼滤波器通过利用蓝色标记的可用信息  A 、  B  、  H  、  zk  、  uk  对系统的状态变量进行估计，得出粉色标记的  x^−k  、  x^k  等，而这个得以实现的关键就是红色标记出的反馈信息的利用，即设置怎样的反馈增益矩阵K使状态变量在某种意义下最准确，最接近真实系统状态。

计算模型推导

    问题描述清楚了，下面开始计算模型的推导。首先作如下 定义：

先验状态估计  x^−k∈Rn  为在已知第k步以前状态情况下第k步的 先验状态估计（  −  表示先验，^表示估计），即由差分方程从  x^k−1  和  uk−1   直接推算出来的估计。

后验状态估计  x^k∈Rn  为已知测量变量  zk  时第k步的后验状态估计，即 反馈回来做了修正后的估计。

    由上面的定义，继续定义

先验估计误差

e−k≡xk−x^−k

后验估计误差

ek≡xk−x^k

先验估计误差协方差矩阵

P−k=E[e−ke−Tk]

后验估计误差协方差矩阵

Pk=E[ekeTk]

    在上面定义的基础上， 构造卡尔曼滤波器的表达式：先验估计  x^−k  和加权的测量变量  zk  与其预测估计  Hx^−k  之差的线性组合构成了后验状态估计  x^k  ，即

x^k=x^−k+K(zk−Hx^−k)(1)

上式中测量变量与其预测之差  (zk−Hx^−k)  被称为 测量过程的革新或残余。残余反映了预测值和实际值之间的不一致程度，残余为零表明二者完全吻合。  n×m  阶矩阵   K  叫做残余的增益或混合因数， 作用是使后验估计误差协方差最小。

    具体推导如下：由式 (1) ，有

ek=xk−x^k=xk−x^−k−K(zk−Hx^−k)=e−k−K(zk−Hx^−k)=e−k−K(Hxk+vk−Hx^−k)=e−k−K(He−k+vk)

    所以，后验估计误差协方差矩阵为

Pk=E[ekeTk]=E{[e−k−K(He−k+vk)][e−k−K(He−k+vk)]T}=E[e−ke−Tk]−E[e−k(e−TkHT+vTk)KT]−E[K(He−k+vk)e−Tk]+E[(K(He−k+vk)(e−TkHT+vTk)KT]=P−k−P−kHTKT−KHP−k+K(HP−kHT+R)KT(2)

注意由于 预测状态与噪声无关，故  E[e−kvTk]=E[vke−Tk]=0  ，故有如上化简。

      K  是通过令后验估计误差协方差为最小得到的，即求  Pk=E[ekeTk]  的迹为最小时的  K  ，故令

0=∂tr(Pk)∂K=−P−kHT−P−kHT+2K(HP−kHT+R)

0=−2P−kHT+2K(HP−kHT+R)

    所以

Kk=P−kHT(HP−kHT+R)−1=P−kHTHP−kHT+R(3)

    上面用到了 对矩阵求导的常用公式

∂(aTw)∂w=∂(wTa)∂w=a

∂(wTAw)∂w=Aw+ATw

    将上面求得的  Kk  带入  Pk=E[ekeTk]  ，得

Pk=(I−KkH)P−k(4)

    上面式子中， (3) 、 (1) 、 (4) 构成了卡尔曼滤波器的测量更新方程。 测量更新方程负责反馈，也就是说，它将先验估计和新测量变量结合以构成改进的后验估计。 所以测量更新方程可视为校正方程，再次列举如下

Kk=P−kHT(HP−kHT+R)−1=P−kHTHP−kHT+R(5)

x^k=x^−k+K(zk−Hx^−k)(6)

Pk=(I−KkH)P−k(7)

测量更新方程的更新过程如下：首先按计算卡尔曼增益  Kk  ，然后计算状态的后验估计  x^k  ，最后估计状态的后验协方差矩阵  Pk  。

    注意到式 (4) 中计算后验协方差矩阵  Pk  需要先验协方差矩阵  P−k=E[e−ke−Tk]  ，而先验协方差矩阵  P−k  可以通过前一步的后验协方差矩阵得到，推导如下

e−k=xk−x^−k=Axk−1+Buk−1+wk−1−Ax^k−1−Buk−1=A(xk−1−x^k−1)+wk−1=Aek−1+wk−1(8)

注意上式用到了 状态更新方程，这个方程在卡尔曼滤波器系统方框图中显而易见。

x^−k=Ax^k−1+Buk−1(9)

    所以

P−k=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=E[Aek−1eTk−1AT]+E[wk−1wTk−1]=APk−1AT+Q(10)

    式 (9) 和 (???) 构成了 卡尔曼滤波器的时间更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便在下一个时间状态构造先验估计。所以 时间更新方程可视为预估方程， 再次列举如下：

x^−k=Ax^k−1+Buk−1

P−k=APk−1AT+Q

    最后的估计算法成为一种具有数值解的 预估-校正算法，如下图

     卡尔曼滤波器的5个主要方程如下

    这种 递归推算是卡尔曼滤波器最吸引人的特性之一―― 它比其它滤波器更容易实现：例如维纳滤波器，每次估计必须直接计算全部数据，而 卡尔曼滤波器每次只根据以前的测量变量递归计算当前的状态估计。

    下面对残余增益  K  做简单分析：由式 (3) ，即

Kk=P−kHT(HP−kHT+R)−1=P−kHTHP−kHT+R

    当观测噪声协方差R越小时，残余增益  K  越大。特别地，  R  趋向于零时，有

limRk→0Kk=H−1

    另一方面，当先验估计误差协方差  P−k  越小时，残余增益  K  越小。特别地，  P−k  趋向于零时，有

limP−k→0Kk=0

    增益K可以这样解释， 当测量噪声协方差  R  越小时，即观测噪声越小，测量变量  zk  将比  Hx^−k  的权重大，即  zk  在反馈调节中起主要作用。另一方面， 当先验估计误差协方差  P−k  越小时，即状态估计的前后变化不大（方差越小不就是在均值周围波动越小），测量变量  zk  的权重变小，  Hx^−k  的权重变大，即状态估计的很可靠，那就主要看状态估计了。

滤波器系数及调整

    滤波器实际实现时， 测量噪声协方差  R  一般可以观测得到，是滤波器的已知条件。因此通常我们离线获取一些系统观测值以计算测量噪声协方差。 通常更难确定过程激励噪声协方差的  Q  值，因为我们无法直接观测到过程信号  xk  。有时可以通过  Q  的选择给过程信号“注入”足够的不确定性来建立一个简单的（差的）过程模型而产生可以接受的结果。当然在这种情况下人们希望信号观测值是可信的。

    在这两种情况下，不管我们是否有一个合理的标准来选择系数，我们通常（统计学上的） 都可以通过调整滤波器系数来获得更好的性能。调整通常离线进行，并经常与另一个（确定无误的）在线滤波器对比，这个过程称为 系统识别。

    另外， 通常在  Q  、  R  都是常数的条件下，过程估计误差协方差  Pk  和卡尔曼增益  Kk 都会很快收敛并保持为常量。所以实际应用中，常常将滤波器系数通过预先离线运行得到。Matlab的 kalman函数就是产生此稳态时的矩阵。

    实际中，观测误差  R  和过程激励噪声  Q  都可能随着滤波器运行而动态变化，这样  Q 变成了  Qk  ，来适应不同的动态状态。   Qk  的幅度要根据用户的意图和模型的不确定性来选择。

滤波器的概率原型解释

    在卡尔曼滤波器表达式中包含了状态分布的 前二阶矩：

E[xk]=x^k

E[(xk−x^k)(xk−x^k)T]=Pk

    后验状态估计式 (1) 反应了 状态分布的均值（一阶矩）――如果  w  和  v  是白噪声成立，均值的估计便是正态分布的。 后验估计误差协方差矩阵式反映了状态分布的方差（二阶非中心矩）。

    在已知  zk  的情况下，  xk  的分布可写为：

p(xk|zk)∼N(E[xk],E[(xk−x^k)(xk−x^k)T])=N(x^k,Pk)

    《 An Introduction to the Kalman Filter》中后面介绍了 扩展卡尔曼滤波器，用在处理被估计的过程和（或）观测变量与过程的关系是非线性的情况，思想和线性的差不多。

    后面的例子后面再做测试。