SLAM中的EKF，UKF，PF原理簡介

這是我在知乎上問題寫的答案，修改了一下排版，轉到博客裏。

 

原問題：

可否簡單而且易懂地介紹一下多個基於濾波方法的SLAM算法原理？

目前SLAM後端都開始用優化的方法來作，題主想要了解一下以前基於濾波的方法，但願有大神可以總結一下各個原理（EKF，UKF，PF，FastSLAM），感激涕零。

 

做者：半閒居士
連接：https://www.zhihu.com/question/46916554/answer/103411007
來源：知乎
著做權歸做者全部。商業轉載請聯繫做者得到受權，非商業轉載請註明出處。

　　我怎麼會寫得那麼長……若是您有興趣能夠和我一塊把公式過一遍。
　　要講清這個問題，得從狀態估計理論來講。先擺上一句名言：

狀態估計乃傳感器之本質。（To understand the need for state estimation is to understand the nature of sensors.）

　　任何傳感器，激光也好，視覺也好，整個SLAM系統也好，要解決的問題只有一個： 如何經過數據來估計自身狀態。每種傳感器的測量模型不同，它們的精度也不同。換句話說，狀態估計問題，也就是「 如何最好地使用傳感器數據」。能夠說，SLAM是狀態估計的一個特例。

 

1. 離散時間系統的狀態估計

　　記機器人在各時刻的狀態爲 ，其中 是離散時間下標。在SLAM中，咱們一般要估計機器人的位置，那麼系統的狀態就指的是機器人的位姿。用兩個方程來描述狀態估計問題：

　　解釋一下變量：

　　 -運動方程
　　 - 輸入
　　 - 輸入噪聲
　　 - 觀測方程
　　 - 觀測數據
　　 - 觀測噪聲

　　運動方程描述了狀態 是怎麼變到 的，而觀測方程描述的是從 是怎麼獲得觀察數據 的。
　　請注意這是一種抽象的寫法。當你有實際的機器人，實際的傳感器時，方程的形式就會變得具體，也就是所謂的 參數化。例如，當咱們關心機器人空間位置時，能夠取 。進而，機器人攜帶了里程計，可以獲得兩個時間間隔中的相對運動，像這樣 ，那麼運動方程就變爲：

　　同理，觀測方程也隨傳感器的具體信息而變。例如激光傳感器能夠獲得空間點離機器人的距離和角度，記爲 ，那麼觀測方程爲：
　　其中 是一個2D路標點。

　　舉這幾個例子是爲了說明， 運動方程和觀測方程具體形式是會變化的。可是，咱們想討論更通常的問題：當我不限制傳感器的具體形式時，可否設計一種方式，從已知的 （輸入和觀測數據）從，估計出 呢？

　　這就是最通常的狀態估計問題。咱們會根據 是否線性，把它們分爲 線性/非線性系統。同時，對於噪聲 ，根據它們是否爲高斯分佈，分爲 高斯/非高斯噪聲系統。最通常的，也是最困難的問題，是非線性-非高斯(NLNG, Nonlinear-Non Gaussian)的狀態估計。下面先說最簡單的狀況：線性高斯系統。

2. 線性高斯系統(LG，Linear Gaussian)

　　在線性高斯系統中，運動方程、觀測方程是線性的，且兩個噪聲項服從零均值的高斯分佈。這是最簡單的狀況。簡單在哪裏呢？主要是由於 高斯分佈通過線性變換以後仍爲高斯分佈。而對於一個高斯分佈，只要計算出它的一階和二階矩，就能夠描述它（高斯分佈只有兩個參數 ）。
　　線性系統形式以下：
　　其中 是兩個噪聲項的協方差矩陣。 爲轉移矩陣和觀測矩陣。
　　對LG系統，能夠用貝葉斯法則，計算 的後驗機率分佈——這條路直接通向 卡爾曼濾波器。卡爾曼是線性系統的遞推形式（recursive，也就是從 估計 ）的無偏最優估計。因爲解釋EKF和UKF都得用它，因此咱們來推一推。若是讀者不感興趣，能夠跳過公式推導環節。
　　符號：用 表示 的後驗機率，用 表示它的先驗機率。由於系統是線性的，噪聲是高斯的，因此狀態也服從高斯分佈，須要計算它的均值和協方差矩陣。記第 時刻的狀態服從：

　　咱們但願獲得狀態變量 的最大後驗估計（MAP，Maximize a Posterior），因而計算：

　　第二行是貝葉斯法則，第三行分母和 無關因此去掉。
　　第一項即觀測方程，有：

　　很簡單。
　　第二項即運動方程

　　也很簡單。
　　如今的問題是如何求解這個最大化問題。對於高斯分佈，最大化問題能夠變成最小化它的負對數。當我對一個高斯分佈取負對數時，它的指數項變成了一個二次項，而前面的因子則變爲一個無關的常數項，能夠略掉（這部分我不敲了，有疑問的同窗能夠問）。因而，定義如下形式的最小化函數：

　　那麼最大後驗估計就等價於：

　　這個問題如今是二次項和的形式，寫成矩陣形式會更加清晰。定義：


　　就獲得矩陣形式的，相似最小二乘的問題：

　　因而令它的導數爲零，獲得：
(*)
　　讀者會問，這個問題和卡爾曼濾波有什麼問題呢？事實上， 卡爾曼濾波就是遞推地求解(*)式的過程。所謂遞推，就是隻用 來計算 。對(*)進行Cholesky分解，就能夠推出卡爾曼濾波器。詳細過程限於篇幅就不推了，把卡爾曼的結論寫一下：

　　前兩個是預測，第三個是卡爾曼增益，四五是校訂。
　　另外一方面，可否直接求解(*)式，獲得 呢？答案是能夠的，並且這就是優化方法（batch optimization）的思路：將全部的狀態放在一個向量裏，進行求解。與卡爾曼濾波不一樣的是，在估計前面時刻的狀態（如 ）時，會用到後面時刻的信息（ 等）。從這點來講，優化方法和卡爾曼處理信息的方式是至關不一樣的。

3. 擴展卡爾曼濾波器

　　線性高斯系統固然性質很好啦，但許多現實世界中的系統都不是線性的，狀態和噪聲也不是高斯分佈的。例如上面舉的激光觀測方程就不是線性的。當系統爲非線性的時候，會發生什麼呢？
　　一件悲劇的事情是： 高斯分佈通過非線性變換後，再也不是高斯分佈。並且，是個什麼分佈，基本說不上來。（攤手）
　　若是沒有高斯分佈，上面說的那些都再也不成立了。 因而EKF說，嘛，咱們睜一隻眼閉一隻眼，用高斯分佈去近似它，而且，在工做點附近對系統進行線性化。固然這個近似是很成問題的，有什麼問題咱們以後再說。
　　EKF的作法主要有兩點。其一，在工做點附近 ，對系統進行線性近似化：

　　這裏的幾個偏導數，都在工做點處取值。因而呢，它就被 活生生地當成了一個線性系統。
　　第二，在線性系統近似下，把噪聲項和狀態都 當成了高斯分佈。這樣，只要估計它們的均值和協方差矩陣，就能夠描述狀態了。通過這樣的近似以後呢，後續工做都和卡爾曼濾波是同樣的了。因此EKF是卡爾曼濾波在NLNG系統下的直接擴展（因此叫擴展卡爾曼嘛）。EKF給出的公式和卡爾曼是一致的，用線性化以後的矩陣去代替卡爾曼濾波器裏的轉移矩陣和觀測矩陣便可。
　　 其中

　　這樣作聽起來仍是挺有道理的，實際上也是能用的，可是問題仍是不少的。
　　考慮一個服從高斯分佈的變量 ，如今 ，問 服從什麼分佈？
　　我機率比較差，不過這個彷佛是叫作卡爾方布。 應該是下圖中k=1那條線。

　　可是按照EKF的觀點，咱們要用一個高斯分佈去近似 。假設咱們採樣時獲得了一個 ，那麼就會近似成一個均值爲0.25的高斯分佈，然而卡方分佈的指望應該是1。……可是各位真以爲k=1那條線像哪一個高斯分佈嗎？
　　因此EKF面臨的一個重要問題是，當一個高斯分佈通過非線性變換後，如何用另外一個高斯分佈近似它？按照它如今的作法，存在如下的侷限性：（注意是濾波器本身的侷限性，還沒談在SLAM問題裏的侷限性）。

1. 即便是高斯分佈，通過一個非線性變換後也不是高斯分佈。EKF只計算均值與協方差，是在用高斯近似這個非線性變換後的結果。（實際中這個近似可能不好）。
2. 系統自己線性化過程當中，丟掉了高階項。
3. 線性化的工做點每每不是輸入狀態真實的均值，而是一個估計的均值。因而，在這個工做點下計算的，也不是最好的。
4. 在估計非線性輸出的均值時，EKF算的是的形式。這個結果幾乎不會是輸出分佈的真正指望值。協方差也是同理。

那麼，怎麼克服以上的缺點呢？途徑不少，主要看咱們想不想維持EKF的假設。若是咱們比較乖，但願維持高斯分佈假設，能夠這樣子改：

1. 爲了克服第3條工做點的問題，咱們以EKF估計的結果爲工做點，從新計算一遍EKF，直到這個工做點變化夠小。是爲迭代EKF（Iterated EKF, IEKF）。
2. 爲了克服第4條，咱們除了計算，再計算其餘幾個精心挑選的採樣點，而後用這幾個點估計輸出的高斯分佈。是爲Sigma Point KF（SPKF，或UKF）。

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

4. UKF 無跡卡爾曼

　　因爲題主問題裏沒談IEKF，咱們就簡單說說UKF和PF。
　　UKF主要解決一個高斯分佈通過非線性變換後，怎麼用另外一個高斯分佈近似它。假設 ，咱們但願用 近似 。按照EKF，須要對 作線性化。但在UKF裏，沒必要作這個線性化。
　　UKF的作法是找一些叫作Sigma Point的點，把這些點用 投影過去。而後，用投影以後的點作出一個高斯分佈，以下圖：
　　這裏選了三個點： 。對於維數爲N的分佈，須要選2N+1個點。篇幅所限，這裏就不解釋這些點怎麼選，以及爲什麼要這樣選了。總之UKF的好處就是：

• 沒必要線性化，也沒必要求導，對沒有光滑性要求。
• 計算量隨維數增加是線性的。

5. PF 粒子濾波 

　　UKF的一個問題是輸出仍假設成高斯分佈。然而，即便在很簡單的狀況下，高斯的非線性變換仍然不是高斯。而且，僅在不多的狀況下，輸出的分佈有個名字（好比卡方），多數時候你都不知道他們是啥……更別提描述它們了。
　　由於描述很困難，因此粒子濾波器採用了一種暴力的，用大量採樣點去描述這個分佈的方法（老子就是無參的你來打我呀）。框架大概像下面這個樣子，就是一個不斷採樣——算權重——重採樣的過程：
　　越符合觀測的粒子擁有越大的權重，而權重越大就越容易在重採樣時被採到。固然，每次採樣數量、權重的計算策略，則是粒子濾波器裏幾個比較麻煩的問題，這裏就不細講了。
　　這種採樣思路的最大問題是： 採樣所需的粒子數量，隨分佈是指數增加的。因此僅限於低維的問題，高維的基本就沒辦法了。

6. 非線性優化

　　非線性優化，計算的也是最大後驗機率估計（MAP），但它的處理方式與濾波器不一樣。對於上面寫的狀態估計問題，能夠簡單地構造偏差項：
　　而後最小化這些偏差項的二次型：

　　這裏僅用到了噪聲項知足高斯分佈的假設，再沒有更多的了。當構建一個非線性優化問題以後，就能夠從一個初始值出發，計算梯度（或二階梯度），優化這個目標函數。常見的梯度降低策略有牛頓法、高斯-牛頓法、Levenberg-Marquardt方法，能夠在許多講數值優化的書裏找到。
　　非線性優化方法如今已經成爲視覺SLAM裏的主流，尤爲是在它的稀疏性質被人發現且利用起來以後。它與濾波器最大不一樣點在於， 一次能夠考慮整條軌跡中的約束。它的線性化，即雅可比矩陣的計算，也是相對於整條軌跡的。相比之下，濾波器仍是停留在馬爾可夫的假設之下，只用上一次估計的狀態計算當前的狀態。能夠用一個圖來表達它們之間的關係：
　　固然優化方式也存在它的問題。例如優化時間會隨着節點數量增加——因此有人會提double window optimization這樣的方式，以及可能落入局部極小。可是就目前而言，它比EKF仍是優很多的。

7. 小結 

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

　　呃好像題主還問了FastSLAM，有空再寫吧……

注：
* 本文大量觀點來自Timothy. Barfoot, "State estimation for Robotics: A Matrix Lei Group Approach", 2016. 圖片如有侵權望告知。

 

PS：

　　SLAM中，狀態變量常常是六自由度的位姿，由旋轉矩陣和平移向量構成。然而問題是，旋轉矩陣並不存在加法，只有對應到李代數上才能夠清楚地定義它的運算。所以，當咱們討論這個位姿的噪聲，說它服從高斯分佈時， 咱們究竟在說什麼，是一個很嚴重的問題。從此的博客將更深刻地介紹李羣李代數的知識。