Week 12 — Simultaneous Localization and Mapping (SLAM) Day 2 of 7
Why This Matters
A robot vacuum that maps your living room but forgets where it is will clean the same corner five times and miss the kitchen. A warehouse AMR (autonomous mobile robot) that localizes but cannot map cannot navigate a newly added pallet rack. SLAM is the joint estimation of the robot’s pose and the map. In 2024, the two dominant classical filtering approaches are EKF-SLAM and FastSLAM. Both are still used in production today: EKF-SLAM appears in low-cost IMU+LiDAR fusion stacks and in embedded air purifier robots; FastSLAM’s particle factorization powers several commercial 2D LiDAR SLAM packages including GMapping-derived forks.
The real-world failure mode is this: if you naively apply a Kalman filter to a map with $N$ landmarks, the covariance matrix grows as $O(N^2)$ and the update step is $O(N^2)$ per observation. A 5,000-landmark map needs 25 million covariance entries and roughly 1.25 × 10¹¹ floating-point operations per LiDAR scan at 10 Hz. That is 1.25 TFLOP/s just for SLAM — no embedded CPU can sustain that. Filtering SLAM methods exist precisely to bound this cost. Without understanding EKF-SLAM and FastSLAM, an engineer will either ship an over-engineered solution or hit a hard wall at 200 landmarks and not know why.
Core Concepts
EKF-SLAM: The Joint State Vector
EKF-SLAM keeps the robot pose $\mathbf{x}t = [x, y, \theta]^\top$ and all landmark positions $\mathbf{m} = [m{1x}, m_{1y}, \dots, m_{Nx}, m_{Ny}]^\top$ in one joint Gaussian:
$$ \mathbf{\mu} = \begin{bmatrix} \mathbf{x}t \ \mathbf{m} \end{bmatrix}, \quad \mathbf{\Sigma} = \begin{bmatrix} \mathbf{\Sigma}{xx} & \mathbf{\Sigma}{xm} \ \mathbf{\Sigma}{mx} & \mathbf{\Sigma}_{mm} \end{bmatrix} $$
The state dimension is $3 + 2N$. For $N = 1000$ landmarks, that is 2,003 dimensions. The covariance matrix is 2003 × 2003 = 4.01 million entries. The prediction step propagates the robot pose and updates the cross-correlations. The update step, when a landmark is observed, corrects every landmark through the cross-covariance $\mathbf{\Sigma}_{mx}$. This is the key insight: observing one landmark refines the estimate of all others. That correlation is the “loop closure” mechanism, and it is why EKF-SLAM is mathematically elegant.
The cost per update is $O(N^2)$ because of the outer product $\mathbf{K} \mathbf{S} \mathbf{K}^\top$ on a $(3+2N)$-dimensional state. For $N = 1000$, that is roughly 4 million multiply-adds per observation. At 10 Hz with 20 landmark observations per scan, that is 800 MFLOP/s — feasible on an ARM Cortex-A72 but not on a Cortex-M4.
FastSLAM: Rao-Blackwellized Particle Filter
FastSLAM (Montemerlo et al., 2002) exploits the conditional independence structure of SLAM: given the robot’s path, the landmarks are independent. This is the Rao-Blackwellization trick. The posterior factors as:
$$ p(\mathbf{x}{1:t}, \mathbf{m} \mid \mathbf{z}{1:t}, \mathbf{u}{1:t}) = p(\mathbf{x}{1:t} \mid \mathbf{z}{1:t}, \mathbf{u}{1:t}) \prod_{i=1}^{N} p(\mathbf{m}i \mid \mathbf{x}{1:t}, \mathbf{z}_{1:t}) $$
Each particle carries its own robot path and its own set of $N$ independent 2×2 landmark EKFs. The cost per particle per update is $O(N)$ instead of $O(N^2)$. With $M = 100$ particles and $N = 1000$ landmarks, total cost is 100 × 1000 = 100,000 operations per scan — 8,000× cheaper than EKF-SLAM at the same map size.
| Method | State size | Update cost | Memory | Loop closure |
|---|---|---|---|---|
| EKF-SLAM | $3 + 2N$ | $O(N^2)$ | $O(N^2)$ | Implicit via cross-covariance |
| FastSLAM 1.0 | $M$ particles × path | $O(MN)$ | $O(MN)$ | Via resampling |
| FastSLAM 2.0 | $M$ particles × path | $O(MN)$ | $O(MN)$ | Proposal-aware sampling |
FastSLAM 2.0 improves on 1.0 by sampling from the observation-aware proposal distribution rather than the motion model alone. This reduces particle depletion dramatically. In practice, FastSLAM 2.0 achieves comparable accuracy to EKF-SLAM with 30–100 particles on standard datasets like the Intel Research Lab (28 m × 15 m, 2D LiDAR).
Mathematical Foundation
EKF-SLAM Prediction
Given motion $\mathbf{u}_t = [\Delta x, \Delta \theta]^\top$ in the robot frame, the state prediction is:
$$ \mathbf{\mu}{t|t-1} = \mathbf{\mu}{t-1} + \mathbf{F}^\top \mathbf{f}(\mathbf{u}t, \mathbf{\mu}{t-1}) $$
where $\mathbf{f}$ is the motion model and $\mathbf{F}$ is the Jacobian of the state transition with respect to the full state. The covariance prediction:
$$ \mathbf{\Sigma}{t|t-1} = \mathbf{F} \mathbf{\Sigma}{t-1} \mathbf{F}^\top + \mathbf{G} \mathbf{Q}_t \mathbf{G}^\top $$
Here $\mathbf{Q}_t$ is the motion noise covariance (typically $\text{diag}(0.01^2, 0.01^2, (0.5°)^2)$ for a differential-drive robot with wheel encoders), and $\mathbf{G}$ is the noise Jacobian.
EKF-SLAM Update
For an observation $\mathbf{z}t = [r, \phi]^\top$ (range, bearing) of landmark $j$ at position $(m{jx}, m_{jy})$:
$$ \hat{\mathbf{z}} = \begin{bmatrix} \sqrt{(m_{jx} - x)^2 + (m_{jy} - y)^2} \ \text{atan2}(m_{jy} - y, m_{jx} - x) - \theta \end{bmatrix} $$
The Kalman gain is $\mathbf{K} = \mathbf{\Sigma} \mathbf{H}^\top (\mathbf{H} \mathbf{\Sigma} \mathbf{H}^\top + \mathbf{R})^{-1}$, where $\mathbf{H}$ is the 2 × (3+2N) Jacobian of $\hat{\mathbf{z}}$ with respect to the state. Only 5 columns of $\mathbf{H}$ are nonzero: the 3 pose columns and the 2 landmark columns. This sparsity is what makes the update tractable in code.
Worked Numeric Example
Robot at $(0, 0, 0)$, landmark at $(5, 0)$ (known perfectly). Measurement: $r = 5.0$ m, $\phi = 0°$. Suppose the true range is 5.1 m due to noise. Measurement noise $\sigma_r = 0.1$ m, $\sigma_\phi = 1° = 0.0175$ rad. Innovation: $\nu_r = 5.1 - 5.0 = 0.1$ m. Kalman gain on range dimension: $K_r = \sigma^2_{r,\text{prior}} / (\sigma^2_{r,\text{prior}} + \sigma^2_r)$. If prior range uncertainty is 0.2 m, $K_r = 0.04 / (0.04 + 0.01) = 0.8$. The corrected range is $5.0 + 0.8 \cdot 0.1 = 5.08$ m. The corrected variance is $(1 - 0.8) \cdot 0.04 = 0.008$ m², so $\sigma_r = 0.089$ m. That is the Kalman filter shrinking uncertainty by 55% from one measurement — and in EKF-SLAM, that same gain also nudges the robot’s pose and every correlated landmark.
FastSLAM Particle Weight
For particle $k$, the importance weight after observing landmark $j$:
$$ w_k = w_k \cdot \frac{1}{\sqrt{2\pi |\mathbf{S}{k,j}|}} \exp\left(-\frac{1}{2} \boldsymbol{\nu}{k,j}^\top \mathbf{S}{k,j}^{-1} \boldsymbol{\nu}{k,j}\right) $$
where $\boldsymbol{\nu}{k,j}$ is the innovation and $\mathbf{S}{k,j} = \mathbf{H} \mathbf{\Sigma}{k,j} \mathbf{H}^\top + \mathbf{R}$ is the innovation covariance for that particle. After all observations, weights are normalized and resampling occurs when the effective sample size $N{\text{eff}} = 1 / \sum_k w_k^2$ drops below $M/2$.
Practical Example
Scenario: A 30 kg differential-drive AMR (TurtleBot4-class) with a 2D LiDAR (RPLidar S2, 30 m range, 10 Hz, 0.5° angular resolution) navigating a 40 m × 25 m warehouse aisle. Target: 5 cm localization accuracy, 10 cm map accuracy at 10 Hz.
EKF-SLAM sizing: Aisle contains roughly 400 detectable corners (shelf edges, pallet corners). State dimension = 3 + 800 = 803. Covariance = 803² ≈ 645,000 entries (5.2 MB in float64). Update cost per LiDAR scan: with 20 landmark observations, 20 × 803² ≈ 12.9 MFLOP. At 10 Hz that is 129 MFLOP/s. Feasible on the robot’s Intel NUC but leaves little headroom. Memory grows quadratically: adding 100 more landmarks adds 1.6 MB.
FastSLAM sizing: Same map, 50 particles. Each particle holds 400 landmark EKFs (each a 2×2 covariance = 4 floats + 2 means). Memory per particle: 400 × 6 × 8 bytes ≈ 19 KB. Total = 50 × 19 KB ≈ 1 MB. Update cost: 50 × 20 × 4 ≈ 4,000 operations per scan. Essentially free.
Decision: Use FastSLAM 2.0. The 5 cm accuracy target is achievable because landmark observations are 30 m-range LiDAR returns with 2 cm range noise. The map is large enough that EKF-SLAM’s $O(N^2)$ becomes a scaling risk if the warehouse adds aisles. The one caveat: FastSLAM needs careful resampling. We set $M = 50$, resample threshold at $N_{\text{eff}} < 25$, and use a roughening noise of 1 mm on landmark means to avoid particle impoverishment. We verify accuracy by comparing to a ground-truth AprilTag grid every 20 m.
Result from field test: After 500 m of driving, FastSLAM 2.0 with 50 particles achieved 4.3 cm RMS pose error and 7 cm map error. EKF-SLAM with the same sensor achieved 3.8 cm pose error but consumed 4× the CPU and 5× the memory. Both met spec; FastSLAM had more headroom for the next aisle expansion.
Common Pitfalls
1. Linearizing a nonlinear motion model too coarsely. EKF-SLAM linearizes the motion and observation models at each step. If the robot rotates 90° in one timestep (e.g., after a wheel slip correction), the Jacobian is evaluated far from the true trajectory and the covariance becomes inconsistent — it shrinks too fast. Detect: monitor the normalized innovation squared (NIS); if it exceeds the chi-squared 95% threshold consistently, the filter is overconfident. Fix: reduce the timestep, add process noise, or use an unscented transform (UKF-SLAM).
2. Data association errors. EKF-SLAM cannot recover from a wrong landmark match. If the robot matches a new shelf corner to an old one 10 m away, the update step corrupts the entire covariance matrix in one step. FastSLAM is more robust because bad particles die, but it still suffers. Detect: track the Mahalanobis distance of each association; reject matches beyond the 99% chi-squared gate (5.99 for 2 DOF). Fix: use joint compatibility branch-and-bound (JCBB) instead of nearest-neighbor matching.
3. Particle depletion in FastSLAM. With too few particles or a poor proposal distribution, all particles collapse onto one hypothesis after a sharp turn. The map becomes overconfident and cannot recover. Detect: watch $N_{\text{eff}}$; if it stays below $M/4$ for many steps, you are depleted. Fix: use FastSLAM 2.0’s observation-aware proposal, increase $M$, or add roughening.
4. Ignoring the loop-closure correlation in EKF-SLAM. Engineers sometimes “reset” the robot pose covariance after returning to a known area, breaking the cross-covariance. The map then drifts because landmarks no longer constrain each other. Detect: compare map consistency against a ground-truth map; check that $\mathbf{\Sigma}_{mm}$ has nonzero off-diagonal blocks. Fix: never reset; if you must, use covariance intersection to merge.
5. Confusing FastSLAM’s per-particle maps with a shared map. Each particle has its own landmark estimates. If you naively average them, you get an inconsistent map. Detect: the averaged map shows ghost landmarks. Fix: extract the map from the highest-weight particle after the run, or use the weighted mean of particle maps only for visualization.
Key Takeaways
- EKF-SLAM is $O(N^2)$ in landmarks — usable up to roughly 500–1000 landmarks on embedded hardware, beyond which FastSLAM or graph-based SLAM wins.
- FastSLAM’s Rao-Blackwellization is the key idea: condition on the path, and landmarks become independent 2×2 EKFs. That drops cost to $O(MN)$.
- FastSLAM 2.0 beats 1.0 because it samples from an observation-aware proposal, avoiding particle depletion on sharp turns.
- Data association is the fragile part of both methods. Use Mahalanobis gating (chi-squared 5.99 for 2 DOF at 95%) and JCBB for dense environments.
- Validate with NIS and $N_{\text{eff}}$ — these two scalars catch most filter inconsistency and particle depletion before the map visibly breaks.
Tomorrow
We move from filtering to feature-based visual SLAM: ORB-SLAM’s three-thread architecture (tracking, local mapping, loop closing) and how it handles relocalization with a bag-of-words database.