卡爾曼濾波器介紹與理論分析（下）

▊ 卡爾曼濾波公式推導

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

對于系統(tǒng)中數(shù)據(jù)的收集，我們一般基于傳感器來實現(xiàn)。但是同一傳感器在同一時刻，可能因為外部原因?qū)е率占玫降?a target="_blank">信號不完全一樣。因而系統(tǒng)輸入初始值對應結(jié)果大概率有很多噪音。這里我們把收集數(shù)據(jù)的均值記為, 方差記為

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

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

因而我們有下式

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

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

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

令

為卡爾曼增益， 我們有

對應的矩陣形式為：

這里為了防止混淆，先停下來總結(jié)一下，

1)我們介紹了預測變量，下標為p，分布均值，方差

分別對應于

2)我們介紹了觀測變量，下標為ob，分布均值記為

, 方差記為

3)基于1，2，我們代入即可推導出完整的卡爾曼濾波公式

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

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

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

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

▊ 總結(jié)

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

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

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

參考文獻:

. 論智：圖說卡爾曼濾波，一份通俗易懂的教程

(https://zhuanlan.zhihu.com/p/39912633)

. 涅索斯襯衫：卡爾曼濾波（Kalman Filter）原理與公式推導

(https://zhuanlan.zhihu.com/p/48876718)