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

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

什么是卡爾曼濾波？

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

因此卡爾曼濾波非常適合不斷變化的系統，它的優點還有內存占用較?。ㄖ恍璞Ａ羟耙粋€狀態）、速度快，是實時問題和嵌入式系統的理想選擇。

如果你曾經Google過卡爾曼濾波的教程（如今有一點點改善），你會發現相關的算法教程非?？膳拢乙矝]具體說清楚是什么。事實上，卡爾曼濾波很簡單，如果我們以正確的方式看它，理解是很水到渠成的事。

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

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

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

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

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

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

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

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

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

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

還是上面這個問題，我們有一個狀態，它和速度、位置有關：

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

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

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

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

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

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

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

用矩陣描述問題

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

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

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

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

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

換成矩陣形式：

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

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

外部影響

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

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

把它轉成矩陣形式：

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

外部不確定性

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

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

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

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

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

我們在原式上加入Qk：

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

現在，有了這些概念介紹，我們可以把傳感器數據輸入其中。

通過測量來細化估計值

我們可能有好幾個傳感器，它們一起提供有關系統狀態的信息。傳感器的作用不是我們關心的重點，它可以讀取位置，可以讀取速度，重點是，它能告訴我們關于狀態的間接信息——它是狀態下產生的一組讀數。

請注意，讀數的規模和狀態的規模不一定相同，所以我們把傳感器讀數矩陣設為Hk。

把這些分布轉換為一般形式：

卡爾曼濾波的一大優點是擅長處理傳感器噪聲。換句話說，由于種種因素，傳感器記錄的信息其實是不準的，一個狀態事實上可以產生多種讀數。

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

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

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

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

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

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

結合高斯

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

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

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

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

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

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

把它們結合在一起

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

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

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

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