一天精通無人機第 43 講 高級篇系列：卡爾曼濾波5個重要公式的推導

歡迎關注公眾號：編程外星人，加入技術交流群

卡爾曼濾波在無人機導航和控制系統中起著至關重要的作用。由於很多感測器的讀數並不是十分準確，而且雜訊很大，無人機的控制模型的不確定性，在許多情況下感測器的不確定性可以假設其雜訊誤差滿足高斯分布。卡爾曼濾波器是通過不斷對上一時刻的狀態進行迭代，得出當前時刻的狀態，其在更新迭代過程中用到了5個重要的公式：

其中，X ?_t為狀態預測；F_t為狀態轉換矩陣；P_t為預測值協方差；R_t為測量雜訊；H_t為觀測矩陣；Q_t為系統雜訊；K為卡爾曼增益；Z_t為系統測量；P_t^為狀態值協方差。

由：

設x和y符合高斯分布，於是有x和y的聯合分布：

根據高斯特性有：

求x，y分布：

其中：

先求x：

其中：

再求y：

其中：

於是，求期望：

方差：

對（9）式子中等號右邊矩陣的每一項進行計算：

1）左上：

2）右下

3）左下

4）右上

接下來計算（6）中的期望：

得到的結果就是公式（4），而且式中的K就是公式（3），即卡爾曼增益。再來計算（6）中的方差：

得到的式子就是公式（5）。再由公式（7）得出（1）。由：

得到式子（10）就是公式（2）。到此，卡爾曼濾波的5個重要公式推導完畢。

歡迎關注公眾號：編程外星人，加入技術交流群

推薦閱讀：