?????????? 知乎上的提問，高翔作了回答：能否簡單并且易懂地介紹一下多個基于濾波方法的SLAM算法原理？

?????????? 寫的比較通順，抄之。如有異議，請拜訪原文。如有侵權，請聯系刪除。

我怎么會寫得那么長……如果您有興趣可以和我一塊把公式過一遍。
要講清這個問題，得從狀態估計理論來說。先擺上一句名言：
狀態估計乃傳感器之本質。（To understand the need for state estimation is to understand the nature of sensors.）

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

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

解釋一下變量：
-運動方程
- 輸入
- 輸入噪聲
- 觀測方程
- 觀測數據
- 觀測噪聲

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

同理，觀測方程也隨傳感器的具體信息而變。

例如激光傳感器可以得到空間點離機器人的距離和角度，記為，那么觀測方程為：
，其中是一個2D路標點。

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

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

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

我們希望得到狀態變量的最大后驗估計（MAP，Maximize a Posterior），于是計算：

第二行是貝葉斯法則，第三行分母和無關所以去掉。
第一項即觀測方程，有：，很簡單。
第二項即運動方程，有：，也很簡單。
現在的問題是如何求解這個最大化問題。對于高斯分布，最大化問題可以變成最小化它的負對數。當我對一個高斯分布取負對數時，它的指數項變成了一個二次項，而前面的因子則變為一個無關的常數項，可以略掉（這部分我不敲了，有疑問的同學可以問）。于是，定義以下形式的最小化函數：

那么最大后驗估計就等價于：

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

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

于是令它的導數為零，得到：
(*)

讀者會問，這個問題和卡爾曼濾波有什么問題呢？事實上，卡爾曼濾波就是遞推地求解(*)式的過程。所謂遞推，就是只用來計算。對(*)進行Cholesky分解，就可以推出卡爾曼濾波器。詳細過程限于篇幅就不推了，把卡爾曼的結論寫一下：

前兩個是預測，第三個是卡爾曼增益，四五是校正。

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

==================擴展卡爾曼濾波器===================
線性高斯系統當然性質很好啦，但許多現實世界中的系統都不是線性的，狀態和噪聲也不是高斯分布的。例如上面舉的激光觀測方程就不是線性的。當系統為非線性的時候，會發生什么呢？
一件悲劇的事情是：高斯分布經過非線性變換后，不再是高斯分布。而且，是個什么分布，基本說不上來。（攤手）
如果沒有高斯分布，上面說的那些都不再成立了。于是EKF說，嘛，我們睜一只眼閉一只眼，用高斯分布去近似它，并且，在工作點附近對系統進行線性化。當然這個近似是很成問題的，有什么問題我們之后再說。
EKF的做法主要有兩點。其一，在工作點附近，對系統進行線性近似化：

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

這樣做聽起來還是挺有道理的，實際上也是能用的，但是問題還是很多的。
考慮一個服從高斯分布的變量，現在，問服從什么分布？
我概率比較差，不過這個似乎是叫做卡爾方布。應該是下圖中k=1那條線。

但是按照EKF的觀點，我們要用一個高斯分布去近似。假設我們采樣時得到了一個，那么就會近似成一個均值為0.25的高斯分布，然而卡方分布的期望應該是1。……但是各位真覺得k=1那條線像哪個高斯分布嗎？

所以EKF面臨的一個重要問題是，當一個高斯分布經過非線性變換后，如何用另一個高斯分布近似它？按照它現在的做法，存在以下的局限性：（注意是濾波器自己的局限性，還沒談在SLAM問題里的局限性）。

即使是高斯分布，經過一個非線性變換后也不是高斯分布。EKF只計算均值與協方差，是在用高斯近似這個非線性變換后的結果。（實際中這個近似可能很差）。

系統本身線性化過程中，丟掉了高階項。

線性化的工作點往往不是輸入狀態真實的均值，而是一個估計的均值。于是，在這個工作點下計算的，也不是最好的。

在估計非線性輸出的均值時，EKF算的是的形式。這個結果幾乎不會是輸出分布的真正期望值。協方差也是同理。

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

為了克服第3條工作點的問題，我們以EKF估計的結果為工作點，重新計算一遍EKF，直到這個工作點變化夠小。是為迭代EKF（Iterated EKF, IEKF）。

為了克服第4條，我們除了計算，再計算其他幾個精心挑選的采樣點，然后用這幾個點估計輸出的高斯分布。是為Sigma Point KF（SPKF，或UKF）。

如果不那么乖，可以說：我們不要高斯分布假設，憑什么要用高斯去近似一個長得根本不高斯的分布呢？于是問題變為，丟掉高斯假設后，怎么描述輸出函數的分布就成了一個問題。一種比較暴力的方式是：用足夠多的采樣點，來表達輸出的分布。這種蒙特卡洛的方式，也就是粒子濾波的思路。

如果再進一步，可以丟棄濾波器思路，說：為什么要用前一個時刻的值來估計下一個時刻呢？我們可以把所有狀態看成變量，把運動方程和觀測方程看成變量間的約束，構造誤差函數，然后最小化這個誤差的二次型。這樣就會得到非線性優化的方法，在SLAM里就走向圖優化那條路上去了。不過，非線性優化也需要對誤差函數不斷地求梯度，并根據梯度方向迭代，因而局部線性化是不可避免的。

可以看到，在這個過程中，我們逐漸放寬了假設。

============== UKF 無跡卡爾曼 （投影方法）==================
由于題主問題里沒談IEKF，我們就簡單說說UKF和PF。
UKF主要解決一個高斯分布經過非線性變換后，怎么用另一個高斯分布近似它。假設，我們希望用近似。按照EKF，需要對做線性化。但在UKF里，不必做這個線性化。
UKF的做法是找一些叫做Sigma Point的點，把這些點用投影過去。然后，用投影之后的點做出一個高斯分布，如下圖：
這里選了三個點：。對于維數為N的分布，需要選2N+1個點。篇幅所限，這里就不解釋這些點怎么選，以及為何要這樣選了。總之UKF的好處就是：

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

=============== PF 粒子濾波 （蒙特卡洛方法）==================
UKF的一個問題是輸出仍假設成高斯分布。然而，即使在很簡單的情況下，高斯的非線性變換仍然不是高斯。并且，僅在很少的情況下，輸出的分布有個名字（比如卡方），多數時候你都不知道他們是啥……更別提描述它們了。
因為描述很困難，所以粒子濾波器采用了一種暴力的，用大量采樣點去描述這個分布的方法（老子就是無參的你來打我呀）。框架大概像下面這個樣子，就是一個不斷采樣——算權重——重采樣的過程：

越符合觀測的粒子擁有越大的權重，而權重越大就越容易在重采樣時被采到。當然，每次采樣數量、權重的計算策略，則是粒子濾波器里幾個比較麻煩的問題，這里就不細講了。
這種采樣思路的最大問題是：采樣所需的粒子數量，隨分布是指數增長的。所以僅限于低維的問題，高維的基本就沒辦法了。

=============== 非線性優化 ==================
非線性優化，計算的也是最大后驗概率估計（MAP），但它的處理方式與濾波器不同。對于上面寫的狀態估計問題，可以簡單地構造誤差項：

然后最小化這些誤差項的二次型：

這里僅用到了噪聲項滿足高斯分布的假設，再沒有更多的了。當構建一個非線性優化問題之后，就可以從一個初始值出發，計算梯度（或二階梯度），優化這個目標函數。常見的梯度下降策略有牛頓法、高斯-牛頓法、Levenberg-Marquardt方法，可以在許多講數值優化的書里找到。

非線性優化方法現在已經成為視覺SLAM里的主流，尤其是在它的稀疏性質被人發現且利用起來之后。它與濾波器最大不同點在于， 一次可以考慮整條軌跡中的約束。它的線性化，即雅可比矩陣的計算，也是相對于整條軌跡的。相比之下，濾波器還是停留在馬爾可夫的假設之下，只用上一次估計的狀態計算當前的狀態。可以用一個圖來表達它們之間的關系：

當然優化方式也存在它的問題。例如優化時間會隨著節點數量增長——所以有人會提double window optimization這樣的方式，以及可能落入局部極小。但是就目前而言，它比EKF還是優不少的。

=============== 小結 ==================

卡爾曼濾波是遞歸的線性高斯系統最優估計。

EKF將NLNG系統在工作點附近近似為LG進行處理。

IEKF對工作點進行迭代。

UKF沒有線性化近似，而是把sigma point進行非線性變換后再用高斯近似。

PF去掉高斯假設，以粒子作為采樣點來描述分布。

優化方式同時考慮所有幀間約束，迭代線性化求解。

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

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

總結

以上是生活随笔為你收集整理的三维重建面试0：*SLAM滤波方法的串联综述的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。