卡爾曼濾波是一種在不確定狀況下組合多源信息得到所需狀態(tài)最優(yōu)估計的一種方法,。本文將簡要介紹卡爾曼濾波的原理及推導,。

這里寫圖片描述

什么是卡爾曼濾波

首先定義問題：對于某一系統(tǒng)，知道當前狀態(tài)XtX_t,，存在以下兩個問題：

1. 經(jīng)過時間

后,，下個狀態(tài)

如何求出？

1. 假定已求出

,，在t+1t+1時刻收到傳感器的非直接信息

,，如何對狀態(tài)

進行更正？

這兩個問題正是卡爾曼濾波要解決的問題,，形式化兩個問題如下：

1. 預測未來
2. 修正當下

下面,，將以機器人導航為例，從預測未來和修正當下兩個角度介紹卡爾曼濾波器,。

卡爾曼濾波的原理

問題場景如下：一個機器人,，我們想知道它實時的狀態(tài)

，同時也想做到預測未來和修正當下這兩件事,。

其狀態(tài)xx表示為一維大小為2的向量,，元素分別表示位置信息與速度信息：

可是狀態(tài)xx不一定是精準的，其不確定性用協(xié)方差表示：

預測未來

只考慮自身狀態(tài)

只考慮自身狀態(tài)的情況下,，根據(jù)物理公式,，可得：

用矩陣表示如下：

在狀態(tài)變化的過程中引入了新的不確定性，根據(jù)協(xié)方差的乘積公式可得：

考慮外部狀態(tài)

外部狀態(tài),，這里以加速度為例,，引入變量

同時，環(huán)境仍然存在我們無法刻畫的誤差,，以

表示,，最終的預測公式如下：

從上述式子可見：

1. 新的最優(yōu)估計是之前最優(yōu)估計之前最優(yōu)估計}}的預測加上已知的外界影響已知的外界影響}}的修正。
2. 新的不確定度是預測的不確定度加上環(huán)境的不確定度,。

修正當下

我們已得到

,，下面要通過觀測到的測量值

對

進行更新。

因為

和

的數(shù)據(jù)尺度不一定相同,，例如

包含了笛卡爾的坐標信息,，使用radar得到的

則包含極坐標信息。所以首先應該把兩者放在相同的尺度下去比較,，尺度轉換使用Hk\mathbf{H}_k將預測信息轉化為測量信息的尺度,。

這樣一來，便得到測量尺度上的兩個分布：

1. 測量值的分布

1. 預測值變換后的分布

下面一個問題就是如何用這個兩個分布組成新的分布,。

這里寫圖片描述

簡單的一維情況如下：

總結

預測未來

修正當下

卡爾曼濾波需要內存少,，計算速度快,，適合實時性情況與嵌入式設備的需要。