Week 12 — Simultaneous Localization and Mapping (SLAM) Day 1 of 7
Why This Matters
A robot that does not know where it is cannot act. A robot that does not know what surrounds it cannot plan. In most real deployments you cannot get one without the other. GPS is unavailable indoors, in tunnels, under canopy, and on Mars. Pre-built maps are wrong the moment furniture moves, a pallet is dropped, or a wall is added. So the robot must build the map and locate itself inside that map at the same time, using the same noisy sensors. That is SLAM.
Consider a warehouse AMR (autonomous mobile robot) doing order picking. It carries 60 kg, moves at 1.5 m/s, and must dock to a charging station with ±2 cm lateral tolerance. Its wheel encoders drift about 3% of distance traveled. After 200 m of driving, pure odometry is off by roughly 6 m — enough to put the robot in the wrong aisle. A 2D LiDAR scanning at 10 Hz gives range returns to walls and racking, but a scan alone tells you nothing about where you are until you match it against a map. If you trust the scan to build the map, and trust the map to fix your pose, you have a circular dependency. SLAM is the machinery that breaks that circle correctly instead of letting errors feed on themselves.
Without a principled SLAM formulation, three things break in the field. First, the map drifts and folds onto itself — corridors that should be 4 m apart appear 3.6 m apart because the estimate accumulated bias. Second, the pose covariance becomes wildly overconfident, so the planner trusts a location that is wrong. Third, loop closures get applied as hard jumps, and the robot teleports 40 cm on the map while the real robot is stationary, which corrupts every downstream controller. SLAM formulation is how you keep those failures bounded and measurable.
Core Concepts
The chicken-and-egg structure
SLAM estimates two things jointly: the robot trajectory $x_{1:t}$ and the map $m$. The map is usually a set of landmarks (points) or an occupancy grid. The key insight is that these two estimates are coupled. If you knew the trajectory exactly, mapping would be a supervised estimation problem — easy. If you knew the map exactly, localization would be a filtering problem — also tractable. The difficulty is that both are unknown and each depends on the other.
Probabilistic robotics handles this by treating everything as random variables with probability distributions. We never claim “the robot is at (3.2, 1.7)”. We claim “the robot pose has mean (3.2, 1.7) with a 2×2 covariance matrix.” The covariance is not decoration. It is the thing that tells the estimator how much to trust a new measurement versus the prior. A LiDAR range return with σ = 0.02 m is far more trustworthy than an odometry step with σ = 0.05 m per meter driven. The estimator must weight them accordingly.
The classic formulation is the Bayes filter applied to the joint posterior:
$$p(x_{1:t}, m \mid z_{1:t}, u_{1:t})$$
where $z$ are measurements (scans, range returns) and $u$ are controls (wheel velocities, IMU). Every SLAM method — EKF-SLAM, FastSLAM, graph-based — is an approximation of this one object.
Correlation is the whole game
Here is the subtle part most tutorials skip. In SLAM, landmark estimates are correlated through the robot’s uncertain pose. Suppose the robot observes landmark A, drives 10 m with 4% odometry error (0.4 m uncertainty), then observes landmark B. The relative position of A and B is actually well known — both were seen from poses linked by the same drift. But their absolute positions share a common error. If you later revisit A and correct its position by 0.3 m, B must move by roughly the same amount, because they were observed in the same local frame.
Methods that ignore this correlation (like simply running a Kalman filter per landmark independently) will produce maps that tear apart at loop closure. The EKF-SLAM covariance matrix has size $(3 + 2N) \times (3 + 2N)$ for a 2D robot with $N$ landmarks — every landmark is cross-correlated with every other. That is why EKF-SLAM is $O(N^2)$ per update and why it scales badly past a few hundred landmarks. Graph-based SLAM stores the same information sparsely as edges, which is why modern systems prefer it.
| Representation | State size (2D, N landmarks) | Update cost | Handles correlation |
|---|---|---|---|
| EKF-SLAM | 3 + 2N | O(N²) | Exactly |
| FastSLAM (particle) | N particles × (pose + N landmarks) | O(M log N) | Approximately |
| Graph SLAM | Nodes + edges | Sparse solve | Exactly (as information matrix) |
Observability and the map-frame gauge
SLAM has a gauge freedom: if you rotate the entire trajectory and map by 30° and translate them together, all measurements are still equally consistent. The absolute frame is unobservable. Only relative geometry is observable. This matters practically: any SLAM solution is defined up to a rigid transform unless you anchor it (e.g., fix the first pose, or fuse a GPS/compass measurement). Engineers who forget this get confused when a “correct” solution disagrees with ground truth by a constant offset.
Mathematical Foundation
The measurement model links pose, map, and sensor reading. For a range-bearing sensor observing landmark $j$ from pose $i$:
$$ z_{i,j} = h(x_i, m_j) + v, \qquad v \sim \mathcal{N}(0, R) $$
Define the robot pose $x_i = (x, y, \theta)^T$ and landmark $m_j = (m_{jx}, m_{jy})^T$. The expected range and bearing are:
$$ r = \sqrt{(m_{jx}-x)^2 + (m_{jy}-y)^2}, \qquad \phi = \operatorname{atan2}(m_{jy}-y,\ m_{jx}-x) - \theta $$
The Jacobian of $h$ with respect to the pose, needed for every EKF and graph update:
$$ \frac{\partial h}{\partial x_i} = \begin{bmatrix} -\frac{m_{jx}-x}{r} & -\frac{m_{jy}-y}{r} & 0 \ \frac{m_{jy}-y}{r^2} & -\frac{m_{jx}-x}{r^2} & -1 \end{bmatrix} $$
Worked example. Robot at $x_i = (2.0,\ 1.0,\ 0.0)$, landmark at $m_j = (5.0,\ 5.0)$.
$$r = \sqrt{3^2 + 4^2} = 5.0\ \text{m}, \qquad \phi = \operatorname{atan2}(4, 3) - 0 = 0.927\ \text{rad}\ (53.13^\circ)$$
Now the pose Jacobian:
$$ \frac{\partial h}{\partial x_i} = \begin{bmatrix} -3/5 & -4/5 & 0 \ 4/25 & -3/25 & -1 \end{bmatrix}
\begin{bmatrix} -0.60 & -0.80 & 0 \ 0.16 & -0.12 & -1 \end{bmatrix} $$
Read the units. The first row is in m/m (range changes per meter of robot motion). The second row is rad/m for the first two entries and rad/rad for the last. This Jacobian is exactly what propagates pose uncertainty into measurement uncertainty. If the robot’s position covariance is $\Sigma_x = \text{diag}(0.01, 0.01, 0.0004)$ (i.e., 10 cm and 1.15° std), then the predicted measurement covariance is $J\Sigma_x J^T + R$. With $R = \text{diag}(0.0004, 0.0001)$ (2 cm range, 0.57° bearing), the range term becomes $0.36(0.01) + 0.64(0.01) + 0.0004 \approx 0.0104$, i.e. about 10.2 cm std. The pose uncertainty dominates the sensor noise — so the estimator correctly discounts the measurement. That number, not intuition, decides the Kalman gain.
Practical Example
Scenario. A differential-drive AMR in a 40 m × 25 m warehouse aisle. Payload 60 kg. Encoders give 3% distance error and 2°/m heading error. A 270° 2D LiDAR scans at 10 Hz with 0.02 m range noise. Target: dock to charger with ±2 cm lateral tolerance after driving up to 200 m.
Step 1 — State definition. Pose $x = (x, y, \theta)$ in the map frame. Landmarks: corners of racking and pallet stacks, detected as line-segment endpoints. Suppose we track 80 landmarks. EKF-SLAM state is $3 + 160 = 163$ dimensions. Covariance is $163 \times 163 = 26{,}569$ entries. That is manageable at 10 Hz on a modern embedded CPU, but the $O(N^2)$ update is the ceiling.
Step 2 — Motion prediction. Between scans (100 ms), the robot moves at most 0.15 m. Encoder uncertainty contributes about $\sigma = 0.03 \times 0.15 = 4.5$ mm in translation and $0.002 \times 0.15 \approx 0.3$ mrad in heading per step. Over 200 m that integrates to roughly 6 m position error if uncorrected — confirming the motivation.
Step 3 — Measurement update. For each observed landmark, compute the innovation $z - h(x, m)$ and its covariance $S = J\Sigma J^T + R$. If the Mahalanobis distance $\nu^T S^{-1} \nu$ exceeds a χ² threshold (e.g., 11.34 for 2 DOF at 99%), reject the association — it is probably a mismatched landmark. This gate prevents a wrong data association from corrupting the map.
Step 4 — Loop closure. After returning to the start aisle, the robot re-observes landmark 1. The innovation is small in relative terms but the accumulated drift is large. The estimator must correct the whole trajectory and all landmarks together — this is where correlation matters. A graph-based solver distributes the 6 m error across the loop, typically leaving residual error under 5 cm after optimization.
Step 5 — Docking. With the map corrected, the local pose relative to the charger fiducial is trusted to ±1.5 cm, inside the ±2 cm tolerance. Without loop closure, the 6 m drift would make docking impossible.
Common Pitfalls
1. Treating landmarks as independent. What goes wrong: the map tears at loop closure, with walls splitting by 20–40 cm. Why: per-landmark filters drop the cross-covariance that couples them through the trajectory. Detect: check the covariance matrix — if off-diagonal blocks are zero, you have lost correlation. Fix: use a joint filter (EKF-SLAM) or a graph representation that keeps shared constraints.
2. Ignoring the gauge freedom. What goes wrong: the estimator “converges” but disagrees with ground truth by a constant rotation. Why: the absolute frame is unobservable; only relative geometry is constrained. Detect: compare relative distances between landmarks, not absolute coordinates. Fix: anchor the first pose or fuse an absolute sensor, and always evaluate on relative error (ATE after alignment, not raw coordinate error).
3. Wrong data association. What goes wrong: a scan return matched to the wrong landmark injects a 2 m error. Why: nearest-neighbor matching fails when landmarks are close or the pose prior is loose. Detect: monitor Mahalanobis gating rejections; a spike means association is failing. Fix: use joint compatibility branch-and-bound (JCBB) or a maximum-likelihood association with a χ² gate.
4. Overconfident covariance. What goes wrong: the filter trusts its pose, rejects all corrections, and drifts unchecked. Why: process noise set too low, so $\Sigma$ shrinks below reality. Detect: normalized innovation squared (NEES) consistently above 1 means overconfidence. Fix: tune process noise with real data; validate NEES against ground truth.
5. Forgetting scan timing. What goes wrong: 5–10 cm map smearing at 1.5 m/s. Why: the LiDAR sweep takes 100 ms; the robot moves 15 cm during it, but the scan is stamped with one time. Detect: maps blur in the direction of travel. Fix: motion-compensate (deskew) scans using IMU or odometry before matching.
Key Takeaways
- SLAM jointly estimates trajectory and map because each depends on the other; the joint posterior $p(x_{1:t}, m \mid z_{1:t}, u_{1:t})$ is the object every method approximates.
- Landmark correlation through the trajectory is the core difficulty — it is why EKF-SLAM is $O(N^2)$ and why graph methods store constraints sparsely.
- Covariance is the decision-maker: the Kalman gain and data-association gates are driven by $\Sigma$, not by point estimates.
- The absolute frame is unobservable (gauge freedom); evaluate on relative geometry and anchor the map deliberately.
- Data association and process-noise tuning cause most field failures — gate with χ² tests and validate with NEES.
Tomorrow
We implement the two classic filters — EKF-SLAM with its full covariance matrix and FastSLAM with Rao-Blackwellized particles — and compare their cost and accuracy on the same warehouse dataset.