卡爾曼濾波器的簡(jiǎn)單推導(dǎo)與直觀理解-中文版-副本

1、卡爾曼濾波器的簡(jiǎn)單直觀推導(dǎo)與直觀理解摘要：本文給出了卡爾曼濾波器的一種簡(jiǎn)單又直觀的推導(dǎo)過(guò)程， 目的是讓那 些沒(méi)有很強(qiáng)數(shù)學(xué)背景的學(xué)生學(xué)會(huì)使用這個(gè)有用的工具。該文中最復(fù)雜的數(shù)學(xué)要求 是理解兩個(gè)高斯函數(shù)的乘積并對(duì)其進(jìn)行化簡(jiǎn)?？柭鼮V波器的提出至今已經(jīng)有五十多年（補(bǔ)充：卡爾曼濾波器的提出時(shí)間 為1960年），但它仍是目前最重要也是最常用的數(shù)據(jù)融合算法之一?？柭鼮V波器以魯?shù)婪蚩柭拿置?，其具有?jì)算量小、遞歸方程簡(jiǎn)潔等優(yōu)點(diǎn)，并且它 是一維高斯線性系統(tǒng)的最優(yōu)估計(jì)器?？柭鼮V波器的典型用途包括噪聲數(shù)據(jù)平滑 和參數(shù)估計(jì)。應(yīng)用包括全球定位系統(tǒng)的接收器、 無(wú)線電設(shè)備中的鎖相環(huán)、筆記本 電腦觸控板的輸出等等。

2、從理論上講，卡爾曼濾波器是一種在線性動(dòng)力學(xué)系統(tǒng)中允許精確推演的算法， 該算法是一種類似隱馬爾可夫的貝葉斯模型，不同的是隱變量的狀態(tài)空間是連續(xù) 的，并且所有隱變量和觀測(cè)量測(cè)均服從高斯分布（通常為多元高斯分布）。本講義的目的是通過(guò)簡(jiǎn)單和直觀的推導(dǎo)讓對(duì)這些概念感到困惑或懼怕的人理解卡爾 曼濾波器的基礎(chǔ)。相關(guān)工作卡爾曼濾波器1（以及其變形例如擴(kuò)展卡爾曼濾波器2和無(wú)跡卡爾曼濾波 器3）是信息處理領(lǐng)域最著名和受歡迎的數(shù)據(jù)融合算法之一。 早期，卡爾曼濾波 器最著名的應(yīng)用是在將尼爾阿姆斯特朗送至月球的阿波羅導(dǎo)航計(jì)算機(jī)中，并且（最重要的是）將他帶了回來(lái)。目前卡爾曼濾波器已經(jīng)應(yīng)用在衛(wèi)星導(dǎo)航設(shè)備、智 能手機(jī)和許多計(jì)

3、算機(jī)游戲中?？柭鼮V波器的典型推導(dǎo)方法是使用最小均方誤差估計(jì)器 4,該方法適合 數(shù)學(xué)功底較強(qiáng)的學(xué)生，但不適用數(shù)學(xué)功底較弱的學(xué)生。此處卡爾曼濾波器的推導(dǎo) 的關(guān)鍵是高斯分布的一條重要特性一一兩個(gè)高斯分布的乘積是另一個(gè)高斯分布。對(duì)讀者的要求本文并非為剛接觸卡爾曼濾波器的學(xué)生提供一個(gè)全面的教程， 而是面向?qū)?爾曼濾波器有所了解，但是數(shù)學(xué)功底不夠強(qiáng)的學(xué)生。讀者應(yīng)該熟悉與卡爾曼濾波 器相關(guān)的一些基本符號(hào)和術(shù)語(yǔ)，比如狀態(tài)矢量和協(xié)方差矩陣。本文面向那些需要也適用于那些已經(jīng)對(duì)卡爾曼本文并非作為面向新手的全以簡(jiǎn)單直觀的方式向其他人教授卡爾曼濾波器的人, 濾波器有一定的了解但是不完全理解其基礎(chǔ)的人們。面而獨(dú)立的教

4、育工具，因?yàn)檫@將需要一章而非幾頁(yè)內(nèi)容能夠講述清楚問(wèn)題描述卡爾曼濾波器的目標(biāo)狀態(tài)演變模型假設(shè)系統(tǒng)狀態(tài)從k 1時(shí)刻演變至k時(shí)刻時(shí)服從方程Xt FtXt 1 BtUt Wt（1）where其中?Xt為狀態(tài)矢量，包含系統(tǒng)在t時(shí)刻感興趣的參數(shù)（例如位置、速度和方位 角）。?Ut為控制矢量，包含控制項(xiàng)（例如轉(zhuǎn)向角、節(jié)氣門(mén)設(shè)置、制動(dòng)力）? Ft為狀態(tài)轉(zhuǎn)移矩陣，它是將t 1時(shí)刻系統(tǒng)的每個(gè)參數(shù)的影響作用于 t時(shí)刻 （例如，t 1時(shí)刻位置和速度都作用于t時(shí)刻），譯者注：狀態(tài)轉(zhuǎn)移矩陣的作 用為系統(tǒng)參數(shù)的預(yù)測(cè)。? Bt是控制輸入矩陣，其將矢量Ut中的每個(gè)控制輸入?yún)?shù)作用于狀態(tài)矢量 （例如，將油門(mén)設(shè)置的效果作用于系統(tǒng)的

5、速度和位置）? Wt為過(guò)程噪聲，并假設(shè)過(guò)程噪聲服從均值為零、協(xié)方差矩陣為Qt的多變量高斯分布系統(tǒng)的量測(cè)可以建模為Zt HtXt Vt其中? Zt為量測(cè)矢量? Ht為量測(cè)矩陣，將狀態(tài)矢量中的參數(shù)映射至量測(cè)維? vt為量測(cè)噪聲矢量，假設(shè)服從均值為零協(xié)方差為 Rt的高斯白噪聲。在下面的推導(dǎo)中，將考慮一個(gè)簡(jiǎn)單的一維跟蹤問(wèn)題，具體的為一列火車(chē)沿著 鐵路線行駛的一維跟蹤問(wèn)題（如圖 1所示）。因此，我們可以考慮該問(wèn)題中的一 些小例矢量和矩陣。狀態(tài)矢量xt包含火車(chē)的位置和速度信息，即XtXt &帶有場(chǎng)聲的最惻Prediebon (Estimate)Measurement Noisy)FIG1 This fig

6、ure the至辟 104n undN con$idertiart4Idi it國(guó)雇示f所考慮的加齦蹤系統(tǒng)火車(chē)駕駛員可以向系統(tǒng)施加剎車(chē)或者加速這樣的輸入，在此我們考慮的輸入為向 系統(tǒng)施加的力ft和火車(chē)的質(zhì)量m。這些控制信息以控制矢量ut的形式進(jìn)行存儲(chǔ),ut工m時(shí)間t期間，力ft與火車(chē)的位置和速度的關(guān)系如下:% xti &i tft2m& &ift tm這些線性方程可以用矩陣的形式寫(xiě)為Xt1 tXt i%01%i2t2 tftm2t2 t通過(guò)與式（i）比較，我看可以發(fā)現(xiàn)i tFtand Bt0 i系統(tǒng)的狀態(tài)Xt無(wú)法直接觀測(cè)，卡爾曼濾波器提供了一種算法，通過(guò)聯(lián)合系統(tǒng)模型 和帶有噪聲的量測(cè)函數(shù)能夠得

7、到系統(tǒng)狀態(tài)的估計(jì)值 又。因此，狀態(tài)矢量中感興趣 參數(shù)的估計(jì)值由概率密度函數(shù)而不是離散的值給出。為了充分的表示高斯分布， 我們需要知道它們的方程和協(xié)方差，并且存儲(chǔ)在協(xié)方差矩陣Pt中。Pt中主對(duì)角線 上的元素為狀態(tài)矢量中相關(guān)元素的方差。Pt的副對(duì)角線提供狀態(tài)矢量中元素之間 的協(xié)方差。在建模良好的一維線性系統(tǒng)中，量測(cè)誤差為零均值的高斯分布，卡爾 曼濾波器已經(jīng)被證明為最優(yōu)估計(jì)器5。在本文的剩余部分，我們將推導(dǎo)卡爾曼濾 波器的方程，該方程能夠聯(lián)合先驗(yàn)知識(shí)、系統(tǒng)模型預(yù)測(cè)和帶噪聲的量測(cè)來(lái)遞歸計(jì)算又?？柭鼮V波算法包括兩個(gè)步驟：預(yù)測(cè)和量測(cè)更新。標(biāo)準(zhǔn)卡爾曼濾波器的預(yù)測(cè) 步驟的方程為?t|t i Pt|t iFt

8、?ti|tiBtUtFPt i|t iFtTQtEXtXt|t iXt%1t i (3),下面對(duì)方程其中Qt為過(guò)程噪聲協(xié)方差矩陣。在上面的討論中推導(dǎo)了方程 進(jìn)行推導(dǎo)。未知的真實(shí)值Xt與預(yù)測(cè)值Xt-的方程可以表示為計(jì)算式(3)和(1)的差，可得|t ixtxt|t i = FtPt|t i EXt i * it iXtWtTXtXt|t iFt Xt iXt it iWtFtXt iXt itTiWtFtE歿 iXt it iTXt iXt it iFtTFtEXt i it iTwtE Wt Xt iT FtTTE WtWt因?yàn)闋顟B(tài)估計(jì)誤差和過(guò)程噪聲不相關(guān)，所以有FtEXt iXt it iT

9、WtE wtTXt i X it iFtTPt|t iFtE Xtit itiXt iTXt i|t iFtTTWtWtPt|t iFtPt it iFtTQt測(cè)量更新方程為?t |tXt|t iKt ZtH tXt|t iPt|tPt|tiKtHtPt|ti(5)(6)where其中de兩個(gè)高斯分KtPt|t iHT HtPt|t iHT Rt在論文的剩余部分，我們將利用第一原理(相關(guān)工作部分中提到 布的成績(jī)還是一個(gè)高斯分布)推導(dǎo)量測(cè)更新方程 (5)(7)式。解決方法|FIG2 The inFilial knowledge st the system at time t = Oi The r

10、ed Gaussian di&lribLitiDn represents the pdf providing thE lirirtbal conf ide ncB in the KlimatE of the position of the tram. The arrow pointing to the right repreuntB ihe known initial vekichy of tht TrMrv & 最堆t印H的M蛤狀思*虻色不廝野樂(lè)界:示的t奔r!畫(huà)數(shù)過(guò)供了火4忡居曲葉的上龍胃仁支，他人次云火1酉打軸“向|FIG3 HttFkF. lliv prodiLtion of Iha

11、hull on of lilt trjin Jit tirna-1 = 1Jnd Un 加廿 3 of uncerljilnty in thml pvditlion ii ihowmi. The ronfldMce in the know/dgR of th哥 pan of tlw tralmdcrpa4ed, a$ ww ar not cerrairii if th train hx und#rgon anyaccelerations or川的atiun等 in the intervening period from r = 0 B t = 1圖3這里繪(Hft二l財(cái)X的火隼粒宣BfflPB

12、JHI的不/定性.火車(chē)位置的K值度降悵J 這是一惶我ri不確定火隼從20到,1時(shí)菊是否遙行AG4| Shows the rrieauremenrt of the kjcaTion of the train at tiinie r = 1 and the Kewell of unceFtainty in that noisy measuremert,. rprHentd by th鑿 b4ue Gaussian pdf. The ccmbimed knciM叁dge of ttiii system is. provided by muftipiyMg these two pdft BgethM國(guó)

13、I火里花廠1時(shí)劃的位置用以覆置Hl不*出性程度用色的*%概率密度晶獨(dú)入示.聚統(tǒng)的取舍信息由兩個(gè)ndf的垂粗根0UfF(G5| Shows the rww pdf (grrap) generated by muhiply ing tfrie pdfs aswaateid wilii the preditrion and meBsunemerit of the trains I outran at tim t = 1B Thh new pdf provbdK the best estimate of tho location of tha traia, by fusing the data fro

14、m the predialon and the meanremenl.由t=l明刎或啟位置的榛器I耶拒陽(yáng)人息宰和li工H ：娓枳生或新晌粗窣密度國(guó)Ik (M色j .上吸新的喉備甯r班輪的il旃府Hi剖機(jī)員用肥令賽三火中位置的眼惺估i I .在此，通過(guò)考慮一個(gè)簡(jiǎn)單的以為跟蹤問(wèn)題，具體為一列火車(chē)行駛在一條鐵路上,車(chē)位置的最佳估計(jì)值（或者更精確地說(shuō)，是安裝在火車(chē)車(chē)頂上的無(wú)線電天線的位置）?？捎玫男畔?lái)源有兩個(gè)：1）基于上次獲得的火車(chē)位置和速度的預(yù)測(cè)值以及2）部署在軌道側(cè)的無(wú)線電測(cè)距系統(tǒng)對(duì)火車(chē)位置的測(cè)量結(jié)果。預(yù)測(cè)和量測(cè)信息的組合能夠提供火車(chē)位置的最可能的估計(jì)值。該系統(tǒng)如圖1所示已知一定精度的系統(tǒng)的初始

15、（t 0秒）狀態(tài)，并如圖2所示?；疖?chē)的位置由高斯概率密度函數(shù)給出。我們能夠根據(jù)t 0秒已知的火車(chē)的位置和速度信息以及火車(chē)最大的加速度來(lái)估計(jì)在下一時(shí)刻（t 1秒）火車(chē)最大可能出現(xiàn)的新位置。實(shí)際飛照（呂計(jì)Pr&cietioni (Esiirraiej /M1的噪聲) MaiwiUFunMNMy)對(duì)卡爾曼濾波器的量測(cè)更新方程進(jìn)行推導(dǎo)在每個(gè)量測(cè)到來(lái)時(shí)，我們希望知道火諭輯估汁)Prediction (Etmate用茗(帶有喚力1Measuwniil (No6y)/zPredielion (Estimate)中，我們可能對(duì)駕駛員在剎車(chē)或者油門(mén)上的控制有所了解。無(wú)論如何，我們都可以火車(chē)的新位置，在圖3中用新

16、的高斯概率密度函數(shù)進(jìn)行表示， 該函數(shù)擁有新的 均值和方差。數(shù)學(xué)上，該步驟由式（1）表示。圖中，方差的增加表示與t 0時(shí) 刻相比，位置估計(jì)的精度有所降低，這是因?yàn)閺膖 0到t 1任何加速或者減速的 不確定性引起的火車(chē)噪聲增加引起的。在t 1時(shí)刻，我們還使用無(wú)線電定位系統(tǒng)對(duì)火車(chē)的位置進(jìn)行了量測(cè)，如圖 4 中藍(lán)色部分的高斯概率密度函數(shù)所示。 通過(guò)結(jié)合預(yù)測(cè)和量測(cè)信息，可以對(duì)火車(chē)的 位置做出最佳估計(jì)。兩者的結(jié)合方法可以通過(guò)兩個(gè)概率密度函數(shù)的乘積獲得， 如 圖5所示。利用高斯函數(shù)的一個(gè)關(guān)鍵性質(zhì)：兩個(gè)高斯函數(shù)的乘積是另一個(gè)高斯函數(shù)。這 是至關(guān)重要的，因?yàn)樗试S隨著時(shí)間的增長(zhǎng)而增加無(wú)數(shù)的高斯 pdf,但是結(jié)果

17、函 數(shù)的復(fù)雜度或項(xiàng)數(shù)不會(huì)增加；在每個(gè)時(shí)間段之后，新的 pdf都完全由高斯函數(shù)表 示。這是卡爾曼濾波的遞歸特性能夠簡(jiǎn)潔的關(guān)鍵所在。圖中所描述的過(guò)程下面給出去數(shù)學(xué)形式以推導(dǎo)卡爾曼濾波器的量測(cè)更新方 程。圖3中紅色高斯函數(shù)表示的預(yù)測(cè)概率密度函數(shù)可以表示為方程Vi r；2(8)圖4中藍(lán)色高斯函數(shù)表示的量測(cè)概率密度函數(shù)可以表示為方程2V2 r；這兩個(gè)概率密度函數(shù)提供的信息通過(guò)將兩者相乘得到，即同時(shí)考慮預(yù)測(cè)和量測(cè) （如圖5所示）。因此，新的概率密度函數(shù)表示的融合信息來(lái)自預(yù)測(cè)和量測(cè)，表示為當(dāng)前時(shí)刻系統(tǒng)的最優(yōu)估計(jì)，并由兩個(gè)高斯函數(shù)的乘積給出，即 TOC o 1-5 h z 22r 1r 2(10)12 121

18、2 fe 1e 2212222r 12 12得到的新函數(shù)可以重新寫(xiě)成高斯形式y(tǒng)fused r ； fused , fused2r fused2 2 fused2fused(11)其中fused2212212212212112212(12)并且22212fused 22124(13) TOC o 1-5 h z 2112212關(guān)于式(11)至(13)的推導(dǎo)過(guò)程可以參考文獻(xiàn)6。最后兩個(gè)方程表示卡爾曼濾波器算法的更新步驟，具體的如下文所示。然而，為 了呈現(xiàn)更一般的情況，我們需要考慮擴(kuò)展此示例。在上面的例子中，假設(shè)預(yù)測(cè)和量測(cè)是相同的坐標(biāo)系中以相同的單位進(jìn)行的。這會(huì)產(chǎn)生一對(duì)特別簡(jiǎn)潔的方程式， 分別代表預(yù)

19、測(cè)和兩個(gè)更新步驟。然而，需要注 意的是，在實(shí)際中往往需要一個(gè)函數(shù)將預(yù)測(cè)和量測(cè)映射到同一域中。 本文中例子 的一個(gè)現(xiàn)實(shí)擴(kuò)展為，火車(chē)的位置的預(yù)測(cè)直接沿著鐵路線并且單位是米， 但是行駛 時(shí)間的以秒為單位記錄。為了使預(yù)測(cè)和量測(cè)的概率密度函數(shù)相乘， 必須將一個(gè)概率密度函數(shù)轉(zhuǎn)換至另一個(gè)概率密度函數(shù)的域，標(biāo)準(zhǔn)的做法是通過(guò)轉(zhuǎn)換矩陣H t將預(yù)測(cè)映射到量測(cè)域?，F(xiàn)在，我彳門(mén)回顧式(8)和(9), y1和y2不在表示沿著鐵路線行進(jìn)的米數(shù)，我們考慮分別y2表示從x 0處無(wú)線電天線發(fā)出的電磁波傳播至火車(chē)所在位置時(shí)所用 的秒數(shù)?？臻g預(yù)測(cè)概率密度函數(shù)通過(guò)光速c來(lái)縮放函數(shù)從而轉(zhuǎn)換至量測(cè)域。因 此，方程(8)和(9)必須重新為2

20、r c2y s; 1,(14)2 1 c以及(15)其中，兩個(gè)分部現(xiàn)在都定義在了量測(cè)域， 射頻信號(hào)沿著時(shí)間軸傳播，并且量測(cè)單 位是秒。按照之前的推導(dǎo)我們現(xiàn)在發(fā)現(xiàn)fusedc211-2-ccfused -21c21c(16)替換 H 1. c and K H 12 : H2 12可得fusedi+K(17)同樣，融合的協(xié)方差估計(jì)變?yōu)? fused2 c41c21c2fused21c212-2cKH 12(18)現(xiàn)在，我們可以將標(biāo)量推導(dǎo)得出的某些項(xiàng)與卡爾曼濾波算法中使用的標(biāo)準(zhǔn)向量和 矩陣進(jìn)行比較：fused12 fused21222HXt|t :數(shù)據(jù)融合后的狀態(tài)向量?t|t 1:數(shù)據(jù)融合之前的狀態(tài)

21、向量，即預(yù)測(cè)心：數(shù)據(jù)融合后的協(xié)方差矩陣（置信度） 卜Pt|t 1 :數(shù)據(jù)融合前的協(xié)方差矩陣（置信度）Zt:量測(cè)矢量Rt:量測(cè)噪聲協(xié)方差矩陣Ht:用于將狀態(tài)向量參數(shù)映射到測(cè)量域的轉(zhuǎn)換矩陣H 21口 2 21 2Kt %K；也巳K；Rt :卡爾曼增益。H 12現(xiàn)在可以很容易地看到標(biāo)準(zhǔn)卡爾曼濾波器方程與上面推導(dǎo)的（17）和（18）的關(guān)系： H12fused 122221H 12?t|tXt|t 1 K t ztHt?t|t 12 fusedH2221Pt|t Pt|t 1 KtHtPtt 1結(jié)論卡爾曼濾波器可以通過(guò)一個(gè)簡(jiǎn)單的推導(dǎo) （涉及標(biāo)量數(shù)學(xué)、基本代數(shù)操作）和一個(gè) 易于理解的思想實(shí)驗(yàn)來(lái)教授。這種方法應(yīng)該允許對(duì)數(shù)學(xué)缺乏興趣的學(xué)生以一種直 觀的方式理解卡爾曼濾波器的數(shù)學(xué)核心，并通過(guò)高斯函數(shù)的唯一乘法特性能夠理 解該濾波器的遞歸特性。參考文獻(xiàn)R. E. Kalman, A new approa