Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M3 Mapping and State Estimation / M3-T7

Localization & SLAM · Theory · M3

M3-T7 — EKF-SLAM

The extended Kalman filter on the joint pose-and-map state, step by step.

  • Lesson 7 of 8
  • Module 3: Mapping and State Estimation
  • Theory
  • About 9 minutes to read

This lesson writes down the EKF-SLAM algorithm completely enough to implement it, and then runs one update by hand so every number can be checked. The structure is the one from the previous lesson; the new content is the Jacobians, the handling of new landmarks and the failure modes of linearisation.

Learning objectives

After this lesson, you should be able to:

  • Write the EKF-SLAM prediction using the extended motion Jacobian and the update with the sparse measurement Jacobian.
  • Initialise a new landmark in the joint state with the correct cross-covariances.
  • Carry out one prediction and one update on a small example and interpret the changes in the covariance.
  • Explain the sources of inconsistency in EKF-SLAM and list mitigations.

The problem and the intuition

An EKF is the Bayes filter with Gaussians and a straight-line approximation of the models around the current estimate. For SLAM there is nothing new in the idea: one state vector, one covariance matrix, two steps. What is new is the bookkeeping. The state is long, but the models are local: moving touches only the robot, and a measurement touches only the robot and one landmark. Careful code exploits that sparsity.

Think of EKF-SLAM as the localisation EKF of M2-L3 with a map attached to the state. Everything you wrote there carries over.

Structure of the EKF-SLAM covarianceA block matrix with a 3 by 3 robot block in the top-left, off-diagonal blocks of robot-landmark correlations, and landmark blocks on the diagonal. The prediction step only changes the robot row and column; an update changes the whole matrix.ΣxxΣxmΣmxΣmm prediction: only the first block row and column change Predictionμ̄ = g(μ, u) Σ̄ = Gₓ Σ Gₓᵀ + noise Update (landmark j)H touches the pose and landmark j only,K = Σ Hᵀ S⁻¹ is dense: all of μ and Σ change. New landmark: append 2 rows and columns.
Which parts of μ and Σ each step touches. Prediction touches the robot’s row and column; an update changes everything through the gain.

The algorithm: EKF-SLAM

Problem. Maintain a Gaussian N(μ, Σ) over y = (x, m) as controls and range-bearing observations of identified landmarks arrive.

Assumptions. Known data association; Gaussian noise; models linearised at the current estimate; static landmarks; the first pose defines the origin (so its covariance starts at zero).

Prediction

EKF-SLAM prediction
μˉ=μ+Fx⊤(g(x, u)−x)\bar{\mu}=\mu +F_{x}^{\top}(g(x,\,u)-x)
only the pose entries change
Σˉ=G Σ G⊤+Fx⊤ Rx Fx\bar{\Sigma}=G\,\Sigma \,G^{\top}+F_{x}^{\top}\,R_{x}\,F_{x}
G=I+Fx⊤(Gx−I) Fx,G=I+F_{x}^{\top}(G_{x}-I)\,F_{x},
Fx=[I3 0 …0](3×(3+2N))F_{x}=[I_{3}\,0\,\ldots 0](3\times (3+2N))

Here g and its pose Jacobian Gx are the odometry motion model from M2-T6 and Rx = Gu Qu Guᵀ the odometry noise in pose space. Since G is the identity everywhere except in its top-left 3×3 block, in practice one updates only the first three rows and columns of Σ.

Update for an observation of landmark j

if landmark j is new
lj=(x+r cos⁡(θ+b), y+r sin⁡(θ+b)), extend μ and Σl_{j}=(x+r\,\cos (\theta +b),\,y+r\,\sin (\theta +b)),\,\text{extend}\,\mu \,\text{and}\,\Sigma
z^=h(x, lj)\hat{z}=h(x,\,l_{j})
ν=z−z^(wrap the bearing)\nu =z-\hat{z}\text{(wrap the bearing)}
H=[Hx 0 …Hl …0]H=[H_{x}\,0\,\ldots H_{l}\,\ldots 0]
non-zero only in the pose and landmark-j columns
S=H Σˉ H⊤+QS=H\,\bar{\Sigma}\,H^{\top}+Q
K=Σˉ H⊤ S−1K=\bar{\Sigma}\,H^{\top}\,S^{-1}
μ=μˉ+K ν\mu =\bar{\mu}+K\,\nu
Σ=(I−K H) Σˉ\Sigma =(I-K\,H)\,\bar{\Sigma}
Fx
selector matrix that picks the three pose entries out of the full state.
Hx, Hl
Jacobians of the range-bearing model with respect to the pose and to the landmark (M2-T6).
Q
measurement noise covariance (2×2).
S, K
innovation covariance (2×2) and Kalman gain ((3+2N)×2).

Initialising a new landmark

The landmark is placed from the current pose estimate and the measurement. Its covariance and cross-covariance with the existing state follow by propagating through the initialisation function:

New landmark covariance
∑ll=Jx ∑xx Jx⊤+Jz Q Jz⊤\sum _{\text{ll}}=J_{x}\,\sum _{xx}\,J_{x}^{\top}+J_{z}\,Q\,J_{z}^{\top}
∑l, y=Jx ∑x, y\sum _{l},\,y=J_{x}\,\sum _{x},\,y
(y=existing state)(y=\text{existing state})

where Jx = ∂l/∂x and Jz = ∂l/∂(r, b). The cross-term is important: the new landmark is correlated with the robot pose and with everything the pose is correlated with.

Failure modes

  • Linearisation error and inconsistency. The Jacobians are evaluated at the estimate, not the truth. After the heading estimate becomes confident, the filter creates information about heading that the data do not contain (the unobservable global rotation). Result: overconfident covariance and divergence (Julier and Uhlmann, 2001; Huang and Dissanayake, 2007).
  • Wrong association. A single bad update shifts the whole state and cannot be undone.
  • Wrapping errors. Unwrapped bearing innovations or heading entries give updates of size 2π.
  • Numerical issues. Σ loses symmetry and positive definiteness: use the Joseph form and symmetrise.
  • Scaling. O(N²) per update (previous lesson).

A worked example: one landmark, one prediction, one update

The robot starts at (0, 0, 0) with zero uncertainty (it defines the frame). The sensor has σr = 0.1 m, σb = 0.03 rad. Odometry noise: translation noise 0.05 per metre (so 5 cm for a 1 m move), rotation noise 0.02 per metre.

  1. First observation (new landmark). r = 5, b = 0. Place l = (5, 0). With Σxx = 0 the landmark covariance is Jz Q Jzᵀ = diag(0.1², (5 × 0.03)²) = diag(0.0100, 0.0225). The cross-covariance with the pose is zero. State size 5.
  2. Prediction. The robot drives u = (1 m, 0 rad). The mean becomes (1, 0, 0). The pose noise from odometry is Rx = Gu Qu Guᵀ with Qu = diag(0.05², 0.02²): Rx = [[0.0025, 0, 0], [0, 0.0001, 0.0002], [0, 0.0002, 0.0004]]. So σx = 5 cm, σy = 1 cm, σθ = 0.02 rad. The landmark block is unchanged.
  3. Second observation of the same landmark. The robot at (1, 0, 0) predicts r̂ = 4 and b̂ = 0. It measures r = 4.02, b = 0.01. The innovation is ν = (0.02, 0.01).
  4. Jacobians. dx = 4, dy = 0, r = 4: Hx = [[−1, 0, 0], [0, −0.25, −1]] and Hl = [[1, 0], [0, 0.25]]. The innovation covariance is S = H Σ̄ Hᵀ + Q = [[0.0225, 0], [0, 0.0028]].
  5. The update. The gain moves the pose by about (−0.002, −0.0008) m and the landmark by about (+0.009, +0.020) m: the new landmark estimate is (5.009, 0.020). The corrected standard deviations are: pose x 4.7 cm (from 5.0), y 0.91 cm, θ 0.018 rad; landmark x 7.5 cm (from 10), y 10.6 cm (from 15). The landmark’s uncertainty fell a lot, the pose’s a little.
  6. The correlations. After the update Σx,l = diag(0.0011, 0.00045) and the heading is correlated with the landmark’s y (0.0009): the pose and the landmark are now tied. A later observation that moves the landmark moves the pose with it.

You will implement exactly these two steps in the lab and check them against a reference.

Engineering implications and limitations

Engineering implications

  • Exploit the sparsity in code. Update only the pose block in prediction; in the update, form H as a 2×(3+2N) matrix with just the relevant columns and use the structure to avoid N×N products where possible.
  • Joseph form and symmetrisation keep Σ a valid covariance over long runs; check eigenvalues occasionally.
  • Observability. The global position and heading of the map are unobservable from relative measurements. Do not let the filter shrink them below the initial uncertainty: a common fix is the first-estimates Jacobian (Huang et al., 2008).
  • Tune the noise by consistency tests (NEES with ground truth, NIS without) as in M2-L3.
  • Use it where it fits. EKF-SLAM is fine for small maps with reliable landmarks (fiducial markers, reflectors). For anything larger, use a graph-based or particle-based method.

Limits

EKF-SLAM linearises once and cannot revisit, scales as O(N²), and assumes Gaussian beliefs, so it cannot hold the multiple hypotheses of a repetitive environment. These are the reasons the field moved to graph optimisation (Module 4) and particle filters (M3-T8).

Common misconceptions

“The update only changes the landmark that was observed.”

The gain is dense: the pose and every correlated landmark change too.

“The new landmark has no correlation with the robot.”

It is placed from the pose estimate, so it is correlated with the pose and with everything correlated with the pose.

“EKF-SLAM is consistent if it converges.”

A converged EKF can still be overconfident because of linearisation error; test consistency separately.

“Covariances in the filter are rigorous probabilities.”

They are first-order approximations and often too optimistic.

Engineering takeaways

  1. EKF-SLAM keeps (μ, Σ) over the pose and all landmarks; prediction touches only the pose block, the update uses a sparse H but a dense gain.
  2. New landmarks are initialised from the pose and the measurement and must inherit the cross-covariances.
  3. The correlations created by the updates are what transmit a correction to the rest of the map.
  4. Linearisation error and unobservable global rotation cause inconsistency; the Joseph form and noise tuning help but do not cure it.
  5. It is a good teaching tool and a fit for small, well-identified maps, but not for large ones.

References and further reading

Established fundamentals. Consistent and invariant filters are an active research topic (Module 8).

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

In the prediction step only the first three rows and columns of Σ change. Why is that correct, and what stays unchanged?

Explanation

The motion model moves only the robot, and landmarks are static. The landmark–landmark block does not change; the pose block grows with odometry noise, and the pose–landmark cross-covariance is transformed by the motion Jacobian: Σ̄xm = Gx Σxm.

Question 2

A new landmark is added with its covariance set to Jz Q Jzᵀ and zero cross-covariance with the robot, even though the robot pose is uncertain. What is wrong?

Explanation

The landmark was placed using the uncertain pose, so its position inherits the pose error and is correlated with it. Setting the cross-covariance to zero removes the correlation, so later updates do not propagate to or from the pose correctly, and the covariance is overconfident. The initialisation must include Jx Σxx Jxᵀ and the cross terms.

Question 3

After several hours the covariance of an EKF-SLAM run is tiny, but the map is visibly rotated relative to the building. What is the likely cause?

Explanation

The filter became overconfident about the unobservable global orientation because of linearisation error (spurious information gain). Its covariance claims a certainty that the data cannot give. Remedies: first-estimates Jacobians, consistent filter variants, or a different estimator (graph optimisation).

What’s next

Gaussian filters assume one hypothesis. The next lesson drops that assumption and represents the belief with samples.

Practice this lesson

Implement a Simplified EKF-SLAM

Complete the prediction and update steps of a landmark EKF-SLAM with known association.

Next lesson

M3-T8 — Particle Filters and FastSLAM

Explain Monte Carlo localization, resampling and the Rao-Blackwellised idea behind FastSLAM.

Nothing below is required to finish this lesson. The last entry is a lesson of other courses on this site that teach the background or go further.