kalman濾波（三）---各種濾波的方法匯總+優化的方法

大神解答

一.前提

       最一般的狀態估計問題，我們會根據系統是否線性，把它們分為線性/非線性系統。同時，對於噪聲，根據它們是否為高斯分布，分為高斯/非高斯噪聲系統。現實中最常見的，也是最困難的問題，是非線性-非高斯(NLNG, Nonlinear-Non Gaussian)的狀態估計。下面先說最簡單的情況：線性高斯系統。

線性高斯系統

在線性高斯系統中，運動方程、觀測方程是線性的，且兩個噪聲項服從零均值的高斯分布。這是最簡單的情況。簡單在哪里呢？主要是因為高斯分布經過線性變換之后仍為高斯分布。而對於一個高斯分布，只要計算出它的一階和二階矩，就可以描述它（高斯分布只有兩個參數）。
　　線性系統形式如下：
　　其中是兩個噪聲項的協方差矩陣；為轉移矩陣和觀測矩陣；表示的后驗概率，用表示它的先驗概率。因為系統是線性的，噪聲是高斯的，所以狀態也服從高斯分布，需要計算它的均值和協方差矩陣。記第時刻的狀態服從：
　　對LG系統，可以用貝葉斯法則，計算的后驗概率分布——這條路直接通向卡爾曼濾波器。卡爾曼是線性系統的遞推形式（recursive，也就是從估計）的無偏最優估計。

    （其中過程可以參考貝葉斯法則-最大似然估計）

     現在的問題是如何求解這個最大化問題。對於高斯分布，最大化問題可以變成最小化它的負對數。當我對一個高斯分布取負對數時，它的指數項變成了一個二次項，而前面的因子則變為一個無關的常數項，可以略掉（這部分我不敲了，有疑問的同學可以問）。於是，定義以下形式的最小化函數：

     寫成矩陣的形式，類似最小二乘的問題：

　　於是令它的導數為零，得到：
 
　　讀者會問，這個問題和卡爾曼濾波有什么問題呢？事實上，卡爾曼濾波就是遞推地求解上式的過程。所謂遞推，就是只用來計算。對(*)進行Cholesky分解，就可以推出卡爾曼濾波器。只把卡爾曼的結論寫一下：

另一方面，能否直接求解(*)式，得到呢？答案是可以的，而且這就是優化方法（batch optimization）：將所有的狀態放在一個向量里，進行求解。與卡爾曼濾波不同的是，在估計前面時刻的狀態（如）時，會用到后面時刻的信息（等）。從這點來說，優化方法和卡爾曼處理信息的方式是相當不同的。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

二.擴展卡爾曼濾波器 --- EKF自己總結（針對非線性非高斯系統）

濾波器自己的局限性:

1. 即使是高斯分布，經過一個非線性變換后也不是高斯分布。EKF只計算均值與協方差，是在用高斯近似這個非線性變換后的結果。（實際中這個近似可能很差）。
2. 系統本身線性化過程中，丟掉了高階項。
3. 線性化的工作點往往不是輸入狀態真實的均值，而是一個估計的均值。於是，在這個工作點下計算的，也不是最好的。
4. 在估計非線性輸出的均值時，EKF算的是的形式。這個結果幾乎不會是輸出分布的真正期望值。協方差也是同理。

那么，怎么克服以上的缺點呢？途徑很多，主要看我們想不想維持EKF的假設。如果我們比較乖，希望維持高斯分布假設，可以這樣子改：

1. 為了克服第3條工作點的問題，我們以EKF估計的結果為工作點，重新計算一遍EKF，直到這個工作點變化夠小，為迭代EKF（Iterated EKF, IEKF）。
2. 為了克服第4條，我們除了計算，再計算其他幾個精心挑選的采樣點，然后用這幾個點估計輸出的高斯分布。是為Sigma Point KF（SPKF，或UKF）。

如果不那么乖，可以說：我們不要高斯分布假設，憑什么要用高斯去近似一個長得根本不高斯的分布呢？於是問題變為，丟掉高斯假設后，怎么描述輸出函數的分布就成了一個問題。一種比較暴力的方式是：用足夠多的采樣點，來表達輸出的分布。這種蒙特卡洛的方式，也就是粒子濾波（PF）的思路。
　　如果再進一步，可以丟棄濾波器思路，說：為什么要用前一個時刻的值來估計下一個時刻呢？我們可以把所有狀態看成變量，把運動方程和觀測方程看成變量間的約束，構造誤差函數，然后最小化這個誤差的二次型。這樣就會得到非線性優化的方法，在SLAM里就走向圖優化那條路上去了。不過，非線性優化也需要對誤差函數不斷地求梯度，並根據梯度方向迭代，因而局部線性化是不可避免的。
　　可以看到，在這個過程中，我們逐漸放寬了假設。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

三.IEKF

四.UKF無跡卡爾曼濾波

五. PF 粒子濾波

六. CKF 容積卡爾曼濾波

七. 非線性優化

非線性優化，計算的也是最大后驗概率估計（MAP），但它的處理方式與濾波器不同。對於上面寫的狀態估計問題，可以簡單地構造誤差項：
　　然后最小化這些誤差項的二次型：
       這里僅用到了噪聲項滿足高斯分布的假設，再沒有更多的了。當構建一個非線性優化問題之后，就可以從一個初始值出發，計算梯度（或二階梯度），優化這個目標函數。常見的梯度下降策略有牛頓法、高斯-牛頓法、Levenberg-Marquardt方法。
　　非線性優化方法現在已經成為視覺SLAM里的主流，尤其是在它的稀疏性質被人發現且利用起來之后。它與濾波器最大不同點在於， 一次可以考慮整條軌跡中的約束。它的線性化，即雅可比矩陣的計算，也是相對於整條軌跡的。相比之下，濾波器還是停留在馬爾可夫的假設之下，只用上一次估計的狀態計算當前的狀態。可以用一個圖來表達它們之間的關系：
　　當然優化方式也存在它的問題。例如優化時間會隨着節點數量增長——所以有人會提double window optimization這樣的方式，以及可能落入局部極小。但是就目前而言，它比EKF還是優不少的。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 總結

1. 卡爾曼濾波是遞歸的線性高斯系統最優估計。
2. EKF將NLNG系統在工作點附近近似為LG進行處理。
3. IEKF對工作點進行迭代。
4. UKF沒有線性化近似，而是把sigma point（采樣點）進行非線性變換后再用高斯近似。
5. PF去掉高斯假設，以粒子作為采樣點來描述分布。
6. 優化方式同時考慮所有幀間約束，迭代線性化求解。

 

PS：

　　SLAM中，狀態變量經常是六自由度的位姿，由旋轉矩陣和平移向量構成。然而問題是，旋轉矩陣並不存在加法，只有對應到李代數上才可以清楚地定義它的運算。因此，當我們討論這個位姿的噪聲，說它服從高斯分布時，需要了解 李群李代數的知識。