01 Kalman用于解決什么的問題

卡爾曼濾波是一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法。由于觀測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計也可看作是濾波過程。

人話就是：線性數(shù)學(xué)模型算出預(yù)測值+傳感測量值=更準(zhǔn)確的測量值。

02 先來看一下姿態(tài)估計問題

03 看幾個例子

（1）例題1

（2）例題2——運(yùn)動模型，寫出勻加速運(yùn)動的狀態(tài)轉(zhuǎn)移方程

第一步，根據(jù)基本的物理運(yùn)動方程，寫出狀態(tài)方程

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

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

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

根據(jù)對應(yīng)關(guān)系，可以得到系數(shù)：

其中A叫做狀態(tài)轉(zhuǎn)移矩陣，G叫做控制矩陣，H叫做預(yù)測矩陣

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

04 計算流程

05 詳細(xì)推導(dǎo)

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

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

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

審核編輯：劉清