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

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

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

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

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

解釋一下變量：
-運(yùn)動方程
- 輸入
- 輸入噪聲
- 觀測方程
- 觀測數(shù)據(jù)
- 觀測噪聲

運(yùn)動方程描述了狀態(tài)是怎么變到的，而觀測方程描述的是從是怎么得到觀察數(shù)據(jù)的。
請注意這是一種抽象的寫法。當(dāng)你有實(shí)際的機(jī)器人，實(shí)際的傳感器時，方程的形式就會變得具體，也就是所謂的參數(shù)化。例如，當(dāng)我們關(guān)心機(jī)器人空間位置時，可以取。進(jìn)而，機(jī)器人攜帶了里程計，能夠得到兩個時間間隔中的相對運(yùn)動，像這樣，那么運(yùn)動方程就變?yōu)?#xff1a;

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

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

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

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

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

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

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

那么最大后驗(yàn)估計就等價于：

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

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

于是令它的導(dǎo)數(shù)為零，得到：
(*)

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

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

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

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

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

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

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

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

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

系統(tǒng)本身線性化過程中，丟掉了高階項(xiàng)。

線性化的工作點(diǎn)往往不是輸入狀態(tài)真實(shí)的均值，而是一個估計的均值。于是，在這個工作點(diǎn)下計算的，也不是最好的。

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

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

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

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

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

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

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

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

• 不必線性化，也不必求導(dǎo)，對沒有光滑性要求。
• 計算量隨維數(shù)增長是線性的。

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

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

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

然后最小化這些誤差項(xiàng)的二次型：

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

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

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

=============== 小結(jié) ==================

卡爾曼濾波是遞歸的線性高斯系統(tǒng)最優(yōu)估計。

EKF將NLNG系統(tǒng)在工作點(diǎn)附近近似為LG進(jìn)行處理。

IEKF對工作點(diǎn)進(jìn)行迭代。

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

PF去掉高斯假設(shè)，以粒子作為采樣點(diǎn)來描述分布。

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

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

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。