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

12-28

目前SLAM後端都開始用優化的方法來做，題主想要了解一下之前基於濾波的方法，希望有大神能夠總結一下各個原理（EKF，UKF，PF，FastSLAM），感激不盡。

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

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

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

=====================離散時間系統的狀態估計======================
記機器人在各時刻的狀態為 $x_1,ldots,x_k$ ，其中 $k$ 是離散時間下標。在SLAM中，我們通常要估計機器人的位置，那麼系統的狀態就指的是機器人的位姿。用兩個方程來描述狀態估計問題：
$[left{ egin{array}{l} {x_k} = fleft( {{x_{k - 1}},{u_k},{w_k}} ight)\ {y_k} = gleft( {{x_k},{n_k}} ight) end{array} ight.]$

解釋一下變數：
$f$ -運動方程
$u$ - 輸入
$w$ - 輸入雜訊
$g$ - 觀測方程
$y$ - 觀測數據
$n$ - 觀測雜訊

運動方程描述了狀態 $x_{k-1}$ 是怎麼變到 $x_k$ 的，而觀測方程描述的是從 $x_k$ 是怎麼得到觀察數據 $y_k$ 的。
請注意這是一種抽象的寫法。當你有實際的機器人，實際的感測器時，方程的形式就會變得具體，也就是所謂的參數化。例如，當我們關心機器人空間位置時，可以取 $x_k = [x,y,z]_k$ 。進而，機器人攜帶了里程計，能夠得到兩個時間間隔中的相對運動，像這樣 $Delta x_k=[Delta x, Delta y, Delta z]_k$ ，那麼運動方程就變為：
$x_{k+1}=x_k+Delta x_k+w_k$
同理，觀測方程也隨感測器的具體信息而變。例如激光感測器可以得到空間點離機器人的距離和角度，記為 $y_k=[r, heta]_k$ ，那麼觀測方程為：
$[{left[ egin{array}{l} r\ heta end{array} ight]_k} = left[ egin{array}{l} sqrt {{{left| {{x_k} - {L_k}} ight|}_2}} \ { an ^{ - 1}}frac{{{L_{k,y}} - {x_{k,y}}}}{{{L_{k,x}} - {x_{k,x}}}} end{array} ight] + {n_k}]$ ，其中 $L_k=[L_{k,x},L_{k,y}]$ 是一個2D路標點。

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

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

=====================線性高斯系統============================
線性高斯系統(LG，Linear Gaussian)
在線性高斯系統中，運動方程、觀測方程是線性的，且兩個雜訊項服從零均值的高斯分布。這是最簡單的情況。簡單在哪裡呢？主要是因為高斯分布經過線性變換之後仍為高斯分布。而對於一個高斯分布，只要計算出它的一階和二階矩，就可以描述它（高斯分布只有兩個參數 $mu, Sigma$ ）。
線性系統形式如下：
$left{ egin{array}{l} {x_k} = {A_{k - 1}}{x_{k - 1}} + {u_k} + {w_k}\ {y_k} = {C_k}{x_k} + {n_k}\ {w_k}sim Nleft( {0,{Q_k}} ight)\ {n_k}sim N(0,{R_k}) end{array} ight.$ 其中 $Q_k,R_k$ 是兩個雜訊項的協方差矩陣。 $A,C$ 為轉移矩陣和觀測矩陣。
對LG系統，可以用貝葉斯法則，計算 $x$ 的後驗概率分布——這條路直接通向卡爾曼濾波器。卡爾曼是線性系統的遞推形式（recursive，也就是從 $x_{k-1}$ 估計 $x_k$ ）的無偏最優估計。由於解釋EKF和UKF都得用它，所以我們來推一推。如果讀者不感興趣，可以跳過公式推導環節。
符號：用 $hat{x}$ 表示 $x$ 的後驗概率，用 $[ ilde x]$ 表示它的先驗概率。因為系統是線性的，雜訊是高斯的，所以狀態也服從高斯分布，需要計算它的均值和協方差矩陣。記第 $k$ 時刻的狀態服從： $x_ksim N({{ar x}_k},{P_k})$

我們希望得到狀態變數 $x$ 的最大後驗估計（MAP，Maximize a Posterior），於是計算：
$[egin{array}{ccl} hat x = arg mathop {max }limits_x pleft( {x|y,u} ight)\ = arg max frac{{pleft( {y|x,u} ight)pleft( {x|u} ight)}}{{pleft( {y|v} ight)}} \ = arg max p(y|x)pleft( {x|u} ight) end{array}]$
第二行是貝葉斯法則，第三行分母和 $x$ 無關所以去掉。
第一項即觀測方程，有： $[pleft( {y|x} ight) = prodlimits_{k = 0}^K {pleft( {{y_k}|{x_k}} ight)} ]$ ，很簡單。
第二項即運動方程，有： $[pleft( {x|v} ight) = prodlimits_{k = 0}^K {pleft( {{x_k}|{x_{k - 1}},v_k} ight)} ]$ ，也很簡單。
現在的問題是如何求解這個最大化問題。對於高斯分布，最大化問題可以變成最小化它的負對數。當我對一個高斯分布取負對數時，它的指數項變成了一個二次項，而前面的因子則變為一個無關的常數項，可以略掉（這部分我不敲了，有疑問的同學可以問）。於是，定義以下形式的最小化函數：

$[egin{array}{l} {J_{y,k}}left( x ight) = frac{1}{2}{left( {{y_k} - {C_k}{x_k}} ight)^T}R_k^{ - 1}left( {{y_k} - {C_k}{x_k}} ight)\ {J_{v,k}}left( x ight) = frac{1}{2}{left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} ight)^T}Q_k^{ - 1}left( {{x_k} - {A_{k - 1}}{x_{k - 1}} - {v_k}} ight) end{array}]$
那麼最大後驗估計就等價於：
$[hat x = arg min sumlimits_{k = 0}^K {{J_{y,k}} + {J_{v,k}}} ]$
這個問題現在是二次項和的形式，寫成矩陣形式會更加清晰。定義：
$[egin{array}{l} z = left[ egin{array}{l} {x_0}\ {v_1}\ vdots \ {v_K}\ {y_0}\ vdots \ {y_K} end{array} ight],x = left[ egin{array}{l} {x_0}\ vdots \ {x_K} end{array} ight]\ H = left[ {egin{array}{*{20}{c}} 1{}{}{}\ { - {A_0}}1{}{}\ {} ddots ddots {}\ {}{}{ - {A_{K - 1}}}1\ {{C_0}}{}{}{}\ {} ddots {}{}\ {}{} ddots {}\ {}{}{}{{C_K}} end{array}} ight],W = left[ {egin{array}{*{20}{c}} {{P_0}}{}{}{}{}{}{}\ {}{{Q_1}}{}{}{}{}{}\ {}{} ddots {}{}{}{}\ {}{}{}{{Q_K}}{}{}{}\ {}{}{}{}{{R_1}}{}{}\ {}{}{}{}{} ddots {}\ {}{}{}{}{}{}{{R_K}} end{array}} ight] end{array}]$

就得到矩陣形式的，類似最小二乘的問題：
$[Jleft( x ight) = frac{1}{2}{left( {z - Hx} ight)^T}{W^{ - 1}}left( {z - Hx} ight)]$
於是令它的導數為零，得到：
$[frac{{partial J}}{{partial {x^T}}} = - {H^T}{W^{ - 1}}left( {z - Hx} ight) = 0 Rightarrow left( {{H^T}{W^{ - 1}}H} ight)x = {H^T}{W^{ - 1}}z]$ (*)

讀者會問，這個問題和卡爾曼濾波有什麼問題呢？事實上，卡爾曼濾波就是遞推地求解(*)式的過程。所謂遞推，就是只用 $x_{k-1}$ 來計算 $x_k$ 。對(*)進行Cholesky分解，就可以推出卡爾曼濾波器。詳細過程限於篇幅就不推了，把卡爾曼的結論寫一下：
$[egin{array}{l} {{ ilde P}_k} = {A_{k - 1}}{{hat P}_{k - 1}}A_{k - 1}^T + {Q_k}\ {{ ilde x}_k} = {A_{k - 1}}{{hat x}_{k - 1}} + {v_k}\ {K_k} = {{ ilde P}_k}C_k^T{left( {{C_k}{{ ilde P}_k}C_k^T + {R_k}} ight)^{ - 1}}\ {{hat P}_k} = left( {I - {K_k}{C_k}} ight){{ ilde P}_k}\ {{hat x}_k} = {{ ilde x}_k} + {K_k}left( {{y_k} - {C_k}{{ ilde x}_k}} ight) end{array}]$
前兩個是預測，第三個是卡爾曼增益，四五是校正。

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

==================擴展卡爾曼濾波器===================
線性高斯系統當然性質很好啦，但許多現實世界中的系統都不是線性的，狀態和雜訊也不是高斯分布的。例如上面舉的激光觀測方程就不是線性的。當系統為非線性的時候，會發生什麼呢？
一件悲劇的事情是：高斯分布經過非線性變換後，不再是高斯分布。而且，是個什麼分布，基本說不上來。（攤手）
如果沒有高斯分布，上面說的那些都不再成立了。於是EKF說，嘛，我們睜一隻眼閉一隻眼，用高斯分布去近似它，並且，在工作點附近對系統進行線性化。當然這個近似是很成問題的，有什麼問題我們之後再說。
EKF的做法主要有兩點。其一，在工作點附近 $[{{hat x}_{k - 1}},{{ ilde x}_k}]$ ，對系統進行線性近似化：
$[egin{array}{l} fleft( {{x_{k - 1}},{v_k},{w_k}} ight) approx fleft( {{{hat x}_{k - 1}},{v_k},0} ight) + frac{{partial f}}{{partial {x_{k - 1}}}}left( {{x_{k - 1}} - {{hat x}_{k - 1}}} ight) + frac{{partial f}}{{partial {w_k}}}{w_k}\ gleft( {{x_k},{n_k}} ight) approx gleft( {{{ ilde x}_k},0} ight) + frac{{partial g}}{{partial {x_k}}}{n_k} end{array}]$
這裡的幾個偏導數，都在工作點處取值。於是呢，它就被活生生地當成了一個線性系統。
第二，在線性系統近似下，把雜訊項和狀態都當成了高斯分布。這樣，只要估計它們的均值和協方差矩陣，就可以描述狀態了。經過這樣的近似之後呢，後續工作都和卡爾曼濾波是一樣的了。所以EKF是卡爾曼濾波在NLNG系統下的直接擴展（所以叫擴展卡爾曼嘛）。EKF給出的公式和卡爾曼是一致的，用線性化之後的矩陣去代替卡爾曼濾波器里的轉移矩陣和觀測矩陣即可。
$[egin{array}{l} {{ ilde P}_k} = {F_{k - 1}}{{hat P}_{k - 1}}F_{k - 1}^T + Q_k$ 其中 $[F_{k-1} = {left. {frac{{partial f}}{{partial {x_{k - 1}}}}} ight|_{{{hat x}_{k - 1}}}},{G_k} = {left. {frac{{partial g}}{{partial {x_k}}}} ight|_{{{ ilde x}_k}}}]$

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

但是按照EKF的觀點，我們要用一個高斯分布去近似 $y$ 。假設我們採樣時得到了一個 $x=0.5$ ，那麼就會近似成一個均值為0.25的高斯分布，然而卡方分布的期望應該是1。……但是各位真覺得k=1那條線像哪個高斯分布嗎？所以EKF面臨的一個重要問題是，當一個高斯分布經過非線性變換後，如何用另一個高斯分布近似它？按照它現在的做法，存在以下的局限性：（注意是濾波器自己的局限性，還沒談在SLAM問題里的局限性）。

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

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

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

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

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

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

============== UKF 無跡卡爾曼 ==================
由於題主問題里沒談IEKF，我們就簡單說說UKF和PF。
UKF主要解決一個高斯分布經過非線性變換後，怎麼用另一個高斯分布近似它。假設 $x sim N(mu_x Sigma_{xx} ), y=g(x)$ ，我們希望用 $N(mu_y, Sigma_{yy})$ 近似 $y$ 。按照EKF，需要對 $g$ 做線性化。但在UKF里，不必做這個線性化。
UKF的做法是找一些叫做Sigma Point的點，把這些點用 $g$ 投影過去。然後，用投影之後的點做出一個高斯分布，如下圖：

這裡選了三個點： $mu_x, mu_x+sigma_x, mu_x-sigma_x$ 。對於維數為N的分布，需要選2N+1個點。篇幅所限，這裡就不解釋這些點怎麼選，以及為何要這樣選了。總之UKF的好處就是：

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

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

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

=============== 非線性優化 ==================
非線性優化，計算的也是最大後驗概率估計（MAP），但它的處理方式與濾波器不同。對於上面寫的狀態估計問題，可以簡單地構造誤差項：
$[egin{array}{l} {e_{v,k}}left( x ight) = {x_k} - fleft( {{x_{k - 1}},{v_k},0} ight)\ {e_{y,k}}left( x ight) = {y_k} - gleft( {{x_k},0} ight) end{array}]$
然後最小化這些誤差項的二次型：
$[min Jleft( x ight) = sumlimits_{k = 1}^K {left( {frac{1}{2}{e_{v,k}}{{left( x ight)}^T}W_{v,k}^{ - 1}{e_{v,k}}left( x ight)} ight) + sumlimits_{k = 1}^K {left( {frac{1}{2}{e_{y,k}}{{left( x ight)}^T}W_{v,k}^{ - 1}{e_{v,k}}left( x ight)} ight)} } ]$
這裡僅用到了雜訊項滿足高斯分布的假設，再沒有更多的了。當構建一個非線性優化問題之後，就可以從一個初始值出發，計算梯度（或二階梯度），優化這個目標函數。常見的梯度下降策略有牛頓法、高斯-牛頓法、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. 圖片若有侵權望告知。

高博說了好多, 好多.....
然而State Estimation這本書的確十分給力, 我再補上一點我的想法.
---------------------------------
filtering, smoothing, 圖優化. 這些其實都是一樣的東西, 本質是我要利用多少信息, 優化多少變數.
譬如: 機器人從 $x_{0}, x_{1},... x_{k}$ 這個過程中, 有時候我只關心最新的那個狀態, 那就用fitlering. 而傳統的KF, EKF這些大多是假設了 $x_{t}$ 只與 $x_{t-1}$ 有關的, 即那個馬爾科夫假設. 但這個假設並不一定要是一階的, 意思是, 我們可以利用之前多個時間點的信息來推斷 $x_{t}$ , 而並不只是 $x_{t-1}.$ 這裡就是我說的, 我們需要決定我們要用多少信息.
然而有時候我們並不只是關注當前的估計狀態, 我們關注的是整條軌跡. 這時候不只是 $x_{t}$ 需要被調整, $x_{t-1}...x_{t-k}$ 都要被調整, 而這個被調整的量又可以由我們自己來控制. 這就是之前說的要決定優化多少變數. 有反向修正的這一個過程, 通常叫做smoothing.
在State Estimation這本書3.2.3節, 推導了Rauch-Tung-Striebel Smoother的Forward部分與Kalman是等價的.我就厚顏無恥地直接貼發圖好了, 有興趣的可以翻書之.

特別喜歡高博答案里最後那部分的圖, 三種方式的信息處理環狀示意圖, sliding window filter即是當前最火的選擇, 很多工作雖然沒有explicit地提到它, 卻也是用了它的思想, 比如ORB-SLAM里的covisibility graph局部優化.
於是, 得出結論, 這些啥啥啥的最優估計方法, 都是一樣的! 這個世界就是那麼和諧統一!!!

---------------------------------
就先說那麼多吧, 只是一點個人思考的補充. 詳細的請看高博博客, 高博答案, 高博QQ(我就是高博NC粉~~~~), 還有vision機器人新聖經: state estimation for robotics!!!!

Reference:
Barfoot, Timothy D. "STATE ESTIMATION FOR ROBOTICS." (2016).

語言都是蒼白的，直接上PR大法。
首先EKF，建立在非線性模型的基礎上，狀態轉移和測量方程為：

EKF: 在dynamic model是非線性的情況下，首先用 $g$ 去計算狀態的先驗分布 $ar{mu}_t$ ，用觀測值Z去糾正一下先驗分布 $ar{mu}_t$ ，原有的狀態值分布就是 $mu_{t-1}$ , $Sigma _{t-1}$ . $G_t$ 實際上是 $g$ 在 $mu _{t-1}$ 處的一階微分，同理 $h$ 和 $H_t$ 也是這樣。

EKF SLAM: 可以理解為機器人位置姿態以及地圖上的點，都作為狀態 $x$ 的一部分，當通過dynamic model得到先驗分布 $ar{mu}_t$ 之後(2-3行)，同樣可以根據先驗分布 $ar{mu}_t$ 中機器人的位置和前一狀態中各個地圖點的位置(比如第i個)，得到第i個點測量結果的先驗分布 $hat{z}_t^i$ (12-14行)。通過每個地圖點測量結果與其先驗分布的差，依次來糾正機器人的位置和對應地圖點的誤差(19-20行，實際上19,20行應該在內循環里，每個點都要更新)。當然還要考慮新生成點的情況(10行)。

至於基於其他濾波，其實也是大體類似，原有濾波器架構解決特定SLAM問題。
具體推倒過程詳見
Thrun, Sebastian. "Probabilistic robotics." Communications of the ACM 45.3 (2002): 52-57.
(坑爹的是這書網上電子版跟紙質版相差好大，頁數少的電子版都是沒校對過的估計，錯誤好多，比如上圖就是...)

基於特徵點的單目屎萊姆現在主要還是用圖優化工具，但是濾波方法在單目屎萊姆中也有用到。比如直接法的svo和lsd，都是利用濾波方法優化深度

高博說得好透徹，也說得很清楚了。
想推薦一下本周四晚的講座
SLAM及在機器人領域中的應用
https://zhuanlan.zhihu.com/p/23659397