EKF SLAM

This is the chicken-and-egg problem at the heart of robotics, and for a long time it looked unsolvable. The trick that cracked it is almost embarrassingly simple, and you can watch it happen below.

The robot does not know where it is and does not know where the landmarks are. But it knows one precious thing: how far apart it and each landmark are, the instant it looks. Hold onto that: it is the entire secret.

Play with the entanglement

The accent ellipse is the robot's uncertainty. The blue ellipses are landmark uncertainties. The faint halo is sensor range: anything inside it gets observed this tick.

Try this:

1. Drive blind. Move the robot. Watch its uncertainty ellipse swell as odometry drift accumulates, the same drunkard's walk you met in the last phase.
2. First sighting. Spot a brand-new landmark. Its ellipse starts huge, then shrinks within a few looks as the robot triangulates it.
3. The magic moment. Drive a loop and return to a landmark you logged way back at the start. The instant you re-recognise it, the entire map snaps tighter at once: robot and every landmark. That is loop closure, and it is the most satisfying half-second in all of SLAM.
4. Spot the strings. Watch two nearby landmark ellipses shrink together even when you only looked at one. They are not independent. They never were.

One matrix to bind them

Here is the idea most courses bury under linear algebra. EKF-SLAM does not keep a separate guess for the robot and a separate guess for each landmark. It keeps a single state vector (robot pose stacked on top of every landmark position) and a single covariance matrix that records how every estimate correlates with every other.

That shared matrix is the secret. When you re-observe an old landmark, the correction doesn't just nudge that one landmark. It flows down the off-diagonal correlations into the robot's pose, and from there back out into landmarks the robot hasn't looked at in minutes. The robot and the map are tied together by invisible springs of mathematics. Pull one, and the whole structure heals.

Predict, then correct, now with a map

EKF-SLAM is just the Kalman two-step you already know, run on a state vector that happens to include the world.

Predict. The robot moves. Push the pose through the motion model, and let its uncertainty grow:

$$\hat{x}_{k|k-1} = f(\hat{x}_{k-1}, u_k)$$ $$P_{k|k-1} = F_k P_{k-1} F_k^T + Q_k$$

where $F_k = \dfrac{\partial f}{\partial x}$ is the Jacobian that linearizes the motion: the "E" in EKF, because the real motion is curved and we pretend, locally, that it is flat.

Update. A landmark comes into view. Compute the Kalman gain, then correct everything by how wrong the prediction was:

$$K = P H^T (H P H^T + R)^{-1}$$ $$\hat{x}_k = \hat{x}_{k|k-1} + K \, (z - h(\hat{x}_{k|k-1}))$$

Here $h$ is the measurement model (the range and bearing the robot expects to see) and $z - h(\cdot)$ is the innovation, the surprise. The genius is that $K$ has a row for every state, so a single observation rewrites the robot pose and all the correlated landmarks in one stroke.

↘ The math: why the covariance matrix is the whole map

Stack the robot pose and all $n$ landmarks into one state:

$$\mathbf{x} = [x_r, y_r, \theta_r, x_1, y_1, x_2, y_2, \ldots, x_n, y_n]^T$$

The robot pose is $(x_r, y_r, \theta_r)$; each $(x_i, y_i)$ is a landmark. The estimate's uncertainty lives in one big covariance matrix:

$$\Sigma = \begin{bmatrix} \Sigma_{rr} & \Sigma_{rm_1} & \Sigma_{rm_2} & \cdots \\ \Sigma_{m_1 r} & \Sigma_{m_1 m_1} & \Sigma_{m_1 m_2} & \cdots \\ \vdots & & & \ddots \end{bmatrix}$$

The diagonal blocks are how unsure you are about each thing on its own. The off-diagonal blocks ($\Sigma_{rm_1}$, $\Sigma_{m_1 m_2}$) are the springs. They say "if the robot turns out to be a metre east, this landmark must be a metre east too." Loop closure works because those blocks are non-zero: a measurement of one landmark reaches every estimate it is correlated with.

For a range-and-bearing sensor, the measurement Jacobian $H$ (with $\Delta x, \Delta y$ the offset from robot to landmark and $d = \sqrt{\Delta x^2 + \Delta y^2}$) is:

$$H = \begin{bmatrix} \frac{-\Delta x}{d} & \frac{-\Delta y}{d} & 0 & \frac{\Delta x}{d} & \frac{\Delta y}{d} \\ \frac{\Delta y}{d^2} & \frac{-\Delta x}{d^2} & -1 & \frac{-\Delta y}{d^2} & \frac{\Delta x}{d^2} \end{bmatrix}$$

Notice it has columns for the robot pose and for the observed landmark. That is the channel through which a single look corrects both ends of the spring at once.

The quadratic cost falls straight out of this picture: a matrix that correlates everything with everything has on the order of $n^2$ entries to maintain. Beautiful, and unscalable.

Where it earns its keep

The hard part in practice is rarely the filter. It is data association: deciding which landmark you're looking at. Guess wrong, fuse a measurement into the wrong landmark, and the springs yank the whole map into a confident, catastrophic lie. The standard defence is the Mahalanobis distance, which measures "how surprising is this observation, scaled by how uncertain I already am," and refuses any match beyond a few sigma.

// Mahalanobis distance: accounts for uncertainty
fn mahalanobis_dist(z: Vec2, landmark: Vec2, s: Mat2) -> f32 {
    let innovation = z - landmark;
    (innovation.transpose() * s.inverse() * innovation).sqrt()
}

// Match to nearest landmark within 3-sigma; otherwise it's new
if mahal_dist < 3.0 { /* associate */ } else { /* spawn a landmark */ }

A complete loop subscribes to wheel odometry at high rate for the predict step, to a lidar scan at a lower rate for the update step, extracts landmarks (walls, corners), associates them, and publishes a corrected pose plus a map of landmark positions.

Key takeaways

• SLAM estimates the robot pose and the map simultaneously; neither comes first.
• All of it lives in one covariance matrix; the off-diagonal correlations are the map's structure.
• Loop closure (re-seeing an old landmark) tightens every estimate at once.
• The elegance costs $O(n^2)$ per update: great for hundreds of landmarks, painful for millions.