最近在看 《概率機器人》，覺得關(guān)于貝葉斯濾波器確實也可以稍作總結(jié)，于是就寫了這篇卡爾曼濾波家族。

本文對于擴展卡爾曼濾波、無跡卡爾曼濾波僅僅做了一些簡要介紹，不再想上次的文章那樣做詳細地推導了。但只要看過之前寫的卡爾曼濾波，相信這篇文章對于你來說也是很好理解的。

本文配圖均來自《概率機器人》

擴展卡爾曼濾波

假設(shè)狀態(tài)轉(zhuǎn)移概率和測量概率分別由非線性函數(shù)g和h控制，而不再是一個線性變換：

這種情況下，由于線性變換的關(guān)系不在了，因此概率分布也不再是高斯分布。整個系統(tǒng)不再有閉式解，這是最讓人頭疼的。

而EKF的主要思想就是線性化：通過一個在高斯函數(shù)的均值處與非線性函數(shù)g相切的線性函數(shù)來近似g。

線性化的主要優(yōu)點就是效率，一旦對g和h進行了線性化，KEF和KF就是等效的。

EKF采用一階泰勒展開的方式來進行線性化，其根據(jù)g的值和斜率構(gòu)造一個函數(shù)g的線性近似函數(shù)：

同理，將測量函數(shù)h線性化，有

最后，整個EKF算法的流程如下：

1. 運動更新：

2. 測量更新

很顯然，EKF 能否成功應(yīng)用取決于兩個因素：

1. 被近似的函數(shù)的局部非線性化程度；

2. 概率分布自身的不確定度（協(xié)方差）。

上兩圖就明確展示了非線性函數(shù)在近似點非線性程度越高、概率分布本身越不確定，所得到的近似結(jié)果就越差。此時，采用擴展卡爾曼濾波的效果往往很差，甚至會導致發(fā)散。

無跡卡爾曼濾波

不同于EKF使用線性化來近似非線性函數(shù)，UKF通過無損變換來近似一個高斯分布，它通過使用加權(quán)統(tǒng)計線性回歸過程來實現(xiàn)隨機線性化。

下面就介紹些UKF無損變換的思想。

將以上過程分別代入到運動更新和測量更新中就可以得到無跡卡爾曼濾波了。

運動更新

測量更新

以上就是擴展卡爾曼濾波和無跡卡爾曼濾波的全過程了。只要明白了貝葉斯濾波或者卡爾曼濾波器，這兩個擴展都是信手拈來。

SLAM中，KEF往往比UKF使用得更廣。我個人淺見，主要原因有一下幾點：

1. UKF是一種抽樣近似，無可避免地導致計算量較大；

2. 只有在高度不線性的情況下（或方差很大），UKF才有明顯的優(yōu)勢；

3. UKF的優(yōu)勢主要集中在開環(huán)的情況下，但SLAM中更常使用回環(huán)檢測這樣的閉環(huán)來消除誤差；

4. 如果建圖與定位分開，UKF在建圖過程中的作用可能會大些（開環(huán)的情況下）；在定位中，用其來提高運動模型作用不大，觀測模型中準確地和地圖進行匹配對定位精度影響更大。

信息濾波

信息濾波(IF)是卡爾曼濾波的對偶濾波算法，二者的不同在于對高斯分布的表示方式。

信息濾波以正則參數(shù)來表示高斯分布，由一個信息矩陣和信息向量組成。

該式在一些情況下會比矩參數(shù)（卡爾曼濾波中均值和協(xié)方差分別是一階矩和二階矩）的表示更加簡潔。例如，信息濾波下高斯分布的負對數(shù)為

對應(yīng)的信息濾波算法和擴展信息濾波算法如下。

信息濾波算法

對應(yīng)的運動方程和測量方程為

對應(yīng)的信息濾波算法為：

對比信息濾波算法和卡爾曼濾波，二者在計算復(fù)雜度上有很大的不同。

信息濾波中運動更新涉及矩陣求逆，計算量大；但在卡爾曼濾波中運動更新只涉及狀態(tài)向量的一部分。

而信息濾波中測量更新是增量的，而KF的測量更新涉及矩陣求逆而較為困難。這也展示了二者的特性是對偶的。

擴展信息濾波算法