經典重溫：卡爾曼濾波器介紹與理論分析

卡爾曼濾波的背景

卡爾曼濾波常用于動態(tài)多變化系統(tǒng)中的狀態(tài)估計，是一種通用性強的自回歸濾波器。它的由來和NASA登月有關。其發(fā)明者魯?shù)婪?E.卡爾曼在一次訪問NASA的時候，發(fā)現(xiàn)阿波羅計劃中一個難點是軌道預測問題，因而提出了一種濾波器，可以幫助高效預測軌跡，輔助導航。NASA最終使用了這個濾波器，然后成功實現(xiàn)人類第一次登月計劃。卡爾曼濾波器由此得名。

卡爾曼濾波器可以用來估計不確定信息，并給出狀態(tài)量下一時刻的情況。即便在有噪聲干擾的情況下，也可以較好的預測下一狀態(tài)的情況，并找出多變量間不易察覺的相關性。因而卡爾曼濾波器可以很好適應不斷變化的系統(tǒng)，并且內存占用量低，推理速度快，比較適合資源受限制的場景。

卡爾曼濾波介紹

先來看一個簡單的問題。考慮一輛車在道路上行駛，他的狀態(tài)和運行速度，位置信息相關，用向量表示為：

其中 變量對應位置 變量對應速度，都為隨機變量且符合正態(tài)分布，每個分布都有一個均值（正太分布的中心）和對應的方定性）。那么卡爾曼濾波是怎么工作的呢？我們還是回到剛才的例子。上面例子中的這兩個變量看起來沒有明顯的對應關系，也就是\cjkstart?讠確定出位置。但是如果我們知道位置和速度的初始信息，其實是有辦法將兩個變量聯(lián)系起來的。假設車輛一直保持勻速行駛，通過下面的2意的是，換成其他問題，我們不一定能意識到這個公式的存在）

基于上面的公式，我們可以直觀的定義當前時刻和上一時刻之間的對應關系;假定系統(tǒng)中所有狀態(tài)量為 ，對應的有

這里我們稱 為預測矩陣，也會用 表示，預測矩陣的目的是刻畫從上一狀態(tài)到當前狀態(tài)對應的關系。這也就是卡爾曼濾波的目標。我們希望從不確定的系統(tǒng)中，盡可能的發(fā)掘出確定的信息。那么另一個問題是，如何知道有多少確定（不確定）的信息可以發(fā)掘呢?我們需要一個指標來衡量系統(tǒng)的不確定性。回到上個例子中，位置和速度的關系其實是不確定的，但是卻是有相關性的，只是不能直觀的看出來。那么如何衡量這種相關性呢?其中一個辦法就是使用協(xié)方差矩陣，來衡量兩個變量的相關程度。給定.，對應的協(xié)方差矩陣p為

考慮到預測矩陣的影響，給定上一時刻的 ，對應可以更新為

由此便解決了理想情況下卡爾曼濾波如何做預測的問題。但是實際情況下，系統(tǒng)中可能還是存在干擾或者噪聲因素。比如車輛行駛過程中，遇到雨天路滑則需要減速慢行，這種情況下道路狀況會作為外部因素，干擾系統(tǒng)行為和決策。因為針對變化量 ，會額外追加一個控制變量來建模外部因素。

假設行車遇到雨天需要減速，我們考慮了運動時刻的車輛加速度 為外部影響，那么有，

對比之前計算狀態(tài)的關系式，可以發(fā)現(xiàn)新引入的狀態(tài)對系統(tǒng)產生了影響，因而記為

同時我們使用 表示，引入的外部因素對于協(xié)方差的影響，因而有

這里總結一下，新引入的外部變量使得 1）當前的最佳估計不再為系統(tǒng)自身的最佳估計，還考慮了外部因素影響；2）當前的不確定性不再為系統(tǒng)自身的不確定性，引入了外部的不確定性做參考。

卡爾曼濾波公式推導

通過預測狀態(tài)量和不確定性，我們可以有較大把握預測對某一時刻下的狀態(tài)。到目前為止，我們很好地完成了對系統(tǒng)的建模，但是有一個問題沒有考慮到，那就是系統(tǒng)的初始值怎么得到。

對于系統(tǒng)中數(shù)據的收集，我們一般基于傳感器來實現(xiàn)。但是同一傳感器在同一時刻，可能因為外部原因導致收集得到的信號不完全一樣。因而系統(tǒng)輸入初始值對應結果大概率有很多噪音。這里我們把收集數(shù)據的均值記為，方差記為 。

現(xiàn)在就有了新的問題。對于當前時刻，我們有了兩組參考值，一組是預測值，一組是測量值。也許兩組都對，也許其中只有一組對，也可能都不準（比如傳感器測得不直接，偏差太大；預測值也偏差太大）。這種情況下，應該如何得到最終結果呢？

其中一個方案是，考慮直接融合兩組結果。我們假設這兩組數(shù)據都是正態(tài)分布，因而直接取pdf相乘就可以得到一個新的分布，也就是做一個聯(lián)合概率分布的計算。這個聯(lián)合概率分布本質上計算了前后狀態(tài)轉化對應分布和輸入輸出狀態(tài)分布聯(lián)合作用下的結果。

因而我們有下式：

這里要注意的是，兩個高斯pdf的乘積不是一個pdf （概率加和明顯不是1） ，但是正比于一個末知高斯分布，因而我們可以求得這個新分布的均方差，根據如下關系，

為了使其成立，我們需要保證：

聯(lián)立二式并稍作變化，我們得到：

為卡爾曼增益，我們有：

對應的矩陣形式為：

這里為了防止混淆，先停下來總結一下， 1）我們介紹了預測變量，下標為 ，分布均值，方差 分別對應于 2）我們介紹了觀測變量，下標為 ，分布均值記為 ，方差記為 3）基于1，2，我們代入即可推導出完整的卡爾曼濾波公式。

所以根據剛才列出的計算式分別代入，我們可以得到，

進而可以求解得狀態(tài)更新方程和卡爾曼增益求解公式：

注意這里的卡爾曼增益更新為

至此，便可以套公式更新卡爾曼濾波的參數(shù)了。

總結

卡爾曼濾波是處理線性系統(tǒng)非常好用的工具，對于不確定性的建模取自于物理、機械模型，所以對于現(xiàn)實生活中的狀態(tài)還是能很好把握的。對于非線性系統(tǒng)，拓展卡爾曼濾波（extended Kalman filter）也可以做到很好地處理。

本文通過現(xiàn)實場景建模引入問題，簡單介紹了卡爾曼濾波算法，對于更加清楚的公式推導請見參考列出的幾篇相關文章。

歡迎大家在下方留言或者將建議發(fā)到我的郵箱（[email protected]），以便更好地交流心得。另外感謝參考文獻所列諸作者。

參考文獻

1.論智：圖說卡爾曼濾波，一份通俗易懂的教程 （https：//zhuanlan.zhihu.com/p/39912633）

2.涅索斯襯衫：卡爾曼濾波（Kalman Filter）原理與公式推導 （https：//zhuanlan.zhihu.com/p/48876718

3.Calculate the product of two Gaussian PDF's （https：//math.stackexchange.com/questions/114420/calculate-the-product-of-two-gaussian-pdfs

4.詳解ADAS之FCW前碰撞預警系統(tǒng)技術原理-公司新聞-深圳前海米樂視科技有限公司 （http：//mileview.cn/gongsixinwen/22-43.html）

5.如何通俗并盡可能詳細地解釋卡爾曼濾波？（https：//www.zhihu.com/question/23971601）

如果覺得有用，就請分享到朋友圈吧！