在上一篇文章中手把手推導了一遍卡爾曼增益，不熟悉的小伙伴可以看

養生的控制人：卡爾曼增益推導?zhuanlan.zhihu.com

這里再回顧一下重點。

問題重述

假設真實系統為

其中

。

我們對系統狀態的估計（數據融合）為

其中卡曼爾增益為

我們可以看到卡爾曼增益中的估計協方差矩陣

還是未知的，因此我們需要把它表示出來。

估計協方差矩陣推導

根據定義

帶入真實系統模型和狀態估計的模型

再帶入估計協方差矩陣的表達式

把轉置放進去

把括號打開

由于四項之間是線性相加的，可以把期望的運算放進去，等于每一項的期望。

注意到

，而 是作用到 上的，所以

和 是相互獨立的。因為相互獨立，所以相乘的期望等于期望的相乘

因此估計的協方差矩陣剩下兩項

有了這個表達式后，我們就可以用卡爾曼濾波器來估計狀態了！

卡爾曼濾波器兩步法

目前為止卡爾曼增益中各項都是已知的了，因此可以用于估計了，卡爾曼濾波分為兩個步驟：預測和校正。

Step1. 預測

（1）利用系統模型對狀態變量進行遞推估計

（2）對協方差矩陣進行估計

Step2. 校正

（3）計算卡爾曼增益

（4）數據融合即對狀態的估計

（5）更新協方差矩陣用于下一次的預測

至此，卡爾曼濾波（估計器）就推導完畢了，給定初值

，它就能遞推估計了。記住核心思想無非是：數據融合、預測校正。

---

公式（5）的推導

在上一篇文章中我們推導得到協方差矩陣的表達式為

合并同類項

帶入卡爾曼增益

總結

以上是生活随笔為你收集整理的卡尔曼_卡尔曼估计两步法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。