▊ 卡爾曼濾波公式推導

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

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

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

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

因而我們有下式

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

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

聯立二式并稍作變化，我們得到：

令

為卡爾曼增益， 我們有

對應的矩陣形式為：

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

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

分別對應于

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

, 方差記為

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

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

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

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

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

▊ 總結

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

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

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

參考文獻:

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

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

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

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