惯性传感器的卡尔曼滤波
一、引言
下面我們引用文獻【1】中的一段話作為本文的開始：
想象你在黃昏時分看著一只小鳥飛行穿過濃密的叢林，你只能隱隱約約、斷斷續(xù)續(xù)地瞥見小鳥運動的閃現(xiàn)。你試圖努力地猜測小鳥在哪里以及下一時刻它會出現(xiàn)在哪里，才不至于失去它的行蹤。或者再想象你是二戰(zhàn)中的一名雷達操作員，正在跟蹤一個微弱的游移目標，這個目標每隔10秒鐘在屏幕上閃爍一次。或者回到更遠的從前，想象你是開普勒，正試圖根據(jù)一組通過不規(guī)則和不準確的測量間隔得到的非常不精確的角度觀測值來重新構(gòu)造行星的運動軌跡。在所有這些情況下，你都試圖根據(jù)隨對問變化并且?guī)в性肼暤挠^察數(shù)據(jù)去估計物理系統(tǒng)的狀態(tài)（例如位置、速度等等）。這個問題可以被形式化表示為時序概率模型上的推理，模型中的轉(zhuǎn)移模型描述了運動的物理本質(zhì)，而傳感器模型則描述了測量過程。為解決這類問題，人們發(fā)展出來了一種特殊的表示方法和推理算法——卡爾曼濾波。
二、基本概念
回想一下HMM的基本模型（如下圖所示），其中涂有陰影的圓圈（yt-2,?yt-1,?yt）相當于是觀測變量，空白圓圈（xt-2,xt-1,?xt）相當于是隱變量。這其實揭示了卡爾曼濾波與HMM之間擁有很深的淵源。回到剛剛提及的那幾個例子，你所觀測到的物體狀態(tài)（例如雷達中目標的位置或者速度）相當于是對其真實狀態(tài)的一種估計（因為觀測的過程中必然存在噪聲），用數(shù)學語言來表述就是P(yt?|?xt)，這就是模型中的測量模型或測量概率（Measurement Probability）。另外一方面，物體當前的（真實）狀態(tài)應(yīng)該與其上一個觀測狀態(tài)相關(guān)，即存在這樣的一個分布P(xt?|?xt-1)，這就是模型中的轉(zhuǎn)移模型或轉(zhuǎn)移概率（Transition Probability）。當然，HMM中隱變量必須都是離散的，觀測變量并無特殊要求。
而從信號處理的角度來講，濾波是從混合在一起的諸多信號中提取出所需信號的過程[2]。例如，我們有一組含有噪聲的行星運行軌跡，我們希望濾除其中的噪聲，估計行星的真實運動軌跡，這一過程就是濾波。如果從機器學習和數(shù)據(jù)挖掘的角度來說，濾波是一個理性智能體為了把握當前狀態(tài)以便進行理性決策所采取的行動[1]。比如，前兩天我們沒出門，但是我們可以從房間里觀察路上的行人有沒有打傘（觀測狀態(tài)）來估計前兩天有沒有下雨（真實狀態(tài)）。基于這些情況，現(xiàn)在我們要來決策今天（是否會有雨以及）外出是否需要打傘，這個過程就是濾波。讀者應(yīng)該注意把握上面兩個定義的統(tǒng)一性。
所謂估計就是根據(jù)測量得出的與狀態(tài)X(t) 有關(guān)的數(shù)據(jù)Y(t) =?h[X(t)] + V(t) 解算出X(t)的計算值，其中隨機向量V(t) 為測量誤差，稱為X的估計，Y 稱為 X 的測量。因為是根據(jù)Y(t) 確定的．所以?是Y(t) 的函數(shù)。若?是Y 的線性函數(shù)，則??稱作 X 的線性估計。設(shè)在 [t0,?t1] 時間段內(nèi)的測量為Y，相應(yīng)的估計為，則
- 當t?=?t1?時，??稱為X(t)的估計；
- 當t?>?t1?肘，稱為X(t)的預(yù)測；
- 當t?<?t1?時，?稱為X(t)的平滑。
最優(yōu)估計是指某一指標函數(shù)達到最值時的估計。卡爾曼濾波就是一種線性最優(yōu)濾波器。
因為后面會用到，這里我們補充一下關(guān)于協(xié)方差矩陣的概念。
設(shè)?n?維隨機變量（X1,?X2,?…,Xn）的2階混合中心距
σij?= cov(Xi,?Xj) = E[(Xi-E(Xi))(Xj-E(Xj))],? (i,j = 1, 2,?…,?n)
都存在，則稱矩陣
為?n?維隨機變量（X1,?X2,?…,Xn）的協(xié)方差矩陣，協(xié)方差矩陣是一個對稱矩陣，而且對角線是各個維度的方差。
維基百科中還給出了協(xié)方差矩陣的一些重要性質(zhì)，例如下面這兩條（此處不做具體證明）。后續(xù)的內(nèi)容會用到其中的第一條。
三、卡爾曼濾波的方程推導(dǎo)
直接從數(shù)學公式和概念入手來考慮卡爾曼濾波無疑是一件非常枯燥的事情。為了便于理解，我們?nèi)匀粡囊粋€現(xiàn)實中的實例開始下面的介紹，這一過程中你所需的預(yù)備知識僅僅是高中程度的物理學內(nèi)容。
假如現(xiàn)在有一輛在路上做直線運動的小車（如下所示），該小車在?t?時刻的狀態(tài)可以用一個向量來表示，其中pt?表示他當前的位置，vt?表示該車當前的速度。當然，司機還可以踩油門或者剎車來給車一個加速度ut，ut?相當于是一個對車的控制量。顯然，如果司機既沒有踩油門也沒有踩剎車，那么ut?就等于0。此時車就會做勻速直線運動。
如果我們已知上一時刻?t-1時小車的狀態(tài)，現(xiàn)在來考慮當前時刻t?小車的狀態(tài)。顯然有
易知，上述兩個公式中，輸出變量都是輸入變量的線性組合，這也就是稱卡爾曼濾波器為線性濾波器的原因所在。既然上述公式表征了一種線性關(guān)系，那么我們就可以用一個矩陣來表示它，則有
如果另其中的
則得到卡爾曼濾波方程組中的第一條公式——狀態(tài)預(yù)測公式，而F就是狀態(tài)轉(zhuǎn)移矩陣，它表示我們?nèi)绾螐纳弦粻顟B(tài)來推測當前狀態(tài)。而B則是控制矩陣，它表示控制量?u?如何作用于當前狀態(tài)。
???（1）
上式中x頂上的hat表示為估計值（而非真實值）。等式左端部分的右上標“-”表示該狀態(tài)是根據(jù)上一狀態(tài)推測而來的，稍后我們還將對其進行修正以得到最優(yōu)估計，彼時才可以將“-”去掉。
既然我們是在對真實值進行估計，那么就理應(yīng)考慮到噪聲的影響。實踐中，我們通常都是假設(shè)噪聲服從一個0均值的高斯分布，即Noise~Guassian(0,?σ)。例如對于一個一維的數(shù)據(jù)進行估計時，若要引入噪聲的影響，其實只要考慮其中的方差σ即可。當我們將維度提高之后，為了綜合考慮各個維度偏離其均值的程度，就需要引入?yún)f(xié)方差矩陣。
回到我們的例子，系統(tǒng)中每一個時刻的不確定性都是通過協(xié)方差矩陣?Σ?來給出的。而且這種不確定性在每個時刻間還會進行傳遞。也就是說不僅當前物體的狀態(tài)（例如位置或者速度）是會（在每個時刻間）進行傳遞的，而且物體狀態(tài)的不確定性也是會（在每個時刻間）進行傳遞的。這種不確定性的傳遞就可以用狀態(tài)轉(zhuǎn)移矩陣來表示，即（注意，這里用到了前面給出的關(guān)于協(xié)方差矩陣的性質(zhì)）
但是我們還應(yīng)該考慮到，預(yù)測模型本身也并不絕對準確的，所以我們要引入一個協(xié)方差矩陣?Q?來表示預(yù)測模型本身的噪聲（也即是噪聲在傳遞過程中的不確定性），則有
? （2）
這就是卡爾曼濾波方程組中的第二條公式，它表示不確定性在各個時刻間的傳遞關(guān)系。
繼續(xù)我們的小汽車例子。你應(yīng)該注意到，前面我們所討論的內(nèi)容都是圍繞小汽車的真實狀態(tài)展開的。而真實狀態(tài)我們其實是無法得知的，我們只能通過觀測值來對真實值進行估計。所以現(xiàn)在我們在路上布設(shè)了一個裝置來測定小汽車的位置，觀測到的值記為Y(t)。而且從小汽車的真實狀態(tài)到其觀測狀態(tài)還有一個變換關(guān)系，這個變換關(guān)系我們記為h(?)，而且這個h(?)還是一個線性函數(shù)。此時便有（該式前面曾經(jīng)給出過）
Y(t) =?h[X(t)] + V(t)
其中V(t)表示觀測的誤差。既然h(?)還是一個線性函數(shù)，所以我們同樣可以把上式改寫成矩陣的形式，則有
Yt=?Hxt?+ v
就本例而言，觀測矩陣?H?= [1 0]，這其實告訴我們x和Z的維度不一定非得相同。在我們的例子中，x是一個二維的列向量，而Z只是一個標量。此時當把x與上面給出的H相乘就會得出一個標量，此時得到的?Y?就是x中的首個元素，也就是小車的位置。同樣，我們還需要用一個協(xié)方差矩陣R來取代上述式子中的v來表示觀測中的不確定性。當然，由于Z是一個一維的值，所以此時協(xié)方差矩陣R也只有一維，也就是只有一個值，即觀測噪聲之高斯分布的參數(shù)σ。如果我們有很多裝置來測量小汽車的不同狀態(tài)，那么Z就會是一個包含所有觀測值的向量。
接下來要做的事情就是對前面得出的狀態(tài)估計進行修正，具體而言就是利用下面這個式子
??? （4） 直觀上來說，上式并不難理解。前面我們提到，是根據(jù)上一狀態(tài)推測而來的，那么它與“最優(yōu)”估計值之間的差距現(xiàn)在就是等式右端加號右側(cè)的部分。表示實際觀察值與預(yù)估的觀測值之間的殘差。這個殘差再乘以一個系數(shù)K就可以用來對估計值進行修正。K稱為卡爾曼系數(shù)，它也是一個矩陣，它是對殘差的加權(quán)矩陣，有的資料上稱其為濾波增益陣。
?? （3）
上式的推導(dǎo)比較復(fù)雜，有興趣深入研究的讀者可以參閱文獻【2】（P35~P37）。如果有時間我會在后面再做詳細推導(dǎo)。但是現(xiàn)在我們?nèi)匀豢梢远ㄐ缘貙@個系數(shù)進行解讀：濾波增益陣首先權(quán)衡預(yù)測狀態(tài)協(xié)方差矩陣?Σ?和觀測值矩陣R的大小，并以此來覺得我們是更傾向于相信預(yù)測模型還是詳細觀測模型。如果相信預(yù)測模型多一點，那么這個殘差的權(quán)重就會小一點。反之亦然，如果相信觀察模型多一點，這個殘差的權(quán)重就會大一點。不僅如此，濾波增益陣還負責把殘差的表現(xiàn)形式從觀測域轉(zhuǎn)換到了狀態(tài)域。例如本題中觀測值?Z?僅僅是一個一維的向量，狀態(tài)?x?是一個二維的向量。所以在實際應(yīng)用中，觀測值與狀態(tài)值所采用的描述特征或者單位都有可能不同，顯然直接用觀測值的殘差去更新狀態(tài)值是不合理的。而利用卡爾曼系數(shù)，我們就可以完成這種轉(zhuǎn)換。例如，在小車運動這個例子中，我們只觀察到了汽車的位置，但K里面已經(jīng)包含了協(xié)方差矩陣P的信息（P里面就給出了速度和位置的相關(guān)性），所以它利用速度和位置這兩個維度的相關(guān)性，從位置的殘差中推算出了速度的殘差。從而讓我們可以對狀態(tài)值?x?的兩個維度同時進行修正。
最后，還需對最優(yōu)估計值的噪聲分布進行更新。所使用的公式為
??（5）
至此，我們便獲得了實現(xiàn)卡爾曼濾波所需的全部五個公式，我在前面分別用（1）~（5）的標記進行了編號。我現(xiàn)在把它們再次羅列出來：
我們將這五個公式分成預(yù)測組和更新組。預(yù)測組總是根據(jù)前一個狀態(tài)來估計當前狀態(tài)。更新組則根據(jù)觀測信息來對預(yù)測信息進行修正，以期達到最優(yōu)估計之目的。
四、一個簡單的實例
當然，你可能困惑于卡爾曼濾波是否真的有效。下面利用文獻[4]中給出的例子（為提升顯示效果，筆者略有修改）來演示卡爾曼濾波的威力。這個例子模擬質(zhì)點進行勻速直線運動（速度為1），然后引入一個非常大的噪聲，再用卡爾曼濾波來對質(zhì)點的運動狀態(tài)進行軌跡。注意是勻速直線運動，所以其中不含有控制變量。
[plain]?view plaincopy
下圖給出了上述代碼的執(zhí)行結(jié)果。可見經(jīng)過最開始的幾次迭代后，質(zhì)點運動的狀態(tài)估計就回到了正確軌跡上，而且估計的結(jié)果基本圍繞在真實值附近，效果還是很理想的。
五、后記
本文相當于是卡爾曼濾波的入門文，在下一篇中我將深入挖掘一些本文未曾提及的細節(jié)（以及一些本文沒有給出的數(shù)學上的推導(dǎo)）。如果你想將自己對卡爾曼濾波的認識提升到一個更高的檔次，推薦你關(guān)注我的后續(xù)博文。Cheers~?
參考文獻：
【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.
【2】秦永元，張洪鉞，汪叔華，卡爾曼濾波與組合導(dǎo)航原理，西北工業(yè)大學出版社
【3】徐亦達博士關(guān)于卡爾曼濾波的公開課，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html
【4】卡爾曼濾波的原理以及在MATLAB中的實現(xiàn)，http://blog.csdn.net/revolver/article/details/37830675
我們在上一篇文章中通過一個簡單的例子算是入門卡爾曼濾波了，本文將以此為基礎(chǔ)討論一些技術(shù)細節(jié)。
卡爾曼濾波（Kalman Filter）
http://blog.csdn.net/baimafujinji/article/details/50646814
在上一篇文章中，我們已經(jīng)對HMM和卡爾曼濾波的關(guān)聯(lián)性進行了初步的討論。參考文獻【3】中將二者之間的關(guān)系歸結(jié)為下表。
上表是什么意思呢？我們其實可以下面的式子來表示，其中，w 和 v 分別表示狀態(tài)轉(zhuǎn)移 和 測量 過程中的不確定性，也即是噪聲，既然是噪聲就可以假設(shè)它們服從一個零均值的高斯分布。這其實跟我們在上一篇文章中所給出的形式是一致的，也就是說我們認為過去的狀態(tài)如果是?xt-1，那么當前狀態(tài)xt應(yīng)該是?xt-1的一個線性變換，而這個估計過程其實是有誤差的，用一個零均值的高斯噪聲（概率分布）來表達。類似地，當前的測量值yt應(yīng)該是真實值?xt?的一個線性變換，而這個測量過程仍然是有誤差的，也用一個零均值的高斯噪聲（概率分布）來表達。
????? (1)
上一節(jié)中我們還講過，在 [t0,?t1] 時間段內(nèi)的測量為Y，相應(yīng)的估計為，則當t?=?t1?時，??稱為X(t)的估計（或者稱為濾波）。當然現(xiàn)在我們也僅僅需要將注意力放在濾波上，所以最終要求的應(yīng)該是下面這個式子
根據(jù)條件概率的鏈式法則以及馬爾科夫鏈的無記憶性，再去掉常值系數(shù)的情況下，就可以得到下面的結(jié)論（如果你對有關(guān)數(shù)學公式記得不是很清楚可以參考http://blog.csdn.net/baimafujinji/article/details/50441927）
其中，P(?xt?|?y1, … ,?yt-1)就是Prediction（預(yù)測），因為它表示的意義是已知從1到t-1時刻的觀測值y1, … ,?yt-1的情況下求t?時刻的狀態(tài)值xt。另一方面，P(?xt?|?y1, … ,?yt)就是Update，因為它表示當我們已經(jīng)獲得yt時，再對xt?進行的一個更新（或修正）。
根據(jù)馬爾科夫鏈的無記憶性，可知P(?yt?|?xt,?y1, … ,?yt-1) =?P(?yt?|?xt) 。就預(yù)測部分而言，我們希望引入xt-1，所以可以采用下面的方法（這其實就是我們在處理普通貝葉斯網(wǎng)絡(luò)時所用過的方法）
到此為止，其實你應(yīng)該可以看出來卡爾曼濾波就形成了一個遞歸求解的過程。也就是說，我們欲求P(?xt?|?y1, … ,?yt-1)，就需要先求P(?xt-1?|?y1, … ,?yt-1)，而欲求P(?xt-1?|?y1, … ,?yt-1)，就要先求P(?xt-2?|?y1, … ,?yt-1)?……結(jié)合上一篇文章介紹的內(nèi)容，其實可以總結(jié)卡爾曼濾波的過程如下
也就是說當t = 1時，我們根據(jù)觀測值y1去估計真實狀態(tài)x1，這個過程服從一個高斯分布。然后，當t = 2時，我們根據(jù)上一個觀測值y1去預(yù)測當前的真實狀態(tài)x2，在獲得該時刻的真實觀測值y2后，我們又可以估計出一個新的真實狀態(tài)x2，這時就要據(jù)此對由y1預(yù)測的結(jié)果進行修正（Update），如此往復(fù)。
接下來，我們引入一個服從零均值高斯分布的（噪聲）變量?Δxt-1，
然后試著將Δxt和Δyt以Δxt-1的形式來給出，而且處于方便的考慮，我們忽略掉公式（1）中的控制項?B?和?C，于是有
根據(jù)獨立性假設(shè)，還可知如下結(jié)論（這些都是后續(xù)計算推導(dǎo)過程中所需要的準備）：
下面我們要做的事情就是推導(dǎo)卡爾曼濾波的五個公式，在上一篇文章中，我們更多地是從感性的角度給出了這些公式。并沒有給出詳細的數(shù)學推導(dǎo)，接下來我們就要來完成這項任務(wù)。
綜上我們已經(jīng)完整地給出了卡爾曼濾波的理論推導(dǎo)。對于結(jié)論性的東西，你當然可以直接拿來使用。在一些軟件包中，卡爾曼濾波無非是一條命令或者一個函數(shù)就能搞定。我們之所以還在這里給出它的詳細推導(dǎo)，主要是鑒于這種思想其實在機器學習中也被廣泛地用到，所以了解這些技術(shù)細節(jié)仍然十分有意義。
===================================================================================================
如果你是圖像處理的同道中人，歡迎加入圖像處理算法交流群（單擊鏈接查看群號）。
參考文獻：
【1】Stuart Russell and Peter Norvig. Artificial Intelligence: A Modern Approach. 3rd Edition.
【2】秦永元，張洪鉞，汪叔華，卡爾曼濾波與組合導(dǎo)航原理，西北工業(yè)大學出版社
【3】徐亦達博士關(guān)于卡爾曼濾波的公開課，http://v.youku.com/v_show/id_XMTM2ODU1MzMzMg.html
【4】卡爾曼濾波的原理以及在MATLAB中的實現(xiàn)，http://blog.csdn.net/revolver/article/details/37830675
總結(jié)
以上是生活随笔為你收集整理的惯性传感器的卡尔曼滤波的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 非常值得一看—九种滤波算法C语言实现
- 下一篇： 卡尔曼滤波器推导与解析 - 案例与图片