Ch 10: SLAM — Probabilistic Robotics

Chapter 0: The SLAM Problem

The robot has no map. It doesn't know where it is. All it has are sensor measurements z_1:t and controls u_1:t. It must simultaneously build a map of the environment and localize itself within that map. This is SLAM — Simultaneous Localization and Mapping.

Why SLAM is hard: It's a chicken-and-egg problem. To build a good map, you need accurate poses. To get accurate poses, you need a good map. Neither is available initially. SLAM algorithms must solve both problems at once, which makes it significantly harder than either localization or mapping alone.

SLAM is more difficult than localization (Chapter 7) because the map is unknown and must be estimated. It is more difficult than mapping (Chapter 9) because the poses are unknown and must be estimated. The interaction between these two unknowns creates rich mathematical structure that we will explore over the next five chapters.

The SLAM problem has two components: a continuous component (estimating poses and landmark positions) and a discrete component (determining which observation corresponds to which landmark — the data association problem).

Check: What makes SLAM harder than localization alone?

The map is unknown and must be estimated jointly with the pose, creating a chicken-and-egg problem The robot moves faster The sensors are noisier

Chapter 1: Online vs Full SLAM

There are two formulations of the SLAM problem:

Formulation	Posterior	Character
Online SLAM	p(x_t, m \| z_1:t, u_1:t)	Current pose + map; incremental
Full SLAM	p(x_1:t, m \| z_1:t, u_1:t)	Entire path + map; batch

Online SLAM estimates only the current pose x_t and the map m. Past poses are integrated out. This is what we need for real-time operation: the robot processes data incrementally and discards old measurements.

Full SLAM estimates the entire path x_1:t and the map. This is a batch problem — all data is considered at once. Full SLAM can produce better maps because it can revise past pose estimates when new information arrives.

The relationship: Online SLAM is obtained from full SLAM by integrating out past poses: p(x_t, m | ...) = ∫ ... ∫ p(x_1:t, m | ...) dx₁ ... dx_t-1. This integration causes past pose information to be "baked into" correlations between landmarks — a crucial structural property we'll explore shortly.

Check: How does online SLAM relate to full SLAM?

Online SLAM integrates out past poses from the full SLAM posterior They are completely unrelated formulations Full SLAM is just online SLAM run backwards

Chapter 2: EKF SLAM Setup

EKF SLAM applies the extended Kalman filter to the online SLAM problem. It makes several key assumptions:

• Feature-based maps: The map is a collection of N point landmarks. Each landmark has coordinates (m_j,x, m_j,y) and a signature s_j.

• Gaussian noise: Motion and measurement noise are Gaussian. The posterior must remain approximately Gaussian.

• Small uncertainty: Linearization only works well when the uncertainty is small (orientation error < 20°).

• Positive measurements only: The algorithm can only process observed landmarks, not the absence of expected landmarks.

The key innovation of EKF SLAM: Instead of estimating only the 3D robot pose (as in EKF localization), EKF SLAM estimates a (3 + 3N)-dimensional state vector that includes the robot pose AND all N landmark positions. The covariance matrix captures the correlations between the robot and every landmark, and between all pairs of landmarks.

Check: What does EKF SLAM estimate beyond EKF localization?

The positions of all landmarks in the map, in addition to the robot pose The velocities of moving objects The sensor noise parameters

Chapter 3: Known Correspondence

With known correspondences, EKF SLAM is a direct extension of EKF localization. The state vector combines the robot pose and all landmarks:

y_t = (x, y, θ, m_1,x, m_1,y, s₁, m_2,x, m_2,y, s₂, ..., m_N,x, m_N,y, s_N)^T

This is a vector of dimension 3 + 3N. The mean μ_t and covariance Σ_t are (3+3N)-dimensional.

Motion update: Only the robot pose changes during motion. The landmark positions stay the same. But the covariance grows because the robot-landmark correlations weaken with motion noise.

Measurement update: Observing landmark j updates both the robot pose estimate and the landmark j position estimate. Because of the correlations in Σ_t, observing landmark j also indirectly improves the estimates of all other landmarks.

New landmarks: When the robot sees a landmark for the first time, it initializes that landmark's position using the current pose estimate and the measurement: m̂_j = (x + r cos(φ + θ), y + r sin(φ + θ)). The initial uncertainty is large, but it decreases as the landmark is re-observed from different positions.

Check: What happens when EKF SLAM observes a landmark?

Both the robot pose and the landmark position are updated, and through correlations, all other landmark estimates improve too Only the robot pose is updated Only the observed landmark is updated

Chapter 4: The State Vector

The covariance matrix Σ_t is the heart of EKF SLAM. It has dimension (3+3N) × (3+3N) and encodes all uncertainty:

Block	Dimension	Meaning
Σ_xx	3 × 3	Robot pose uncertainty
Σ_{xm_j}	3 × 3	Correlation between robot and landmark j
Σ_{m_im_j}	3 × 3	Correlation between landmarks i and j

The off-diagonal blocks are crucial. When the robot observes landmark A and then moves to observe landmark B, the two landmarks become correlated through the shared robot pose. The covariance matrix captures this: Σ_{m_Am_B} ≠ 0.

The fundamental insight: In the limit, as the robot observes landmarks repeatedly, all landmark estimates become fully correlated. The covariance matrix becomes dense. This is the mathematical reason EKF SLAM is O(N²) per update — you must update the entire dense covariance matrix whenever you observe any landmark.

The computational cost: storing Σ_t requires O(N²) space. Updating it requires O(N²) time per observation. For 1,000 landmarks, that's a million-element matrix — manageable. For 100,000 landmarks, it's ten billion elements — impractical. This quadratic scaling is the main limitation of EKF SLAM.

Check: Why is EKF SLAM O(N²) per update?

The covariance matrix is (3+3N) x (3+3N) and becomes dense as landmarks correlate through shared pose observations The robot must visit every landmark at each step The map is stored as a grid

Chapter 5: Correlations

Why do landmark estimates become correlated? Consider this scenario:

1. The robot sees landmark A. Now the robot pose and A are correlated.

2. The robot moves and sees landmark B. Now the robot pose and B are correlated.

3. Through the shared robot pose, A and B become correlated: if A turns out to be further east than expected, the robot was probably further east when it saw A, which means it was also further east when it saw B, which means B is probably further east too.

Loop closure: When the robot revisits a previously seen landmark, the correlation structure creates a powerful "loop closure" effect. Re-observing landmark A after a long journey not only improves A's estimate, but through the correlations, it simultaneously improves the estimates of ALL landmarks observed along the way. This is the magic of SLAM — information propagates through the correlation network.

The price of correlations: you cannot estimate landmarks independently. Updating any single landmark requires updating its correlations with all other landmarks. This is why the covariance matrix is dense and updates are O(N²).

Check: Why does re-observing one landmark improve all other landmark estimates?

Because all landmarks are correlated through shared robot poses — improving one propagates through the covariance matrix Because the robot can see all landmarks at once Because the map is a grid

Chapter 6: Unknown Correspondences

In practice, the robot doesn't know which landmark it's seeing. EKF SLAM uses the same maximum likelihood correspondence approach as EKF localization (Chapter 7):

j(i) = argmin_k (z_tⁱ - ẑ_t^k)^T Ψ_k^-1 (z_tⁱ - ẑ_t^k)

Minimizing the Mahalanobis distance over all known landmarks. If no landmark produces a sufficiently small distance, the observation is treated as a new landmark and added to the state vector.

The danger in SLAM: Wrong correspondences are even more catastrophic in SLAM than in localization. A wrong association doesn't just corrupt the pose — it corrupts the map, which corrupts future correspondences, which corrupts the map further. A single bad association can cause the entire SLAM system to diverge irreversibly.

Provisional landmarks: A practical defense: don't permanently add a landmark until it has been observed multiple times with consistent correspondences. This prevents spurious features (sensor noise, moving objects) from polluting the map.

Check: Why are wrong correspondences more dangerous in SLAM than in localization?

They corrupt both the map and the pose, creating a feedback loop of errors that can't be recovered They are not more dangerous SLAM uses more landmarks

Chapter 7: Feature Management

The quality of EKF SLAM depends heavily on the quality of the features. Several strategies help:

Strategy	Purpose
Provisional landmarks	Require multiple consistent observations before adding to map
Outlier rejection	Discard observations with large Mahalanobis distance
Mutual exclusion	Two different observations can't correspond to the same landmark
Landmark pruning	Remove landmarks that haven't been seen in a long time

The landmark density trade-off: Too few landmarks and the robot can't localize accurately (large pose uncertainty increases wrong-correspondence risk). Too many landmarks and the computation becomes prohibitive, plus dense landmarks are easily confused with each other.

Practical advice: Use artificial beacons or highly distinctive natural features (corners, unique textures). Keep landmarks sparse enough that confusion is unlikely. Apply outlier rejection aggressively. And remember: EKF SLAM is fundamentally a position tracking algorithm — it requires good initial estimates and cannot handle global localization.

Check: What is the purpose of provisional landmarks?

To prevent spurious features from being permanently added to the map before being verified To make the algorithm faster To reduce the number of sensor readings

Chapter 8: SLAM Demo

Watch EKF SLAM in action. The robot moves through an environment, discovering landmarks. The state vector grows as new landmarks are added. Covariance ellipses show the uncertainty of each estimate.

EKF SLAM: Building a Map

Orange dot = robot, teal diamonds = estimated landmark positions, ellipses = uncertainty. Click Step to move the robot and discover/re-observe landmarks.

Ready. Click Step to begin.

What to notice: When a new landmark is first seen, its uncertainty ellipse is large. As the robot re-observes it from different positions, the ellipse shrinks. When the robot "closes the loop" by returning to an early landmark, ALL landmark uncertainties decrease simultaneously — that's the correlation structure at work.

Check: What happens to landmark uncertainties during loop closure in EKF SLAM?

All landmark uncertainties decrease simultaneously because of the correlations in the covariance matrix Only the re-observed landmark's uncertainty decreases Uncertainties increase during loop closure

Chapter 9: Summary

Aspect	EKF SLAM
State	Robot pose + all landmark positions (3+3N dimensions)
Complexity	O(N²) per update
Map type	Feature-based (point landmarks)
Correlations	Dense covariance; all landmarks correlated
Loop closure	Powerful — improves all estimates simultaneously
Limitations	Quadratic scaling, Gaussian assumption, brittle correspondences

What comes next: EKF SLAM's O(N²) scaling limits it to hundreds of landmarks. The next chapters explore alternatives: the Extended Information Filter (Ch 11) and Sparse Extended Information Filter (Ch 12) exploit sparsity in the information matrix to achieve better scaling. Chapters 13-14 tackle the data association problem and fast incremental mapping.

"See a landmark, learn where it is.
See it again from elsewhere, learn where everything is.
That is the miracle of SLAM."

Check: What is the fundamental computational limitation of EKF SLAM?

O(N²) per update due to the dense covariance matrix over all landmarks It cannot handle loop closure It requires GPS

Simultaneous Localization & Mapping