01 Kalman用于解決什么的問題

卡爾曼濾波是一種利用線性系統狀態方程，通過系統輸入輸出觀測數據，對系統狀態進行最優估計的算法。由于觀測數據中包括系統中的噪聲和干擾的影響，所以最優估計也可看作是濾波過程。

人話就是：線性數學模型算出預測值+傳感測量值=更準確的測量值。

02 先來看一下姿態估計問題

03 看幾個例子

（1）例題1

（2）例題2——運動模型，寫出勻加速運動的狀態轉移方程

第一步，根據基本的物理運動方程，寫出狀態方程

第二步，寫出觀測方程模型

我開始也不明白這個觀測方程是啥意思，實際上這是模擬傳感器的測量值，S代表位移，V代表誤差。這里代表目標測量量為位移。

第三步，將第一步和第二步的狀態方程與觀測方程寫成矩陣形式

根據對應關系，可以得到系數：

其中A叫做狀態轉移矩陣，G叫做控制矩陣，H叫做預測矩陣

給定一個初值，就可以迭代得到后面的值了。

04 計算流程

05 詳細推導

下圖更簡潔的展示了計算流程：

其中F為控制矩陣，Q為預測不確定性，R為傳感器噪聲，H為映射矩陣，y為誤差，

S為方差之和，K為卡爾曼增益，P為更新后的協方差

審核編輯：劉清