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

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

什么是卡爾曼濾波？

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

因此卡爾曼濾波非常適合不斷變化的系統(tǒng)，它的優(yōu)點還有內(nèi)存占用較小（只需保留前一個狀態(tài)）、速度快，是實時問題和嵌入式系統(tǒng)的理想選擇。

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

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

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

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

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

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

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

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

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

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

卡爾曼濾波眼里的機器人問題

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

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

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

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

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

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

這種關(guān)系對目標跟蹤來說非常重要，因為它提供了更多信息：一個可以衡量可能性的標準。這就是卡爾曼濾波的目標：從不確定信息中擠出盡可能多的信息！

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

用矩陣描述問題

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

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

我們可以用矩陣Fk表示這個預測步驟：

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

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

換成矩陣形式：

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

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

外部影響

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

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

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

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

外部不確定性

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

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

這要求我們在每個預測步驟后再加上一些新的不確定性，來模擬和“世界”相關(guān)的所有不確定性：

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

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

我們在原式上加入Qk：

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

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

通過測量來細化估計值

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

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

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

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

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

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

如果要生成靠譜預測，模型必須調(diào)和這兩個信息。也就是說，對于任何可能的讀數(shù)(z1,z2)，這兩種方法預測的狀態(tài)都有可能是準的，也都有可能是不準的。重點是我們怎么找到這兩個準確率。

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

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

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

結(jié)合高斯

讓我們從一維看起，設方差為σ2，均值為μ，一個標準一維高斯鐘形曲線方程如下所示：

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

把這個式子按照一維方程進行擴展，可得：

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

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

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

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

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

相應的，卡爾曼增益就是：

考慮到K里還包含著一個Hk，我們再精簡一下上述等式：

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