這一節，在熟悉了Featue maps相關概念之后，我們將開始學習基於EKF的特征圖SLAM算法。
1. 機器人，圖和增強的狀態向量
隨機SLAM算法一般存儲機器人位姿和圖中的地標在單個狀態向量中，然后通過一個遞歸預測和量測過程來估計狀態參數。其中，預測階段通過增量航跡估計來處理機器人的運動，並增加了機器人位姿不確定性的估計。當再次觀測到Map中存儲的特征后，量測階段，或者叫更新階段開始執行，這個過程可以改善整個的狀態估計。當機器人在運動過程中觀測到新特征時，便通過一個狀態增強的過程將新觀測的特征添加到狀態向量中。
機器人的狀態，即一個相對於參考笛卡爾坐標系的自身位姿參數通過均值和協方差來定義：
（1）
（2）
圖的協方差矩陣Pm 包含特征之間的互相關信息（即非對角項），這些交叉關聯信息表征了每個特征對Map中其他特征相關信息的依賴。由於特征的位置是靜態的，當周圍的環境也即Map是剛性的話，特征之間的交叉關聯將會隨着的特征的二次觀測增強。圖中的特征可以表示如下
（3）
（4）
SLAM中的圖通過一個串聯了機器人位姿和特征圖狀態的增強狀態向量定義，如圖1所示。這是很有必要的，因為一致的SLAM依賴於機器人位姿和圖之間相關性Pvm
（5）
（6）
Fig. 1 增強的狀態向量.
需要注意的是，狀態的初始化時通常為
和
.也就是說，還沒有特征被機器人觀測到，並且初始時刻機器人位於參考坐標系的原點。
2. 預測階段
隨機SLAM算法的過程模型根據航跡推算運動估計來確定機器人相對於先前時刻位姿的運動，圖特征仍然保持靜止。這個模型對狀態估計的影響存在於狀態向量
部分和狀態協方差矩陣中的
以及
項。而
和
這些圖特征相關項仍保持不變。
估計帶來的機器人（這里以移動小車為例）位姿變化
和協方差
通常用車輪的編碼器測量和小車對應的運動學模型獲取。本文里面的位姿改變通過基於激光測距的航跡推算獲取，該算法可以通過批處理數據關聯算法來找出序列激光掃描之間的相對位姿關系。
Fig. 2 機器人位姿變化示意.
因此，預測的增強狀態可以表示為
（7）
（8）
其中，雅各比矩陣
和
的定義為
（9）
（10）
雅各比矩陣
和
定義為
（11）
（12）
因為雅各比矩陣僅僅影響機器人的協方差矩陣
和對應的交叉相關性
，方程（8）用下式高效地實現
（13）
3. 更新階段
如果已經存儲在圖中的估計特征
被距離方位傳感器二次觀測到，此時對應的量測量可以表示為
（14）
（15）
其中，量測量
分別表示對應的距離值和相對於觀測機器人的偏轉角度。
為量測協方差。傳感器獲取的量測值和圖之間的關聯可以通過下面的方程表示
（16）
Kalman增益
可以得到
（17）
（18）
（19）
其中，雅各比矩陣
為
（20）
對於包含許多特征的SLAM圖，雅各比矩陣
將會含有許多零項，這會使得方程（18）和（19）的計算非常高效。
隨后我們可以從更新過程得到一個后驗SLAM估計
（21/22）
實踐表明，如果幾個獨立的量測同時可用，即知道
，便有可能獲取更加精確的更新。
4. 狀態增強
當機器人在環境中漫游時，一旦新特征被觀測到，就必須存儲到圖中。接下來將給出新觀測到的特征初始化方法，首先，狀態向量和對應的協方差矩陣通過新的量測的極值增強為
（23）
（24）
量測z 通過函數
被轉換位全局的笛卡爾坐標系特征位置，對應的變換表示如下
（25）
可以通過線性化函數
將增強的狀態初始化
（26/27）
其中，雅克比矩陣
表示為
（28）
與特征的儲存相比，特征的刪除非常簡單，只需將特征對應的元素直接從狀態向量刪除，同時將相關的行和列中的項目從協方差矩陣中刪除。