MATLAB實現一個EKF-2D-SLAM（已開源）
1. SLAM問題定義
同時定位與建圖（SLAM）的本質是一個估計問題，它要求移動機械人利用傳感器信息實時地對外界環境結構進行估計，並且估算出自己在這個環境中的位置，Smith 和Cheeseman在上個世紀首次將EKF估計方法應用到SLAM。
以濾波為主的SLAM模型主要包括三個方程：
1）運動方程：它會增加機械人定位的不確定性
2）根據觀測對路標初始化的方程：它根據觀測信息，對新的狀態量初始化。
3）路標狀態對觀測的投影方程：根據觀測信息，對狀態更新，糾正，減小不確定度。
2. EKF-SLAM維護的數據地圖
系統狀態x是一個很大的向量，它包括機械人的狀態和路標點的狀態，
[ mathbf{x}=left[begin{array}{c} {mathcal{R}} \ {mathcal{M}} end{array}right]=left[begin{array}{c} {mathcal{R}} \ {mathcal{L}_{1}} \ {vdots} \ {mathcal{L}_{n}} end{array}right] ]
其中({mathcal{R}})是機械人狀態，({mathcal{M}} = left({mathcal{L}_{1}}， dots，{mathcal{L}_{n}}right))是n個當前已經觀測過的路標點狀態集合。
在EKF中，x被認為服從高斯分佈，所以，EKF-SLAM的地圖被建模為x的均值(overline{x})與協方差(mathbf{P}),
[ overline{mathbf{x}}=left[frac{overline{mathcal{R}}}{mathcal{M}}right]=left[begin{array}{c} {overline{mathcal{R}}} \ {overline{mathcal{L}}_{1}} \ {vdots} \ {overline{mathcal{L}}_{n}} end{array}right] quad mathbf{P}=left[begin{array}{cc} {mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{M}}} \ {mathbf{P}_{mathcal{M R}}} & {mathbf{P}_{mathcal{M M}}} end{array}right]=left[begin{array}{cccc} {mathbf{P}_{mathcal{R R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{R} mathcal{L}_{n}}} \ {mathbf{P}_{mathcal{L}_{1} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{1} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{L}_{n}} mathcal{L}_{n}} \ {vdots} & {vdots} & {ddots} & {vdots} \ {mathbf{P}_{mathcal{L}_{n} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{n} mathcal{L}_{1}}} & {cdots} & {mathbf{P}_{mathcal{L}_{n} mathcal{L}_{n}}} end{array}right] ]
因此EKF-SLAM的目標就是根據運動模型和觀測模型及時更新地圖量(left{overline{mathbf{x}},mathbf{P}right})
3. EKF-SLAM算法實施
3.1 地圖初始化
顯而易見，在機械人開始之前是沒有任何路標點的信息的，因此此時地圖中只有機械人自己的狀態信息，因此({n} = 0,{x} = {mathcal{R}})。SLAM中經常把機械人初始位姿認為是地圖的原點，其初始協方差可以按實際情況設定，比如：
[ overline{mathbf{x}}=left[begin{array}{l} {x} \ {y} \ {theta} end{array}right]=left[begin{array}{l} {0} \ {0} \ {0} end{array}right] quad mathbf{P}=left[begin{array}{lll} {0} & {0} & {0} \ {0} & {0} & {0} \ {0} & {0} & {0} end{array}right] ]
3.2 運動模型
在EKF中如果x是狀態量，u是控制輸入，n是噪聲變量，那麼我們可以得到一般的狀態更新函數：
[mathbf{x} leftarrow f(mathbf{x}, mathbf{u}, mathbf{n}) ]
EKF的預測步驟為：
[begin{array}{l} {overline{mathbf{x}} leftarrow f(overline{mathbf{x}}, mathbf{u}, 0)} \ {mathbf{P} leftarrow mathbf{F}_{mathbf{x}} mathbf{P} mathbf{F}_{mathbf{x}}^{top}+mathbf{F}_{mathbf{n}} mathbf{N} mathbf{F}_{mathbf{n}}^{top}} end{array} ]
其中雅克比矩陣(mathbf{F}_{mathbf{x}}=frac{partial f(overline{mathbf{x}}, mathbf{u})}{partial mathbf{x}})，(mathbf{F}_{mathbf{n}}=frac{partial f(overline{mathbf{x}}, mathbf{u})}{partial mathbf{n}})，({mathbf{N}})是隨機變量n的協方差。
但是在EKF-SLAM中，只有一部分狀態({mathcal{R}})是隨運動而改變的，其餘所有路標狀態不改變，所以SLAM的運動方程為：
[begin{array}{l} {mathcal{R} leftarrow f_{mathcal{R}}(mathcal{R}, mathbf{u}, mathbf{n})} \ {mathcal{M} leftarrow mathcal{M}} end{array} ]
因此我們可以得到稀疏的雅克比矩陣：
[ mathbf{F}_{mathbf{x}}=left[begin{array}{cc} {frac{partial f_{mathcal{R}}}{partial mathcal{R}}} & {0} \ {0} & {mathbf{I}} end{array}right] quad mathbf{F}_{mathbf{n}}=left[begin{array}{c} {frac{partial f_{mathcal{R}}}{partial mathbf{n}}} \ {0} end{array}right] ]
最終我們得到了用於運動模型的EKF稀疏預測公式
[overline{mathcal{R}} leftarrow f_{mathcal{R}}(overline{mathcal{R}}, mathbf{u}, 0) ]
[ mathbf{P}_{mathcal{R} mathcal{R}} leftarrow frac{partial f_{mathcal{R}}}{partial mathcal{R}} mathbf{P}_{mathcal{R} mathcal{R}} frac{partial f_{mathcal{R}}^{T}}{partial mathcal{R}}+frac{partial f_{mathcal{R}}}{partial mathbf{n}} mathbf{N} frac{partial f_{mathcal{R}}^{T}}{partial mathbf{n}} ]
[mathbf{P}_{mathcal{R} mathcal{M}} leftarrow frac{partial f_{mathcal{R}}}{partial mathcal{R}} mathbf{P}_{mathcal{R} mathcal{M}} ]
[ mathbf{P}_{mathcal{M} mathcal{R}} leftarrow mathbf{P}_{mathcal{R} mathcal{M}}^{top} ]
3.3 已經加入地圖的狀態量觀測更新
在EKF中，我們有以下一般的觀測方程
[ mathbf{y}=h(mathbf{x})+mathbf{v} ]
其中(mathbf{y})是測量噪聲，(mathbf{x})是全狀態，(h())是觀測函數，(v)是測量噪聲。
典型的EKF觀測更新為：
[ overline{mathbf{z}}=mathbf{y}-h(overline{mathbf{x}}) ]
[mathbf{Z}=mathbf{H}_{mathbf{x}} mathbf{P} mathbf{H}_{mathbf{x}}^{top}+mathbf{R} ]
[ mathbf{K}=mathbf{P} mathbf{H}_{mathbf{x}}^{top} mathbf{Z}^{-1} ]
[overline{mathbf{x}} leftarrow overline{mathbf{x}}+mathbf{K} overline{mathbf{z}} ]
[ mathbf{P} leftarrow mathbf{P}-mathbf{K} mathbf{Z} mathbf{K}^{top} ]
雅克比矩陣(mathbf{H}_{mathbf{x}}=frac{partial h(overline{mathbf{x}})}{partial mathbf{x}})，(mathbf{R})是測量噪聲的協方差矩陣。
在SLAM中，觀測指的是機械人上的傳感器觀測到某些路標點，並對路標點進行參數化的輸出。每次可能對路標點有多個觀測值，這裡每使用一個觀測值，就進行一次狀態更新。
觀測的結果依賴於機械人的狀態(mathcal{R}),傳感器的狀態(mathcal{S})和路標點的狀態(mathcal{L}_{i})，並且這裡假設，傳感器的狀態與機械人的狀態差了一個固定的坐標變化，其實也就是外參固定。當觀測到路標點(i)時，可以得到如下關係：
[ mathbf{y}_{i}=h_{i}left(mathcal{R}, mathcal{S}, mathcal{L}_{i}right)+mathbf{v} ]
這就是觀測方程，它不依賴於除了(mathcal{L}_{i})外的任何路標點狀態。因此EKF-SLAM的雅克比(mathbf{H}_{mathbf{x}})也是稀疏的：
[ mathbf{H}_{mathbf{x}}=left[begin{array}{lllllllll} {mathbf{H}_{mathcal{R}}} & {0} & {cdots} & {0} & {mathbf{H}_{mathcal{L}_{i}}} & {0} & {cdots} & {0} end{array}right] ]
其中(mathbf{H}_{mathcal{R}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}right)}{partial mathcal{R}})，(mathbf{H}_{mathcal{L}_{i}}=frac{partial h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}right)}{partial mathcal{L}_{i}})。由於這裡的稀疏性，EKF-SLAM的觀測更新變成：
[overline{mathbf{z}}=mathbf{y}_{i}-h_{i}left(overline{mathcal{R}}, mathcal{S}, overline{mathcal{L}}_{i}right) ]
[ mathbf{Z}=left[begin{matrix}mathbf{H}_{mathcal{R}} &mathbf{H}_{mathcal{L}_{i}}end{matrix}right]left[begin{array}{cc} {mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{i}}} \ {mathbf{P}_{mathcal{L}_{i} mathcal{R}}} & {mathbf{P}_{mathcal{L}_{i} mathcal{L}_{i}}} end{array}right]left[begin{array}{c} {mathbf{H}_{mathcal{R}}^{top}} \ {mathbf{H}_{mathcal{L}_{i}}^{top}} end{array}right]+mathbf{R} ]
[mathbf{K}=left[begin{array}{ll} {mathbf{P}_{mathcal{R} mathcal{R}}} & {mathbf{P}_{mathcal{R} mathcal{L}_{i}}} \ {mathbf{P}_{mathcal{M} mathcal{R}}} & {mathbf{P}_{mathcal{M} mathcal{L}_{i}}} end{array}right]left[begin{array}{l} {mathbf{H}_{mathcal{R}}^{top}} \ {mathbf{H}_{mathcal{L}_{i}}^{top}} end{array}right] mathbf{Z}^{-1} ]
[ overline{mathbf{x}} leftarrow overline{mathbf{x}}+mathbf{K} overline{mathbf{z}} ]
[mathbf{P} leftarrow mathbf{P}-mathbf{K} mathbf{Z} mathbf{K}^{top} ]
3.4 觀測方程可逆時的狀態增廣
這裡的狀態增廣指的是新發現的路標點的初始化。當機械人發現了曾經未觀測到的路標點時，會利用觀測方程將新的路標狀態加入地圖，這一步操作會增大總狀態向量的大小。可以看到EKF-SLAM中的濾波器大小動態變化的。
當傳感器信息可以提供新發現路標點的所有自由度，也就是觀測方程是雙射時，只需要根據觀測方程(h())的逆運算，即可以得到機械人狀態(mathcal{R})，傳感器狀態(mathcal{S})，觀測量(mathbf{y}_{n+1})，觀測噪聲(v)，它們與新路標點狀態的關係：
[ mathcal{L}_{n+1}=gleft(mathcal{R}, mathcal{S}, mathbf{y}_{n+1},vright) ]
上式是單個路標點的逆觀測模型。
路標點的均值和雅克比：
[ overline{mathcal{L}}_{n+1}=gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0right) ]
[mathbf{G}_{mathcal{R}}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0right)}{partial mathcal{R}} ]
[ mathbf{G}_{mathbf v}=frac{partial gleft(overline{mathcal{R}}, mathcal{S}, mathbf{y}_{n+1},0right)}{partial mathbf{v}} ]
顯然，新加路標點狀態的協方差(mathbf{P}_{mathcal{L} mathcal{L}})，以及該狀態與地圖其它狀態的互協方差為：
[ mathbf{P}_{mathcal{L} mathcal{L}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathcal{R}} mathbf{G}_{mathcal{R}}^{top}+mathbf{G}_{mathbf v} mathbf{R} mathbf{G}_{mathbf v}^{top} ]
[mathbf{P}_{mathcal{L} mathbf{x}}=mathbf{G}_{mathcal{R}} mathbf{P}_{mathcal{R} mathbf{x}}=mathbf{G}_{mathcal{R}}left[begin{matrix}mathbf{P}_{mathcal{R} mathcal{R}} &mathbf{P}_{mathcal{R} mathcal{M}}end{matrix}right] ]
然後將這些結果加入到地圖中，可以得到總狀態的均值與協方差矩陣：
[ overline{mathbf{x}} leftarrow left[begin{array}{c} {overline{mathbf{x}}} \ {overline{mathcal{L}}_{n+1}} end{array}right] ]
[ mathbf{P} leftarrowleft[begin{array}{cc} {mathbf{P}} & {mathbf{P}_{mathcal{L} mathbf{x}}^{top}} \ {mathbf{P}_{mathcal{L}mathbf{x}}} & {mathbf{P}_{mathcal{L}mathcal{L}}} end{array}right] ]
4. 仿真實驗
4.1 模型設置
4.1.1 傳感器模型
傳感器是一個360度的雷達，可以探測發現周圍一定範圍內的路標點及其類型，其探測半徑為r，其觀測值為((xi)，s)。(xi)為在當前雷達坐標系中路標點與x軸的的夾角，s為路標點與當前雷達坐標系原點的距離。
4.1.2 運動模型
將當前時刻雷達局部坐標系中的（({u}_{1}),0）點作為下一時刻雷達的位置，所以運動方程可以設為：
[ left[begin{array}{cc} {x_{n}} \ {y_{n}} end{array} right] = left[begin{array}{cc} costheta_{n-1} & -sintheta_{n-1} \ sintheta_{n-1} & costheta_{n-1} end{array}right] left[begin{array}{cc} {u}_{1} \ 0 end{array}right] + left[begin{array}{cc} {x_{n-1}} \ {y_{n-1}} end{array} right] + left[begin{array}{cc} {n}_{1} \ 0 end{array} right] ]
其方位每次增加固定的(u_2):
[ {theta_{n+1}} = {theta_n} + {u}_{2} + {n}_{2} ]
其中(n_1,n_2)為系統噪聲。
4.1.3 觀測模型
設路標點(i)的狀態為(x_{L_i}=)((m_1)，({m}_{2}))，則其在當前雷達坐標系的坐標為：
[ left[begin{array}{cc} {{{ladar}_{1}}} \ {{{ladar}_{2}}} end{array} right] = {left[begin{array}{cc} costheta_n & -sintheta_n \ sintheta_n & costheta_n end{array}right]}^{-1} left[begin{array}{cc} {{{m}_{1}}} \ {{{m}_{2}}} end{array} right] – left[begin{array}{cc} {x_n} \ {y_n} end{array} right] ]
則觀測量為：
[ xi = atan2left({{{ladar}_{2}}},{{{ladar}_{1}}}right) + {v}_{1} ]
[ s = sqrt{{left({{{ladar}_{1}}}right)}^{2} + {left({{{ladar}_{2}}}right)}^{2}} + {v}_{2} ]
其中(v_1,v_2)為測量噪聲。
4.2 實驗結果
使用MATLAB編寫程序進行仿真。
代碼地址:https://github.com/liuzhenboo/EKF-2D-SLAM
4.2.1 第一次狀態增廣
其中左圖黑色的點表示濾波器新加入的路標點狀態均值，綠色橢圓表徵路標狀態的協方差，其橢圓方程為：
[ (mathbf{x}-overline{mathbf{x}})^{top} mathbf{P}^{-1}(mathbf{x}-overline{mathbf{x}})=text { const } ]
其中x為路標狀態, P為路標的協方差。
程序中是對P進行SVD分解，得到橢圓的方向R以及半軸到的長度，進行繪圖。
4.2.2 狀態增廣，觀測更新
如上圖，黑色橢圓對應的路標點表示雷達曾經觀測過它，但是當前時刻沒有觀測到；紅色橢圓對應的路標點表示雷達曾經觀測過它，並且當前時刻也觀測到了；藍色代表當前剛剛初始化的新路標點。
注意：由於數值計算的原因，圖中幾何元素的位置關係可能和實際有些許差別；比如有的路標點明明不在雷達範圍內，卻開始初始化（綠色），這是因為計算過程中matlab四捨五入導致的結果。
4.2.3 狀態不再增廣，只有觀測更新
如上圖，不再存在藍色的協方差橢圓，代表狀態增廣停止，濾波器的大小不再改變。
