01 Kalman用于解決什么的問(wèn)題

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

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

02 先來(lái)看一下姿態(tài)估計(jì)問(wèn)題

03 看幾個(gè)例子

（1）例題1

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

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

第二步，寫出觀測(cè)方程模型

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

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

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

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

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

04 計(jì)算流程

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

下圖更簡(jiǎn)潔的展示了計(jì)算流程：

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

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

審核編輯：劉清