卡爾曼濾波家族
之前寫完了
David LEE：卡爾曼濾波：從入門到精通
之後一直說要寫非線性情況下的推導。但後來想想，EKF不過就是做了一階泰勒展開而已，要寫成一篇文章實在是太短，感覺也沒有太多乾貨，就一直沒有動筆。最近在看 《概率機器人》，覺得關於貝葉斯濾波器確實也可以稍作總結，於是就寫了這篇卡爾曼濾波家族。
本文對於擴展卡爾曼濾波、無跡卡爾曼濾波僅僅做了一些簡要介紹，不再想上次的文章那樣做詳細地推導了。但只要看過之前寫的卡爾曼濾波，相信這篇文章對於你來說也是很好理解的。
本文配圖均來自《概率機器人》
擴展卡爾曼濾波
假設狀態轉移概率和測量概率分別由非線性函數g和h控制，而不再是一個線性變換：
這種情況下，由於線性變換的關係不在了，因此概率分布也不再是高斯分布。整個系統不再有閉式解，這是最讓人頭疼的。
而EKF 的主要思想就是線性化：通過一個在高斯函數的均值處與非線性函數g相切的線性函數來近似g。
線性化的主要優點就是效率，一旦對g和h進行了線性化，KEF和KF就是等效的。
EKF採用一階泰勒展開的方式來進行線性化，其根據g的值和斜率構造一個函數g的線性近似函數：
線性點的選擇依據的是在線性化點附近自變數最有可能的狀態。對於高斯函數，最可能的狀態自然就是後驗的均值 ：
同理，將測量函數h線性化，有
最後，整個EKF演算法的流程如下：
- 運動更新：
2. 測量更新
很顯然，EKF 能否成功應用取決於兩個因素：
- 被近似的函數的局部非線性化程度；
- 概率分布自身的不確定度（協方差）。
上兩圖就明確展示了非線性函數在近似點非線性程度越高、概率分布本身越不確定，所得到的近似結果就越差。此時，採用擴展卡爾曼濾波的效果往往很差，甚至會導致發散。
無跡卡爾曼濾波
不同於EKF使用線性化來近似非線性函數，UKF通過無損變換來近似一個高斯分布，它通過使用加權統計線性回歸過程來實現隨機線性化。
下面就介紹些UKF無損變換的思想。
UKF明確地從高斯分布中提取 幾個 點，並將這些
點經過函數g變換。這些
點分別位於均值處、對稱分布於主軸的協方差處。對於具有均值
和協方差
的n維高斯分布，對應的2n+1個
點
根據如下規則進行選擇：
其中， ，
是確定
點分布在離均值多遠的範圍內的比例參數（就是你的採樣範圍）。
每個 有兩個與之相關的權值：
在計算均值時使用；
在計算高斯協方差的時候使用。
選擇參數 對高斯分布的附加的高階分布信息進行編碼。如果概率分布是精確的高斯分布，則
。
然後， 點經過函數g變換，來探測非線性函數g如何改變了高斯分布的形狀：
所得的結果高斯分布的參數 由映射的
點
獲得：
將以上過程分別代入到運動更新和測量更新中就可以得到無跡卡爾曼濾波了。
運動更新
1. 計算 點：
2. 非線性函數映射
3. 進行運動更新
測量更新
1. 依據運動更新的結果計算新的 點
2. 計算每個 點所對應的觀測值的預測
3.計算預測的觀測值的不確定性
4. 計算 點集在狀態空間和測量空間之間的互協方差
5. 進行測量更新
以上就是擴展卡爾曼濾波和無跡卡爾曼濾波的全過程了。只要明白了貝葉斯濾波或者卡爾曼濾波器，這兩個擴展都是信手拈來。
SLAM中，KEF往往比UKF使用得更廣。我個人淺見，主要原因有一下幾點：
1. UKF是一種抽樣近似，無可避免地導致計算量較大； 2. 只有在高度不線性的情況下（或方差很大），UKF才有明顯的優勢； 3. UKF的優勢主要集中在開環的情況下，但SLAM中更常使用迴環檢測這樣的閉環來消除誤差； 4. 如果建圖與定位分開，UKF在建圖過程中的作用可能會大些（開環的情況下）；在定位中，用其來提高運動模型作用不大，觀測模型中準確地和地圖進行匹配對定位精度影響更大。
信息濾波
信息濾波(IF)是卡爾曼濾波的對偶濾波演算法，二者的不同在於對高斯分布的表示方式。信息濾波以正則參數來表示高斯分布，由一個信息矩陣和信息向量組成。
對於一個高斯分布
經過變換後
該式在一些情況下會比矩參數（卡爾曼濾波中均值和協方差分別是一階矩和二階矩）的表示更加簡潔。例如，信息濾波下高斯分布的負對數為
此外，IF表示全局不確定性更加簡單，設置 即可。
對應的信息濾波演算法和擴展信息濾波演算法如下。
信息濾波演算法
對應的運動方程和測量方程為
對應的信息濾波演算法為：
對比信息濾波演算法和卡爾曼濾波，二者在計算複雜度上有很大的不同。
信息濾波中運動更新涉及矩陣求逆，計算量大；但在卡爾曼濾波中運動更新只涉及狀態向量的一部分。而信息濾波中測量更新是增量的，而KF的測量更新涉及矩陣求逆而較為困難。這也展示了二者的特性是對偶的。
擴展信息濾波演算法
推薦閱讀：
