從這一講開始我們要開始學習SLAM的後端，前面我們學習了視覺裡程計（VO），通過視覺裡程計，我們可以建構一個短時間内效果還不錯的軌迹和地圖，但是長時間的誤差的累計，還是會迅速的降低我們的軌迹和地圖的精度，是以維護一個更大的地圖和軌迹，使其達到最優狀态，在實際中還是非常需要的。而這項任務的完成就離不開——後端。
概述
狀态估計和機率解釋
我們再來看一下SLAM過程的運動方程和觀測方程：
{ x k = f ( x k − 1 , u k ) + w k （ 運 動 方 程 ） z k = h ( x k ) + v k ， k = 1 , … , N （ 觀 測 方 程 ） (0) \left\{ \begin{aligned} &x_k = f(x_{k-1},u_k)+w_k （運動方程）\\ &z_{k}=h(x_k)+v_{k} ，k=1,\dots,N（觀測方程） \end{aligned} \right. \tag 0 {xk=f(xk−1,uk)+wk （運動方程）zk=h(xk)+vk，k=1,…,N（觀測方程）(0)這兩個方程我們已經多次見過了，依然有必要再仔細看看它：
系統狀态： x x x
輸入： u u u（也可以了解為控制量，這裡和書中的說法不一樣！！）
過程噪聲： w w w
測量： z z z
測量噪聲： v v v
我們不妨通過描述的方法去認識一下（0）式：
首先看運動方程： k k k時刻的系統狀态 x k x_k xk，是由 k − 1 k-1 k−1時刻的狀态 x k − 1 x_{k-1} xk−1， k k k時刻的控制量 u k u_k uk，再加上過程噪聲共同決定的。
測量方程： z k z_{k} zk時刻的測量值，是由在 x k x_k xk狀态下看到的觀測，在加上測量噪聲決定的。
這個式子是一個很抽象的式子，它表示了一大類運動過程，如果不根據實際的情況分析，可能你不太能明白這其中的具體過程。
現在我們想獲得 k k k時刻的系統狀态分布，我們希望這個分布是從過去0到 k k k中的所有資料估計而來的：
P ( x k ∣ x 0 , u 1 : k , z 1 : k ) P(x_k |x_0, u_{1:k},z_{1:k}) P(xk∣x0,u1:k,z1:k)注意從0到 k k k時刻，我們能得到的資料就是0時刻的系統狀态 x 0 x_0 x0、控制量 u u u，測量值 z z z。
根據貝葉斯公式：
P ( x k ∣ x 0 , u 1 : k , z 1 : k ) ∝ P ( z k ∣ x k ) P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) (1) P(x_k |x_0, u_{1:k},z_{1:k}) \propto P(z_k|x_k)P(x_k|x_0,u_{1:k},z_{1:k-1}) \tag 1 P(xk∣x0,u1:k,z1:k)∝P(zk∣xk)P(xk∣x0,u1:k,z1:k−1)(1)對于上式不用過多解釋了，非常簡單的貝葉斯公式。
P ( z k ∣ x k ) P(z_k|x_k) P(zk∣xk)是似然，也就是 x k x_k xk條件下， z k z_k zk的機率分布，對應到SAMM問題中，也就是某一個狀态下的測量值，這個可以通過測量方程給出。
P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) P(x_k|x_0,u_{1:k},z_{1:k-1}) P(xk∣x0,u1:k,z1:k−1)是先驗，它的求法和我們選取什麼樣的狀态有關，可以選擇上一個時刻的狀态，也可以選取以前所有的狀态。如果按照 x k − 1 x_{k-1} xk−1時刻的狀态為條件：
P ( x k ∣ x 0 , u 1 : k , z 1 : k − 1 ) = ∫ P ( x k ∣ x k − 1 , x 0 , u 1 : k , z 1 : k − 1 ) P ( x k − 1 ∣ x 0 , u 1 : k , z 1 : k − 1 ) d x k − 1 (2) P(x_k|x_0,u_{1:k},z_{1:k-1}) = \int P(x_k|x_{k-1},x_0,u_{1:k},z_{1:k-1}) P(x_{k-1}|x_0,u_{1:k},z_{1:k-1}) dx_{k-1} \tag 2 P(xk∣x0,u1:k,z1:k−1)=∫P(xk∣xk−1,x0,u1:k,z1:k−1)P(xk−1∣x0,u1:k,z1:k−1)dxk−1(2)上式子（2）的操作方法很多，選擇不同的操作，就對應着不同的理論，如果認為隻和上一時刻有關，那麼就會得到以卡爾曼濾波器為代表的濾波器方法。如果認為和所有狀态都有關，那麼得到的就是非線性優化的方法。
線性系統和KF
首先我們看一下認為隻和上一個時刻有關的馬兒可夫理論，也就是基于濾波器模型。
在這裡由于推導過程比較複雜（如果你覺得推導看着費勁，那就先記住結論吧），是以我就直接總結出線性卡爾曼濾波器的公式：
①預測：
x ‾ k = A k x ^ k − 1 + u k , P ‾ k = A k P ^ k − 1 A k T + R (3) \overline x_k = A_k \hat x_{k-1} + u_k, \overline P_k=A_k \hat P_{k-1}A_k^{T}+R \tag 3 xk=Akx^k−1+uk, Pk=AkP^k−1AkT+R(3) ②更新：先計算卡爾曼增益 K K K
K k = P ‾ k C k T ( C k P ‾ k C k T + Q k ) − 1 (4) K_k=\overline P_kC_k^{T}(C_k \overline P_kC_k^{T}+Q_k)^{-1} \tag 4 Kk=PkCkT(CkPkCkT+Qk)−1(4) 然後計算後驗機率分布
x ^ k = x ‾ k + K k ( z k − C k x ‾ k ) (5) \hat x_k = \overline x_k + K_k(z_k-C_k \overline x_k) \tag 5 x^k=xk+Kk(zk−Ckxk)(5) P ^ k = ( I − K k C k ) P ‾ k \hat P_k = (I-K_kC_k) \overline P_k P^k=(I−KkCk)Pk（３）（４）（５）就是偉大的卡爾曼濾波器公式，你不要小瞧它，它的用處遠比你想象的還要大，你可以不會推導，但是一定要學會怎麼使用，這一點非常重要，因為它的用處實在太大了！！讓我們看一下每一個變量的意義吧：
狀态轉移矩陣： A A A
過程噪聲： u u u
過程噪聲的協方差矩陣： Ｑ Ｑ Ｑ
觀測矩陣： C C C
觀測噪聲協方差： R R R
卡爾曼濾波器的更直白的描述是：現在我們有一個目前時刻的測量值，和目前狀态的預測值，我們覺得兩個值都不是最準确的，但是我們不能丢棄任何一個，而是通過對這兩個值取一個折中值（不是平均值，不然也太low了），将這個折中值就作為目前狀态的最優估計。（４）式的卡爾曼增益，就是告訴我們了，應該相信測量值更多一點兒，還是應該相信預測值更多一點兒。
關于卡爾曼濾波器更細緻的了解和使用方法，我推薦大家看以下部落格
《詳解卡爾曼濾波原理》
《卡爾曼濾波應用及其matlab實作》
非線性系統和EKF
上面講的是線性的系統，對于（０）式這樣的非線性的系統，我們可以将其通過泰勒展開保留一階項，也就可以将非線性系統線性化，然後同樣的思路推導出擴充卡爾曼方程：
①預測：
x ‾ k = f ( x ^ k − 1 , u k ) , P ‾ k = F P ^ k − 1 F T + R k (6) \overline x_k = f(\hat x_{k-1},u_k), \overline P_k=F\hat P_{k-1}F^{T} +R_k \tag 6 xk=f(x^k−1,uk), Pk=FP^k−1FT+Rk(6) ②更新：先計算卡爾曼增益 K K K：
K k = P ‾ k H T ( H P ‾ k H T + Q k ) − 1 (7) K_k=\overline P_kH^{T}(H \overline P_kH^{T}+Q_k)^{-1} \tag 7 Kk=PkHT(HPkHT+Qk)−1(7) 然後計算後驗機率分布
x ^ k = x ‾ k + K k ( z k − h ( x ‾ k ) ) (8) \hat x_k = \overline x_k + K_k(z_k-h(\overline x_k)) \tag 8 x^k=xk+Kk(zk−h(xk))(8) P ^ k = ( I − K k H ) P ‾ k \hat P_k = (I-K_kH) \overline P_k P^k=(I−KkH)Pk
其中 F F F是 f ( x ) f(x) f(x)關于 x x x的偏導數， H H H是 h ( x ) h(x) h(x)關于 x x x的偏導數。
EKF的讨論
KF也好，EKF也好，總之基于卡爾曼濾波器發展起來的各種濾波器種類繁多，在不同的應用方向上發揮着巨大的作用。不管現今的SLAM是否還在使用濾波器，但是對于計算資源受限的硬體來說，選擇卡爾曼濾波器是不錯的選擇。
随着計算機硬體的提升，主流的SLAM系統基本都已經摒棄了基于濾波的方法。主要原因是因為以下方面：
- 1.濾波器的方法是基于前後兩個時刻的狀态進行計算的，實際上我們知道，我們目前的狀态不是任何一個狀态導緻的，而是很多狀态累積出來的結果，如果隻認為與前一狀态有關，勢必會丢掉很多有用的資訊。
- 2.EKF在非線性強烈的情況下，線性近似就不在成立，導緻誤差會很大。
- 3.EKF需要存儲的資訊量很多，不能适應大型場景。
BA和圖優化
既然基于濾波器的方法不能适應現在的需求，那麼我們就能來看看更好的方法。
關于BA我在第7講的視覺裡程計中給了一個還算直覺的解釋，在這裡我就不在贅述了。
投影模型和BA代價函數
1.首先将一個世界坐标下的空間點 p p p轉換到相機坐标下：
P ′ = R p + t = [ X ′ , Y ′ , Z ′ ] T P'=Rp+t=[X',Y',Z']^{T} P′=Rp+t=[X′,Y′,Z′]T2.然後 P ′ P' P′投影到歸一化平面，得到歸一化坐标：
P c = [ u c , v c , 1 ] T = [ X ′ Z ′ , Y ′ Z ′ , 1 ] T P_c=[u_c,v_c,1]^{T}=[\frac{X'}{Z'},\frac{Y'}{Z'},1]^{T} Pc=[uc,vc,1]T=[Z′X′,Z′Y′,1]T
3.考慮歸一化坐标的畸變情況，得到去畸變（隻考慮徑向畸變）後的歸一化坐标：
{ u c ′ = u c ( 1 + k 1 r c 2 + k 2 r c 4 ) v c ′ = v c ( 1 + k 1 r c 2 + k 2 r c 4 ) \left\{ \begin{aligned} u_c'=u_c(1+k_1r_c^{2}+k_2r_c^4) \\ v_c'=v_c(1+k_1r_c^2+k_2r_c^4) \end{aligned} \right. {uc′=uc(1+k1rc2+k2rc4)vc′=vc(1+k1rc2+k2rc4)
4.最後根據相機内參模型，将去畸變後歸一化平面的點投影到像素平面：
{ u s = f x u c ′ + c x v s = f y v c ′ + c y \left\{ \begin{aligned} u_s=f_xu_c'+c_x \\ v_s=f_yv_c'+c_y \end{aligned} \right. {us=fxuc′+cxvs=fyvc′+cy
上面四步也就是前面講的觀測方程，在前面我們的觀測方程被抽象成了：
z = h ( x , y ) z=h(x,y) z=h(x,y)那麼 x x x就相當于是這裡的相機外參數 R , t R,t R,t，對應的李代數為 ξ \xi ξ， y y y就相當于是這裡的三維點 p p p， z z z就是這裡的觀測資料 [ u s , v s ] [u_s,v_s] [us,vs]。那麼觀測誤差就是：
e = z − h ( ξ , p ) (9) e = z-h(\xi,p) \tag 9 e=z−h(ξ,p)(9)很多人不明白這個式子，在這裡我解釋一下：空間中的點已經投影到了像素平面上，這個已經是一個事實了，誰也改變不了了。現在我們有一個相機姿态 ξ \xi ξ和空間點位置的不準确值，按說如果我們的 ξ , p \xi,p ξ,p完全準确的話（9）式就會嚴格成立， e = 0 e=0 e=0。但是實際上算出來的 ξ , p \xi,p ξ,p并不準，是以我們按照這兩個不準确的值進行投影的時候得到的值，和我實際測量的像素值就會有差異，上面的（9）式沒有展現出這樣的邏輯關系，如果你還是不懂的話，請你停下來再仔細整理一下思路！！
我們将所有的誤差疊加到一起，具體說就是所有位姿下觀測到的所有路标點，然後寫成最小二乘的形式：
1 2 ∑ i = 1 m ∑ j = 1 n ∥ e i j ∥ 2 = 1 2 ∑ i = 1 m ∑ j = 1 n ∥ z i j − h ( ξ i , p j ) ∥ 2 (10) \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\| e_{ij} \|^2 = \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\| z_{ij}-h(\xi_i,p_j) \|^2 \tag{10} 21i=1∑mj=1∑n∥eij∥2=21i=1∑mj=1∑n∥zij−h(ξi,pj)∥2(10)上式的 z i j z_{ij} zij表示的是在 ξ i \xi_i ξi處觀察到的路标點 p j p_j pj.
BA優化最終做的就是優化上面這個式子的位姿和路标點，使得整體誤差最小。我不知道你是否清楚這個過程，我還是願意多費幾句口舌跟你重複一下這個過程：對于優化上式獲得誤差和最小時的位姿和路标點，其實就是在不斷地尋找每一個位姿、每一個路标點的值，讓它不斷地接近誤差和最小。
至于怎麼去尋找，下面我們就講講BA的求解
BA的求解
（10）中待優化的變量是位姿和路标，我們将它們寫在一起，組成一個整體的待優化變量：
x = [ ξ 1 , ξ 2 , ⋯ , ξ m , p 1 , p 2 , ⋯ , p n ] T x=[\xi_1,\xi_2,\cdots,\xi_m,p_1,p_2,\cdots,p_n]^{T} x=[ξ1,ξ2,⋯,ξm,p1,p2,⋯,pn]T相應的，（10）式可以簡化為下式：
1 2 ∥ f ( x ) ∥ 2 = 1 2 ∑ i = 1 m ∑ j = 1 n ∥ z i j − h ( ξ i , p j ) ∥ 2 (11) \frac{1}{2} \| f(x)\|^2 = \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\| z_{ij}-h(\xi_i,p_j) \|^2 \tag{11} 21∥f(x)∥2=21i=1∑mj=1∑n∥zij−h(ξi,pj)∥2(11)那麼對（11）進行泰勒展開，保留一階項：
1 2 ∥ f ( x ) ∥ 2 ≈ 1 2 ∑ i = 1 m ∑ j = 1 n ∥ e i j + F i j Δ ξ i + E i j Δ p j ∥ 2 (12) \frac{1}{2} \| f(x)\|^2 \approx \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\| e_{ij}+F_{ij}\Delta\xi_i+E_{ij}\Delta p_j\|^2 \tag{12} 21∥f(x)∥2≈21i=1∑mj=1∑n∥eij+FijΔξi+EijΔpj∥2(12)其中 F i j , E i j F_{ij},E_{ij} Fij,Eij分别是代價函數對相機姿态和路标點的偏導數.
将（12）式右邊寫成向量的形式：
1 2 ∥ f ( x ) ∥ 2 ≈ 1 2 ∥ e + F Δ x c + E Δ x p ∥ 2 (12) \frac{1}{2} \| f(x)\|^2 \approx \frac{1}{2}\| e+F\Delta x_c+E\Delta x_p\|^2 \tag{12} 21∥f(x)∥2≈21∥e+FΔxc+EΔxp∥2(12)實際上，上面的步驟不知道你注意到沒有，我們已經由整體的代價函數，變成了在代價函數某一點處進行一階近似了，希望你别迷糊了。
對于（12）式的優化，不論是GN還是LM方法，它們的雅克比矩陣可以寫成如下形式：
J = [ F E ] J=[F E] J=[F E]海森矩陣可以寫成： H = J T J = [ F T F F T E E T F E T E ] (13) H=J^TJ= \left[ \begin{matrix} F^TF&F^TE \\ E^TF&E^TE \end{matrix} \right] \tag {13} H=JTJ=[FTFETFFTEETE](13)
如果你還記得非線性優化那一講的内容，那麼你應該知道對于非線性優化增量的線性方程：
H Δ x = g H \Delta x = g HΔx=g我們要想解出 Δ x \Delta x Δx，就得對 H H H矩陣求逆，實際上是一個非常大的矩陣，因為包含了數百個位姿，幾十上百萬的路标點，是以對于它的求逆能不能實時就決定了，SLAM整個系統能不能實時運作，這一塊之前的研究者研究了很多，才找到了 H H H的特别之處。
H H H矩陣的特别之處在于，它是稀疏的！！這一點很重要！我們來看一個特别大型的實際例子下 H H H矩陣的結構，如下圖：
上圖就是 H H H矩陣的結構，小黑塊就代表是有資料的，别的地方都是０，也就是說B和C是對角矩陣塊，E是一個普通的矩陣，我們對對角矩陣求逆是很容易的，是以按照分塊求逆可以很容易解出 Δ x \Delta x Δx。
我并沒有仔細去講述怎麼得出 H H H的矩陣形式，這部分内容書中介紹的很詳細，是以我就隻是告訴你結論。
求出了 Δ x \Delta x Δx之後，對于代價函數的優化就很容易了。實際上我們并不需要自己去設計整個算法，g2o我們已經用過好幾次了，我們隻要按照要求構造優化變量，程式就會為我們自動去優化和計算。
魯棒核函數
我們想一想如果某一個測量值誤差特别大，那麼它的梯度也會很大，那麼優化的時候，整體梯度就會被它給拉偏了，這個時候可能就會導緻别的邊得不到合适的值，是以就必須對邊加入魯棒核函數，抑制某些邊過大，我們常常采用Huber核：
H ( e ) { 1 2 e 2 當 ∣ e ∣ < δ δ ( ∣ e ∣ − 1 2 δ ) 其 他 H(e)\left\{ \begin{aligned} &\frac{1}{2}e^2 當|e|<\delta\\ &\delta(|e|-\frac12\delta) 其他 \end{aligned} \right. H(e)⎩⎪⎨⎪⎧21e2 當∣e∣<δδ(∣e∣−21δ) 其他
非線性優化是非常重要的，光靠看書我覺得永遠都不能真正了解它，如果你有時間可以挑戰一下手寫一個優化器，完成優化任務，可能你沒有g2o或者ceres寫得好，但是寫完一遍你就真正的了解了非線性優化的真谛了！
實踐
實踐部分的兩個不同庫編寫的代碼，都不能直接運作在比較新的庫上，是以需要做一些修改，這部分大家自己改吧，留給大家一個挑戰（主要是我不想寫了，哈哈）
如果自己都在偷懶，命運又怎麼會認可你。别再虛度光陰，叫醒那個沉睡的自己。記住，隻要開始，就永遠不晚。
![XGBoost Plotting API以及GBDT組合特征實踐 XGBoost Plotting API以及GBDT組合特征實踐[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)