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}=g\left(\mathcal{R}, \mathcal{S}, \mathbf{y}_{n+1},v\right) \]
上式是單個路標點的逆觀測模型。
路標點的均值和雅克比：
\[ \overline{\mathcal{L}}_{n+1}=g\left(\overline{\mathcal{R}}, \mathcal{S}, \mathbf{y}_{n+1},0\right) \]
\[\mathbf{G}_{\mathcal{R}}=\frac{\partial g\left(\overline{\mathcal{R}}, \mathcal{S}, \mathbf{y}_{n+1},0\right)}{\partial \mathcal{R}} \]
\[ \mathbf{G}_{\mathbf v}=\frac{\partial g\left(\overline{\mathcal{R}}, \mathcal{S}, \mathbf{y}_{n+1},0\right)}{\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} \leftarrow\left[\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} cos\theta_{n-1} & -sin\theta_{n-1} \\ sin\theta_{n-1} & cos\theta_{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} cos\theta_n & -sin\theta_n \\ sin\theta_n & cos\theta_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 = atan2\left({{{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 狀態不再增廣，只有觀測更新
如上圖，不再存在藍色的協方差橢圓，代表狀態增廣停止，濾波器的大小不再改變。
