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

本文對(duì)于擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波僅僅做了一些簡(jiǎn)要介紹，不再想上次的文章那樣做詳細(xì)地推導(dǎo)了。但只要看過(guò)之前寫(xiě)的卡爾曼濾波，相信這篇文章對(duì)于你來(lái)說(shuō)也是很好理解的。

本文配圖均來(lái)自《概率機(jī)器人》

擴(kuò)展卡爾曼濾波

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

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

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

線(xiàn)性化的主要優(yōu)點(diǎn)就是效率，一旦對(duì)g和h進(jìn)行了線(xiàn)性化，KEF和KF就是等效的。

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

同理，將測(cè)量函數(shù)h線(xiàn)性化，有

最后，整個(gè)EKF算法的流程如下：

1. 運(yùn)動(dòng)更新：

2. 測(cè)量更新

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

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

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

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

無(wú)跡卡爾曼濾波

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

下面就介紹些UKF無(wú)損變換的思想。

將以上過(guò)程分別代入到運(yùn)動(dòng)更新和測(cè)量更新中就可以得到無(wú)跡卡爾曼濾波了。

運(yùn)動(dòng)更新

測(cè)量更新

以上就是擴(kuò)展卡爾曼濾波和無(wú)跡卡爾曼濾波的全過(guò)程了。只要明白了貝葉斯濾波或者卡爾曼濾波器，這兩個(gè)擴(kuò)展都是信手拈來(lái)。

SLAM中，KEF往往比UKF使用得更廣。我個(gè)人淺見(jiàn)，主要原因有一下幾點(diǎn)：

1. UKF是一種抽樣近似，無(wú)可避免地導(dǎo)致計(jì)算量較大；

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

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

4. 如果建圖與定位分開(kāi)，UKF在建圖過(guò)程中的作用可能會(huì)大些（開(kāi)環(huán)的情況下）；在定位中，用其來(lái)提高運(yùn)動(dòng)模型作用不大，觀測(cè)模型中準(zhǔn)確地和地圖進(jìn)行匹配對(duì)定位精度影響更大。

信息濾波

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

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

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

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

信息濾波算法

對(duì)應(yīng)的運(yùn)動(dòng)方程和測(cè)量方程為

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

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

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

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

擴(kuò)展信息濾波算法