本文是國外博主Bzarg在2015年寫的一篇圖解烦秩。雖然是幾年前的文章撤奸，但是動態(tài)定位诡壁、自動導航麸锉、時間序列模型晶府、衛(wèi)星導航——卡爾曼濾波的應用范圍依然非常廣堪置。
什么是卡爾曼濾波沐绒？
對于這個濾波器犁柜，我們幾乎可以下這么一個定論：只要是存在不確定信息的動態(tài)系統(tǒng)剖毯，卡爾曼濾波就可以對系統(tǒng)下一步要做什么做出有根據的推測圾笨。即便有噪聲信息干擾，卡爾曼濾波通常也能很好的弄清楚究竟發(fā)生了什么逊谋，找出現象間不易察覺的相關性擂达。因此卡爾曼濾波非常適合不斷變化的系統(tǒng)，它的優(yōu)點還有內存占用較薪鹤獭（只需保留前一個狀態(tài)）板鬓、速度快悲敷，是實時問題和嵌入式系統(tǒng)的理想選擇。
本文會用大量清晰俭令、美觀的圖片和顏色來解釋這個概念镀迂，讀者只需具備概率論和矩陣的一般基礎知識。
我們能用卡爾曼濾波做什么唤蔗？
讓我們舉個例子：你造了一個可以在樹林里四處溜達的小機器人探遵，為了讓它實現導航，機器人需要知道自己所處的位置妓柜。
也就是說箱季，機器人有一個包含位置信息和速度信息的狀態(tài):
注意，在這個例子中棍掐，狀態(tài)是位置和速度藏雏，放進其他問題里，它也可以是水箱里的液體體積作煌、汽車引擎溫度掘殴、觸摸板上指尖的位置，或者其他任何數據粟誓。
我們的小機器人裝有GPS傳感器奏寨，定位精度10米。雖然一般來說這點精度夠用了鹰服，但我們希望它的定位誤差能再小點病瞳，畢竟樹林里到處都是土坑和陡坡，如果機器人稍稍偏了那么幾米悲酷，它就有可能滾落山坡套菜。所以GPS提供的信息還不夠充分。
我們也可以預測機器人是怎么移動的：它會把指令發(fā)送給控制輪子的馬達设易，如果這一刻它始終朝一個方向前進逗柴，沒有遇到任何障礙物，那么下一刻它可能會繼續(xù)堅持這個路線顿肺。但是機器人對自己的狀態(tài)不是全知的：它可能會逆風行駛戏溺，輪子打滑，滾落顛簸地形……所以車輪轉動次數并不能完全代表實際行駛距離挟冠，基于這個距離的預測也不完美于购。
這個問題下，GPS為我們提供了一些關于狀態(tài)的信息知染，但那是間接的肋僧、不準確的；我們的預測提供了關于機器人軌跡的信息，但那也是間接的嫌吠、不準確的止潘。
但以上就是我們能夠獲得的全部信息，在它們的基礎上辫诅，我們是否能給出一個完整預測凭戴，讓它的準確度比機器人搜集的單次預測匯總更高？用了卡爾曼濾波炕矮，這個問題可以迎刃而解么夫。
卡爾曼濾波眼里的機器人問題
還是上面這個問題，我們有一個狀態(tài)肤视，它和速度档痪、位置有關：
我們不知道它們的實際值是多少，但掌握著一些速度和位置的可能組合邢滑，其中某些組合的可能性更高：
卡爾曼濾波假設兩個變量（在我們的例子里是位置和速度）都應該是隨機的腐螟，而且符合高斯分布。每個變量都有一個均值 困后，它是隨機分布的中心乐纸；有一個方差
，它衡量組合的不確定性摇予。
在上圖中汽绢，位置和速度是不相關的，這意味著我們不能從一個變量推測另一個變量趾盐。那么如果位置和速度相關呢庶喜？如下圖所示，機器人前往特定位置的可能性取決于它擁有的速度救鲤。
這不難理解，如果基于舊位置估計新位置秩冈，我們會產生這兩個結論：如果速度很快本缠，機器人可能移動得更遠，所以得到的位置會更遠入问；如果速度很慢丹锹，機器人就走不了那么遠。
這種關系對目標跟蹤來說非常重要芬失，因為它提供了更多信息：一個可以衡量可能性的標準楣黍。這就是卡爾曼濾波的目標：從不確定信息中擠出盡可能多的信息！
為了捕獲這種相關性棱烂，我們用的是協(xié)方差矩陣租漂。簡而言之，矩陣的每個值是第個變量和第
個變量之間的相關程度（由于矩陣是對稱的，
和
的位置可以隨便交換）哩治。我們用
表示協(xié)方差矩陣秃踩，在這個例子中，就是
用矩陣描述問題
為了把以上關于狀態(tài)的信息建模為高斯分布（圖中色塊）业筏，我們還需要 時的兩個信息：最佳估計
(均值憔杨，也就是
)，協(xié)方差矩陣
蒜胖。（雖然還是用了位置和速度兩個變量消别，但只要和問題相關，卡爾曼濾波可以包含任意數量的變量）
接下來台谢，我們要通過查看當前狀態(tài)（k-1時）來預測下一個狀態(tài)（k時）妖啥。這里我們查看的狀態(tài)不是真值，但預測函數無視真假对碌，可以給出新分布：
我們可以用矩陣表示這個預測步驟：
它從原始預測中取每一點荆虱，并將其移動到新的預測位置。如果原始預測是正確的朽们，系統(tǒng)就會移動到新位置怀读。這是怎么做到的？為什么我們可以用矩陣來預測機器人下一刻的位置和速度骑脱？下面是個簡單公式：
換成矩陣形式：
這是一個預測矩陣菜枷，它能給出機器人的下一個狀態(tài)，但目前我們還不知道協(xié)方差矩陣的更新方法叁丧。這也是我們要引出下面這個等式的原因：如果我們將分布中的每個點乘以矩陣A啤誊，那么它的協(xié)方差矩陣會發(fā)生什么變化？
把這個式子和上面的最佳估計結合拥娄，可得：
外部影響
但是蚊锹，除了速度和位置，外因也會對系統(tǒng)造成影響稚瘾。比如模擬火車運動牡昆，除了列車自駕系統(tǒng)，列車操作員可能會手動調速摊欠。在我們的機器人示例中丢烘，導航軟件也可以發(fā)出停止指令。對于這些信息些椒，我們把它作為一個向量 播瞳，納入預測系統(tǒng)作為修正。
假設油門設置和控制命令是已知的免糕，我們知道火車的預期加速度 赢乓。根據運動學基本定理忧侧，我們可得：
把它轉成矩陣形式：
是控制矩陣，
是控制向量骏全。如果外部環(huán)境異常簡單苍柏，我們可以忽略這部分內容，但是如果添加了外部影響后姜贡，模型的準確率還是上不去试吁，這又是為什么呢？
外部不確定性
當我們監(jiān)控無人機時楼咳，它可能會受到風的影響熄捍；當我們跟蹤輪式機器人時，它的輪胎可能會打滑母怜，或者粗糙地面會降低它的移速余耽。這些因素是難以掌握的，如果出現其中的任意一種情況苹熏，預測結果就難以保障碟贾。
如上圖所示，加上外部不確定性后轨域， 的每個預測狀態(tài)都可能會移動到另一點袱耽，也就是藍色的高斯分布會移動到紫色高斯分布的位置，并且具有協(xié)方差
干发。換句話說朱巨，我們把這些不確定影響視為協(xié)方差
的噪聲。
這個紫色的高斯分布擁有和原分布相同的均值枉长，但協(xié)方差不同冀续。
我們在原式上加
簡而言之，
新的最佳估計是基于原最佳估計 和已知外部影響校正后得到的預測必峰。
新的不確定性是基于原不確定性 和已知外部影響得到的預測洪唐。
現在，有了這些概念介紹自点，我們可以把傳感器數據輸入其中桐罕。
通過測量來細化估計值
我們可能有好幾個傳感器，它們一起提供有關系統(tǒng)狀態(tài)的信息桂敛。傳感器的作用不是我們關心的重點，它可以讀取位置溅潜，可以讀取速度术唬，重點是，它能告訴我們關于狀態(tài)的間接信息——它是狀態(tài)下產生的一組讀數滚澜。
請注意粗仓，讀數的規(guī)模和狀態(tài)的規(guī)模不一定相同，所以我們把傳感器讀數矩陣設為 。
把這些分布轉換為一般形式:
卡爾曼濾波的一大優(yōu)點是擅長處理傳感器噪聲借浊。換句話說塘淑，由于種種因素，傳感器記錄的信息其實是不準的蚂斤，一個狀態(tài)事實上可以產生多種讀數存捺。
我們將這種不確定性（即傳感器噪聲）的協(xié)方差設為，讀數的分布均值設為
∈镎簦現在我們得到了兩塊高斯分布捌治，一塊圍繞預測的均值，另一塊圍繞傳感器讀數纽窟。
如果要生成靠譜預測肖油，模型必須調和這兩個信息。也就是說臂港，對于任何可能的讀數 森枪，這兩種方法預測的狀態(tài)都有可能是準的，也都有可能是不準的审孽。重點是我們怎么找到這兩個準確率县袱。
最簡單的方法是兩者相乘
兩塊高斯分布相乘后，我們可以得到它們的重疊部分瓷胧，這也是會出現最佳估計的區(qū)域显拳。換個角度看，它看起來也符合高斯分布：
事實證明搓萧，當你把兩個高斯分布和它們各自的均值和協(xié)方差矩陣相乘時杂数，你會得到一個擁有獨立均值和協(xié)方差矩陣的新高斯分布。最后剩下的問題就不難解決了：我們必須有一個公式來從舊的參數中獲取這些新參數瘸洛！
結合高斯
讓我們從一維看起揍移，設方差為，均值為
反肋，一個標準一維高斯鐘形曲線方程如下所示：
那么兩條高斯曲線相乘呢那伐？
把這個式子按照一維方程進行擴展，可得：
如果有些太復雜石蔗，我們用簡化一下：
以上是一維的內容罕邀，如果是多維空間，把這個式子轉成矩陣格式：
這個矩陣就是我們說的卡爾曼增益 !!
把它們結合在一起
截至目前养距，我們有用矩陣預測的分布诉探，有用傳感器讀數
預測的分布。把它們代入上節(jié)的矩陣等式中
相應的棍厌，卡爾曼增益就是：
考慮到 里還包含著一個
肾胯，我們再精簡一下上式：
最后竖席，是我們的最佳估計值，我們可以把它繼續(xù)放進去做另一輪預測：
