本節(jié)我們介紹機器人定位中技術中的卡爾曼濾波(Kalman Filter),，卡爾曼濾波作為連續(xù)狀態(tài)空間問題的一種解決方案,，已經(jīng)成功運用在火星登陸和自動導彈制導等領域。本質上卡爾曼濾波(Kalman Filter)是一置信度為正態(tài)分布的貝葉斯(Bayes Filter)濾波器,，它的置信度可以表示為一個均值向量和協(xié)方差矩陣的形式,，均值向量表示可能的狀態(tài)，協(xié)方差矩陣表示該狀態(tài)的不確定度,。

前提假設

卡爾曼濾波需要滿足以下三個前提假設,，才能保證在任意一個時刻t的置信度bel(x(t))都是正態(tài)分布：

1) 運動模型必須是一個線性高斯模型。也就是說t+1時刻的狀態(tài)必須是t時刻的狀態(tài)的線性函數(shù),，附加一個高斯隨機噪聲,。

其中A是一個矩陣，Δ是運動向量,，ξ是高斯隨機噪聲

2) 傳感器模型也是一個線性高斯模型,。

其中是一個矩陣,，ζ表示傳感器測量的高斯噪聲,。

3) 初始置信度bel(x0)是一個正態(tài)分布。

一維狀態(tài)空間卡爾曼濾波

先從最簡單的一維卡爾曼濾波說起,，假設一輛小車以δ的速度和一定的隨機噪聲?沿著一條直線前進,，則它的運動方程：

運動轉移模型為：

?^2是對運動不確定度的度量，是對運動噪聲?的一種反應,。

傳感器模型為：

ψ^2是對傳感器測量噪聲不確定度的度量,。

假設t時刻的置信度為：

則t+1時刻的預測置信度為:

置信度更新后：

其中：

可以看到傳感器模型更新后的正態(tài)分布的均值為傳感器測量的均值與舊的運動預測的均值的加權平均，權重為各自的不確定度,。

一維卡爾曼濾波算法

其中變量k被成為卡爾曼增益,，它可以用來衡量傳感器測量值的重要程度。預測置信度越低,，卡爾曼增益就越小,，新的測量值在最終的結果中被采納的比例就越大，反之,，傳感器測量值在最終結果中被采納的比例就越小,。

多維卡爾曼濾波算法

卡爾曼濾波的適用性

針對于線性高斯系統(tǒng)，卡爾曼濾波高效,、簡單,、易行，然而遺憾的是在實際問題中，這樣的理想的場景非常少,。于是又有了擴展卡爾曼濾波,，擴展卡爾曼濾波的思想是非線性系統(tǒng)在均值的一個小的鄰域內(nèi)可以近似認為是線性的，從而可以得到一個近似的高斯置信度,。