转载从,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