卡爾曼濾波（Kalman filter）是一種高效的自回歸濾波器，它能在存在諸多不確定性情況的組合信息中估計(jì)動(dòng)態(tài)系統(tǒng)的狀態(tài)，是一種強(qiáng)大的、通用性極強(qiáng)的工具。它的提出者，魯?shù)婪?E.卡爾曼，在一次訪問NASA埃姆斯研究中心時(shí)，發(fā)現(xiàn)這種方法能幫助解決阿波羅計(jì)劃的軌道預(yù)測問題，后來NASA在阿波羅飛船的導(dǎo)航系統(tǒng)中確實(shí)也用到了這個(gè)濾波器。最終，飛船正確駛向月球，完成了人類歷史上的第一次登月。

本文是國外博主Bzarg在2015年寫的一篇圖解。雖然是幾年前的文章，但是動(dòng)態(tài)定位、自動(dòng)導(dǎo)航、時(shí)間序列模型、衛(wèi)星導(dǎo)航——卡爾曼濾波的應(yīng)用范圍依然非常廣。那么，作為軟件工程師和機(jī)器學(xué)習(xí)工程師，你真的了解卡爾曼濾波嗎？

什么是卡爾曼濾波？

對(duì)于這個(gè)濾波器，我們幾乎可以下這么一個(gè)定論：只要是存在不確定信息的動(dòng)態(tài)系統(tǒng)，卡爾曼濾波就可以對(duì)系統(tǒng)下一步要做什么做出有根據(jù)的推測。即便有噪聲信息干擾，卡爾曼濾波通常也能很好的弄清楚究竟發(fā)生了什么，找出現(xiàn)象間不易察覺的相關(guān)性。

因此卡爾曼濾波非常適合不斷變化的系統(tǒng)，它的優(yōu)點(diǎn)還有內(nèi)存占用較?。ㄖ恍璞Ａ羟耙粋€(gè)狀態(tài)）、速度快，是實(shí)時(shí)問題和嵌入式系統(tǒng)的理想選擇。

如果你曾經(jīng)Google過卡爾曼濾波的教程（如今有一點(diǎn)點(diǎn)改善），你會(huì)發(fā)現(xiàn)相關(guān)的算法教程非?？膳?，而且也沒具體說清楚是什么。事實(shí)上，卡爾曼濾波很簡單，如果我們以正確的方式看它，理解是很水到渠成的事。

本文會(huì)用大量清晰、美觀的圖片和顏色來解釋這個(gè)概念，讀者只需具備概率論和矩陣的一般基礎(chǔ)知識(shí)。

我們能用卡爾曼濾波做什么？

讓我們舉個(gè)例子：你造了一個(gè)可以在樹林里四處溜達(dá)的小機(jī)器人，為了讓它實(shí)現(xiàn)導(dǎo)航，機(jī)器人需要知道自己所處的位置。

也就是說，機(jī)器人有一個(gè)包含位置信息和速度信息的狀態(tài)xk：

注意，在這個(gè)例子中，狀態(tài)是位置和速度，放進(jìn)其他問題里，它也可以是水箱里的液體體積、汽車引擎溫度、觸摸板上指尖的位置，或者其他任何數(shù)據(jù)。

我們的小機(jī)器人裝有GPS傳感器，定位精度10米。雖然一般來說這點(diǎn)精度夠用了，但我們希望它的定位誤差能再小點(diǎn)，畢竟樹林里到處都是土坑和陡坡，如果機(jī)器人稍稍偏了那么幾米，它就有可能滾落山坡。所以GPS提供的信息還不夠充分。

我們也可以預(yù)測機(jī)器人是怎么移動(dòng)的：它會(huì)把指令發(fā)送給控制輪子的馬達(dá)，如果這一刻它始終朝一個(gè)方向前進(jìn)，沒有遇到任何障礙物，那么下一刻它可能會(huì)繼續(xù)堅(jiān)持這個(gè)路線。但是機(jī)器人對(duì)自己的狀態(tài)不是全知的：它可能會(huì)逆風(fēng)行駛，輪子打滑，滾落顛簸地形……所以車輪轉(zhuǎn)動(dòng)次數(shù)并不能完全代表實(shí)際行駛距離，基于這個(gè)距離的預(yù)測也不完美。

這個(gè)問題下，GPS為我們提供了一些關(guān)于狀態(tài)的信息，但那是間接的、不準(zhǔn)確的；我們的預(yù)測提供了關(guān)于機(jī)器人軌跡的信息，但那也是間接的、不準(zhǔn)確的。

但以上就是我們能夠獲得的全部信息，在它們的基礎(chǔ)上，我們是否能給出一個(gè)完整預(yù)測，讓它的準(zhǔn)確度比機(jī)器人搜集的單次預(yù)測匯總更高？用了卡爾曼濾波，這個(gè)問題可以迎刃而解。

卡爾曼濾波眼里的機(jī)器人問題

還是上面這個(gè)問題，我們有一個(gè)狀態(tài)，它和速度、位置有關(guān)：

我們不知道它們的實(shí)際值是多少，但掌握著一些速度和位置的可能組合，其中某些組合的可能性更高：

卡爾曼濾波假設(shè)兩個(gè)變量（在我們的例子里是位置和速度）都應(yīng)該是隨機(jī)的，而且符合高斯分布。每個(gè)變量都有一個(gè)均值μ，它是隨機(jī)分布的中心；有一個(gè)方差σ2，它衡量組合的不確定性。

在上圖中，位置和速度是不相關(guān)的，這意味著我們不能從一個(gè)變量推測另一個(gè)變量。

那么如果位置和速度相關(guān)呢？如下圖所示，機(jī)器人前往特定位置的可能性取決于它擁有的速度。

這不難理解，如果基于舊位置估計(jì)新位置，我們會(huì)產(chǎn)生這兩個(gè)結(jié)論：如果速度很快，機(jī)器人可能移動(dòng)得更遠(yuǎn)，所以得到的位置會(huì)更遠(yuǎn)；如果速度很慢，機(jī)器人就走不了那么遠(yuǎn)。

這種關(guān)系對(duì)目標(biāo)跟蹤來說非常重要，因?yàn)樗峁┝烁嘈畔ⅲ阂粋€(gè)可以衡量可能性的標(biāo)準(zhǔn)。這就是卡爾曼濾波的目標(biāo)：從不確定信息中擠出盡可能多的信息！

為了捕獲這種相關(guān)性，我們用的是協(xié)方差矩陣。簡而言之，矩陣的每個(gè)值是第i個(gè)變量和第j個(gè)變量之間的相關(guān)程度（由于矩陣是對(duì)稱的，i和j的位置可以隨便交換）。我們用Σ表示協(xié)方差矩陣，在這個(gè)例子中，就是Σij。

用矩陣描述問題

為了把以上關(guān)于狀態(tài)的信息建模為高斯分布（圖中色塊），我們還需要k時(shí)的兩個(gè)信息：最佳估計(jì)x?k（均值，也就是μ），協(xié)方差矩陣Pk。（雖然還是用了位置和速度兩個(gè)變量，但只要和問題相關(guān)，卡爾曼濾波可以包含任意數(shù)量的變量）

接下來，我們要通過查看當(dāng)前狀態(tài)（k-1時(shí)）來預(yù)測下一個(gè)狀態(tài)（k時(shí)）。這里我們查看的狀態(tài)不是真值，但預(yù)測函數(shù)無視真假，可以給出新分布：

我們可以用矩陣Fk表示這個(gè)預(yù)測步驟：

它從原始預(yù)測中取每一點(diǎn)，并將其移動(dòng)到新的預(yù)測位置。如果原始預(yù)測是正確的，系統(tǒng)就會(huì)移動(dòng)到新位置。

這是怎么做到的？為什么我們可以用矩陣來預(yù)測機(jī)器人下一刻的位置和速度？下面是個(gè)簡單公式：

換成矩陣形式：

這是一個(gè)預(yù)測矩陣，它能給出機(jī)器人的下一個(gè)狀態(tài)，但目前我們還不知道協(xié)方差矩陣的更新方法。這也是我們要引出下面這個(gè)等式的原因：如果我們將分布中的每個(gè)點(diǎn)乘以矩陣A，那么它的協(xié)方差矩陣會(huì)發(fā)生什么變化？

把這個(gè)式子和上面的最佳估計(jì)x?k結(jié)合，可得：

外部影響

但是，除了速度和位置，外因也會(huì)對(duì)系統(tǒng)造成影響。比如模擬火車運(yùn)動(dòng)，除了列車自駕系統(tǒng)，列車操作員可能會(huì)手動(dòng)調(diào)速。在我們的機(jī)器人示例中，導(dǎo)航軟件也可以發(fā)出停止指令。對(duì)于這些信息，我們把它作為一個(gè)向量uk，納入預(yù)測系統(tǒng)作為修正。

假設(shè)油門設(shè)置和控制命令是已知的，我們知道火車的預(yù)期加速度a。根據(jù)運(yùn)動(dòng)學(xué)基本定理，我們可得：

把它轉(zhuǎn)成矩陣形式：

Bk是控制矩陣，uk是控制向量。如果外部環(huán)境異常簡單，我們可以忽略這部分內(nèi)容，但是如果添加了外部影響后，模型的準(zhǔn)確率還是上不去，這又是為什么呢？

外部不確定性

當(dāng)一個(gè)國家只按照自己的步子發(fā)展時(shí)，它會(huì)自生自滅。當(dāng)一個(gè)國家開始依賴外部力量發(fā)展時(shí)，只要這些外部力量是已知的，我們也能預(yù)測它的存亡。

但是，如果存在我們不知道的力量呢？當(dāng)我們監(jiān)控無人機(jī)時(shí)，它可能會(huì)受到風(fēng)的影響；當(dāng)我們跟蹤輪式機(jī)器人時(shí)，它的輪胎可能會(huì)打滑，或者粗糙地面會(huì)降低它的移速。這些因素是難以掌握的，如果出現(xiàn)其中的任意一種情況，預(yù)測結(jié)果就難以保障。

這要求我們?cè)诿總€(gè)預(yù)測步驟后再加上一些新的不確定性，來模擬和“世界”相關(guān)的所有不確定性：

如上圖所示，加上外部不確定性后， x?k?1的每個(gè)預(yù)測狀態(tài)都可能會(huì)移動(dòng)到另一點(diǎn)，也就是藍(lán)色的高斯分布會(huì)移動(dòng)到紫色高斯分布的位置，并且具有協(xié)方差Qk。換句話說，我們把這些不確定影響視為協(xié)方差Qk的噪聲。

這個(gè)紫色的高斯分布擁有和原分布相同的均值，但協(xié)方差不同。

我們?cè)谠缴霞尤隥k：

簡而言之，這里“新的最佳估計(jì)=原最佳估計(jì)+已知外部影響”，“新的不確定性預(yù)測=原預(yù)測+外部環(huán)境的不確定性”。

現(xiàn)在，有了這些概念介紹，我們可以把傳感器數(shù)據(jù)輸入其中。

通過測量來細(xì)化估計(jì)值

我們可能有好幾個(gè)傳感器，它們一起提供有關(guān)系統(tǒng)狀態(tài)的信息。傳感器的作用不是我們關(guān)心的重點(diǎn)，它可以讀取位置，可以讀取速度，重點(diǎn)是，它能告訴我們關(guān)于狀態(tài)的間接信息——它是狀態(tài)下產(chǎn)生的一組讀數(shù)。

請(qǐng)注意，讀數(shù)的規(guī)模和狀態(tài)的規(guī)模不一定相同，所以我們把傳感器讀數(shù)矩陣設(shè)為Hk。

把這些分布轉(zhuǎn)換為一般形式：

卡爾曼濾波的一大優(yōu)點(diǎn)是擅長處理傳感器噪聲。換句話說，由于種種因素，傳感器記錄的信息其實(shí)是不準(zhǔn)的，一個(gè)狀態(tài)事實(shí)上可以產(chǎn)生多種讀數(shù)。

我們將這種不確定性（即傳感器噪聲）的協(xié)方差設(shè)為Rk，讀數(shù)的分布均值是zk。

現(xiàn)在我們得到了兩塊高斯分布，一塊圍繞預(yù)測的均值，另一塊圍繞傳感器讀數(shù)。

如果要生成靠譜預(yù)測，模型必須調(diào)和這兩個(gè)信息。也就是說，對(duì)于任何可能的讀數(shù)(z1,z2)，這兩種方法預(yù)測的狀態(tài)都有可能是準(zhǔn)的，也都有可能是不準(zhǔn)的。重點(diǎn)是我們?cè)趺凑业竭@兩個(gè)準(zhǔn)確率。

最簡單的方法是兩者相乘：

兩塊高斯分布相乘后，我們可以得到它們的重疊部分，這也是會(huì)出現(xiàn)最佳估計(jì)的區(qū)域。換個(gè)角度看，它看起來也符合高斯分布：

事實(shí)證明，當(dāng)你把兩個(gè)高斯分布和它們各自的均值和協(xié)方差矩陣相乘時(shí)，你會(huì)得到一個(gè)擁有獨(dú)立均值和協(xié)方差矩陣的新高斯分布。最后剩下的問題就不難解決了：我們必須有一個(gè)公式來從舊的參數(shù)中獲取這些新參數(shù)！

結(jié)合高斯

讓我們從一維看起，設(shè)方差為σ2，均值為μ，一個(gè)標(biāo)準(zhǔn)一維高斯鐘形曲線方程如下所示：

那么兩條高斯曲線相乘呢？

把這個(gè)式子按照一維方程進(jìn)行擴(kuò)展，可得：

如果有些太復(fù)雜，我們用k簡化一下：

還記得之前我們算不確定性的時(shí)候多麻煩嗎？這里結(jié)合高斯算多簡單！以上是一維的內(nèi)容，如果是多維空間，把這個(gè)式子轉(zhuǎn)成矩陣格式：

這個(gè)矩陣K就是我們說的卡爾曼增益，easy！

把它們結(jié)合在一起

截至目前，我們有用矩陣 (μ0,Σ0)=(Hkx?k,HkPkHTk)預(yù)測的分布，有用傳感器讀數(shù) (μ1,Σ1)=(zk,Rk)預(yù)測的分布。把它們代入上節(jié)的矩陣等式中：

相應(yīng)的，卡爾曼增益就是：

考慮到K里還包含著一個(gè)Hk，我們?cè)倬喴幌律鲜龅仁剑?/p>

最后，x?k′是我們的最佳估計(jì)值，我們可以把它繼續(xù)放進(jìn)去做另一輪預(yù)測：