本文详细介绍了Kalman滤波的工作原理及其在状态估计中的应用。文章首先回顾了Bayes滤波的基本步骤，并在此基础上深入解释了Kalman滤波的数学模型、算法流程及其实现方法。此外，还通过实例展示了Kalman滤波在降低观测噪声方面的有效性。
转载从，http://www.cnblogs.com/ycwang16/p/5999034.html
前面介绍了Bayes滤波方法，我们接下来详细说说Kalman滤波器。虽然Kalman滤波器已经被广泛使用，也有很多的教程，但我们在Bayes滤波器的框架上，来深入理解Kalman滤波器的设计，对理解采用Gaussian模型来近似状态分布的多高斯滤波器(Guassian Multi-Hyperthesis-Filter)等都有帮助。
一. 背景知识回顾
1.1 Bayes滤波
首先回顾一下Bayes滤波. Bayes滤波分为两步：1.状态预测；和 2.状态更新
1. 状态预测，基于状态转移模型：
bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1bel¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1
2. 状态更新，基于新的观测
bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)bel(xt)=ηp(zt|xt)bel¯(xt)
我们可以看到，我们的目的是计算xtxt的后验概率，如果bel(xt)bel(xt)是任意分布，我们需要在xtxt的所有可能取值点上，计算该取值的概率，这在计算上是难于实现的。这一计算问题可以有多种方法来近似，比如利用采样的方法，就是后面要讲的粒子滤波和无迹Kalman滤波。
这节要说的近似方法是，当假设bel(xt)bel(xt)服从Gauss分布，那么我们只需要分布的均值和方差就可以完全描述bel(xt)bel(xt)，而无需在xtxt的每个可能取值点上进行概率计算。这也是用高斯分布来近似bel(xt)bel(xt)的好处，因为我们在每一个时刻，只需要计算均值μtμt和方差ΣtΣt这两个数值，就可以对bel(xt)bel(xt)完全描述，所以我们就可以推导出这两个数值的递推公式，从而在每个时刻由这两个数值的递推公式完全获得状态估计，这就是The Kalman Filter的基本思想。
1.2 正态分布(Guassian Distribution)
然后我们再回顾一下正态分布的基础知识。正态分布是一种特殊的概率分布，分布的形态完全由二阶矩决定。一元高斯分布表述如下：
其中，一阶矩为均值μμ表示期望值，二阶矩为方差σσ表示分布的不确定程度。
多元高斯分布的表达式为：
同样，一阶矩为μμ表示各元变量的期望值，二阶矩为方差矩阵ΣΣ表示各元变量的不确定程度。
1.3 正态分布的特点
在线性变换下，一旦高斯，代代高斯。
首先，高斯变量线性变换后，仍为高斯分布，均值和方差如下：
X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT)X∼N(μ,Σ)Y=AX+B}⇒Y∼N(Aμ+B,AΣAT)
然后，两个高斯变量线性组合，仍为高斯分布，均值和方差如下：
X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1+X2)∼N(μ1+μ2,σ21+σ22+2ρσ1σ2)X1∼N(μ1,σ12)X2∼N(μ2,σ22)}⇒p(X1+X2)∼N(μ1+μ2,σ12+σ22+2ρσ1σ2)
最后，两个相互独立的高斯变量的乘积，仍然为高斯分布，均值和方差如下：
X1∼N(μ1,σ21)X2∼N(μ2,σ22)}⇒p(X1)⋅p(X2)∼N(σ22σ21+σ22μ1+σ21σ21+σ22μ2,σ21σ22σ21+σ22)X1∼N(μ1,σ12)X2∼N(μ2,σ22)}⇒p(X1)⋅p(X2)∼N(σ22σ12+σ22μ1+σ12σ12+σ22μ2,σ12σ22σ12+σ22)
正因为高斯分布有这些特点，所以，在Bayes滤波公式中的随机变量的加法、乘法，可以用解析的公式计算均值和方差，这使得Bayes滤波的整个计算过程非常简便，即Kalman滤波器的迭代过程。
二. Kalman滤波
2.1 Kalman滤波的模型假设
Kalman滤波所解决的问题，是对一个动态变化的系统的状态跟踪的问题，基本的模型假设包括：1）系统的状态方程是线性的；2）观测方程是线性的；3）过程噪声符合零均值高斯分布；4）观测噪声符合零均值高斯分布；从而，一直在线性变化的空间中操作高斯分布，状态的概率密度符合高斯分布。
- 状态方程
xt=Atxt−1+Btut+εtxt=Atxt−1+Btut+εt - 观测方程
zt=Htxt+δtzt=Htxt+δt
其中过程噪声εtεt假设符合零均值高斯分布；观测噪声δtδt假设符合零均值高斯分布。对于上述模型，我们可以用如下参数描述整个问题：
2.2 Kalman滤波器的模型
- xtxt，nn维向量，表示tt时刻观测状态的均值。
- PtPt，n∗nn∗n方差矩阵，表示tt时刻被观测的nn个状态的方差。
- utut，ll维向量，表示tt时刻的输入
- ztzt，mm维向量，表示tt时刻的观测
- AtAt，n∗nn∗n矩阵，表示状态从t−1t−1到tt在没有输入影响时转移方式
- BtBt，n∗nn∗n矩阵，表示utut如何影响xtxt
- HtHt，m∗nm∗n矩阵，表示状态xtxt如何被转换为观测ztzt
- RtRt，n∗nn∗n矩阵，表示过程噪声εtεt的方差矩阵
- QtQt，m∗mm∗m矩阵，表示观测噪声δtδt的方差矩阵
图1. 在没有观测情况下，系统状态的从t−1t−1到tt的转移方式
图1给出了在没有观测，仅有输入utut时，状态变量的均值和方差从t−1t−1到tt的转移方式，可见均值和方差的计算，完全是基于高斯分布的线性变化的方法来算的。
图2. Kalman 滤波解决在收到t时刻的输入utut和观测ztzt的情况下，更新状态xtxt的问题
图2给出了Kalman滤波所解决的问题，即在获得t时刻的输入和观测的情况下，如何更新xtxt的均值和方差的问题。当然utut和ztzt也并不是每一个时刻都需要同时获得，就像贝叶斯滤波一样，可以在获得utut时就做一次状态预测，在获得ztzt时做一次状态更新。
2.3 Kalman滤波算法
Kalman滤波整体算法如下：
| Kalman Filter (xt−1,Pt−1,ut,ztxt−1,Pt−1,ut,zt)
|
- 第一行基于转移矩阵和控制输入，预测tt时刻的状态
- 第二行是预测方差矩阵
- 第三行计算Kalman增益，Kt
- 第四行基于观测的新息进行状态更新
- 第五行计算更新状态的方差矩阵。
可以看到算法的所有的精妙之处都在于第三行和第四行。我们可以这样来理解：
- (HtP¯¯¯¯tHTt+Qt)(HtP¯tHtT+Qt)代表对状态进行观测时，观测的不确定程度，它与Kalman增益Kt成反比，表示观测的可能噪声越大的时候，Kalman增益Kt越小。
- 再看第四行，xtxt的更新是在x¯¯¯tx¯t上加一个 KtKt 乘以 (zt−Htx¯¯¯t)(zt−Htx¯t)。(zt−Htx¯¯¯t)(zt−Htx¯t)代表的是预测的值与观测之间的差异，这个差异当预测和观测都比较接近于真实值时比较小。当观测不准，或者预测不准时都会比较大。而前面的乘子Kt是在观测噪声大的时候比较小，所以整个Kt(zt−Htx¯¯¯t)Kt(zt−Htx¯t)这个修正量，表示利用观测对预测结果的修正量。
- 当观测噪声比较小，预测误差比较大时修正幅度比较大
- 当观测噪声比较小预测误差比较小的时候，或者观测噪声比较大的时候，修正误差的幅度也比较小，从而起到了一种平滑的作用。
- 利用较准确的观测修正预测误差，不准确的观测修正量也较小，所以在误差较大的时候能快速修正，而在误差较小时能逐渐收敛。
2.4 Kalman滤波算法的推导
这里我们用Bayes公式，给出Kalman Filter是如何导出的。
1. 系统的初始状态是：
bel(x0)=N(μ0,P0)bel(x0)=N(μ0,P0)
2. 预测过程的推导
状态转移模型是线性函数
xt=Atxt−1+Btut+εtxt=Atxt−1+Btut+εt
所以，由xt−1xt−1到xtxt状态转移的条件概率为:
p(xt|ut,xt−1)=N(Atxt−1+Btut,Rt)p(xt|ut,xt−1)=N(Atxt−1+Btut,Rt)
回顾Bayes公式，计算预测状态的分布，需要考虑所有可能的xt−1xt−1：
bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1bel¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1
这正是计算两个高斯分布的卷积的过程，参考文献[2]：
bel¯¯¯¯¯¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1⇓⇓∼N(Atμt−1+Btut,Rt)∼N(μt−1,Pt−1)⇓bel¯¯¯¯¯¯(xt)=η∫exp{−12(xt−Atxt−1−Btut)TR−1t(xt−Atxt−1−Btut)}exp{−12(xt−1−μt−1)TP−1t−1(xt−1−μt−1)}dxt−1bel¯¯¯¯¯¯(xt)={μ¯t=Atμt−1+BtutP¯¯¯¯t=AtPt−1ATt+Rtbel¯(xt)=∫p(xt|ut,xt−1)bel(xt−1)dxt−1⇓⇓∼N(Atμt−1+Btut,Rt)∼N(μt−1,Pt−1)⇓bel¯(xt)=η∫exp{−12(xt−Atxt−1−Btut)TRt−1(xt−Atxt−1−Btut)}exp{−12(xt−1−μt−1)TPt−1−1(xt−1−μt−1)}dxt−1bel¯(xt)={μ¯t=Atμt−1+BtutP¯t=AtPt−1AtT+Rt
所以Kalman滤波器的预测过程，正是基于两个高斯分布的卷积计算得到的解析表达式。
3. 观测更新过程的推导
观测方程也是线性方程，并且噪声是高斯噪声
zt=Htxt+δtzt=Htxt+δt
所以p(zt|xt)p(zt|xt)的条件概率是高斯分布的线性变换计算：
p(zt|xt)=N(Htxt,Qt)p(zt|xt)=N(Htxt,Qt)
再考虑贝叶斯公式的状态更新步骤
bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)bel(xt)=ηp(zt|xt)bel¯(xt)
这正是两个高斯分布的乘积的问题，参考文献[2]
bel(xt)=ηp(zt|xt)bel¯¯¯¯¯¯(xt)⇓⇓∼N(zt;Htxt,Qt)∼N(xt;μ¯¯¯t,P¯¯¯¯t)⇓bel(xt)=ηexp{−12(zt−Htxt)TQ−1t(zt−Htxt)}exp{−12(xt−μ¯t)TP¯−1t(xt−μ¯t)}bel(xt)=ηp(zt|xt)bel¯(xt)⇓⇓∼N(zt;Htxt,Qt)∼N(xt;μ¯t,P¯t)⇓bel(xt)=ηexp{−12(zt−Htxt)TQt−1(zt−Htxt)}exp{−12(xt−μ¯t)TP¯t−1(xt−μ¯t)}
所以，基于求高斯变量乘积的分布的方法，可以导出结果仍然是高斯分布，用它的二阶矩表示：
bel(xt)={μt=μ¯t+Kt(zt−Htμ¯t)Pt=(I−KtHt)P¯¯¯¯twithKt=P¯¯¯¯tHTt(HtP¯¯¯¯tHTt+Qt)−1bel(xt)={μt=μ¯t+Kt(zt−Htμ¯t)Pt=(I−KtHt)P¯twithKt=P¯tHtT(HtP¯tHtT+Qt)−1
所以状态更新中的Kalman增益，均值和方差的更新公式，都是这样导出的。
2.5 Kalman滤波算法的举例
图3和图4通过一维高斯分布的例子，给出在预测和更新过程中状态变量的概率密度分布是如何变化的。
图3. 预测过程的举例，蓝色曲线表示xt−1xt−1的pdf，紫色曲线表示x¯¯¯tx¯t的pdf.
图4. 更新过程举例，紫色为预测后的pdf, 黄色为更新后的pdf，青色为观测的结果
从这个例子中可以值得注意的是，在预测部分高斯分布的卷积一般会使状态估计的方差加大；在观测部分高斯分布的乘积一般会将估计的方差收窄。
2.6 Kalman滤波的代码实现
Kalman滤波算法可以非常方便的用矩阵计算方法实现，其迭代更新过程的Matlab实现的代码仅有如下几行：
2.7 Kalman滤波的效果示例
通过实现一个简单的Kalman滤波器，我们可以直观的看一下Kalman滤波器的提高跟踪准确性的效果。
图5. Kalman滤波器的实验效果示例，其中红色实线是真值；蓝色点是观测；绿色线是滑动平均的结果；紫色曲线是Kalman滤波的结果。
图6. 比较Kalman滤波的跟踪结果和滑动平均的跟踪结果
图6给出了直观的跟踪结果与真实值之间的最小二乘误差的比较，课件Kalman滤波算法相比滑动平均等，提供了更高的跟踪准确性。
2.8 Kalman滤波的算法特点
- Kalman滤波计算快速，计算复杂度为O(m2.376+n2)O(m2.376+n2)，其中mm是观测的维数；nn是状态的个数。
- 对于线性系统，零均值高斯噪声的系统，Kalman是理论上无偏的，最优滤波器。
- Kalman滤波在实际使用中，要注意参数RR和QQ的调节，这两者实际上是相对的，表示更相信观测还是更相信预测。具体使用时，RR可以根据过程噪声的幅度决定，然后QQ可以相对RR来给定。当更相信观测时，把QQ调小，不相信观测时，把QQ调大。
- QQ越大，表示越不相信观测，这是系统状态越容易收敛，对观测的变化响应越慢。QQ越小，表示越相信观测，这时对观测的变化响应快，但是越不容易收敛。
参考文献
[1]. Sebastian Thrun, Wolfram Burgard, Dieter Fox, Probabilistic Robotics, 2002, The MIT Press.
[2]. P.A. Bromiley, Products and Convolutions of Gaussian Probability Density Functions, University of Manchester
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
