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

Localization & SLAM · Theory · M3

M3-T6 — Filtering-Based SLAM

Treat the robot pose and every landmark as one big state and run the Bayes filter on all of it.

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

The first practical solutions to SLAM were filters: keep one belief over the robot and the whole map, and update it as data arrive. They are elegant and they taught the field its main lessons: that errors are correlated, that correlation is what lets loop closure fix the whole map, and that a straightforward filter does not scale. This lesson sets up the filter; the next one derives the EKF version in detail.

Learning objectives

After this lesson, you should be able to:

  • Write the SLAM state and the two filter steps (predict, update) over the joint belief.
  • Describe the structure of the joint covariance and explain why correlations between landmarks make SLAM converge.
  • Compute the cost in memory and time as the map grows.
  • List the weaknesses of filtering SLAM: scaling, linearisation error and inconsistency, no re-linearisation of the past.

The problem and the intuition

Suppose the robot sees landmark A, drives, and sees landmark B. If its pose estimate was a little off, then A and B are both placed with the same wrong shift: their errors are tied together. When the robot later returns and sees A again, it learns that its pose was off, and, because B’s position was computed from that same pose, B can be corrected without being observed at all. A filter that remembers these ties, in the cross-terms of the covariance, gets this for free.

The cross-covariances between the robot and the landmarks, and between landmarks, are not a nuisance: they are the information that propagates a correction through the whole map.

The covariance grows quadraticallyA plot of memory for the full covariance matrix of a filter SLAM state with N landmarks, growing as the square of N: about 3.2 GB at ten thousand landmarks.02500500075001000000.81.62.43.2landmarks Ncovariance memory (GB) state = 3 + 2N numberscovariance = (3 + 2N)² numbers N = 100: 41,000 entries (0.3 MB)N = 10,000: 400 million entries (3.2 GB) Every measurement updates all of them.
The joint covariance of N landmarks has about 4N² entries and every update touches all of them.

The algorithm: the Bayes filter on the joint state

Problem. Estimate, online, the current robot pose xt and all N landmark positions m = (m1, …, mN) given controls and measurements so far: p(xt, m | z1:t, u1:t) (online SLAM, M1-T1).

Assumptions. Landmarks are static and identified (data association is solved or assumed); motion and measurement models are known with Gaussian noise; the Markov assumption of the Bayes filter holds.

The joint state and covariance

Joint state and covariance
y=(x, y, θ, l1x, l1y, …, lNx, lNy)⊤y=(x,\,y,\,\theta ,\,l_{1x},\,l_{1y},\,\ldots ,\,l_{\text{Nx}},\,l_{\text{Ny}})^{\top}
dimension 3+2N\text{dimension}\,3+2N
Σ=[∑xx∑xm∑mx∑mm]\Sigma =\begin{bmatrix}\sum _{xx} & \sum _{\text{xm}} \\ \sum _{\text{mx}} & \sum _{\text{mm}}\end{bmatrix}

The blocks mean: Σxx the uncertainty of the robot pose, Σmm the landmarks’ uncertainty and the correlations between them, and Σxm the correlations between robot and landmarks.

Pipeline

  1. Predict. The motion model changes only the robot pose: μ̄ = g(μ, u). Landmarks do not move. The covariance update touches the pose rows and columns: Σ̄xx = G Σxx Gᵀ + noise and Σ̄xm = G Σxm. The landmark block Σmm is unchanged.
  2. Observe. For each detection, associate it with a landmark j (M2-T7). If it is new, initialise its position from the pose and the measurement and augment the state.
  3. Update. The measurement depends only on the robot pose and landmark j, so its Jacobian H is non-zero only in those columns. But the Kalman gain K = Σ Hᵀ S⁻¹ is not sparse: through the correlations in Σ, every state element is corrected, and every entry of Σ changes.
  4. Repeat. As more landmarks are observed from more poses, the correlations build up and the landmark covariances shrink toward a limit set by the initial pose uncertainty (the first pose defines the frame).

What the structure teaches (convergence results)

  • The determinant of any sub-block of Σmm decreases monotonically: the map becomes more certain as observations accumulate (Dissanayake et al., 2001).
  • In the limit the landmarks become fully correlated: their relative positions become known to the accuracy of the sensors, and the remaining uncertainty is a common shift determined by the initial pose.
  • This is why SLAM works: relative information accumulates even if the absolute pose never does.

Strengths and weaknesses

StrengthWeakness / failure mode
Principled; correlations give loop-closure corrections for freeMemory O(N²) and update time O(N²) per measurement: tens of thousands of landmarks are infeasible
Online, and simple to implement for small mapsLinearisation at the current estimate is never revisited: errors introduced early stay (inconsistent filters: Julier and Uhlmann, 2001; Bailey et al., 2006)
Gives a covariance at every instantNeeds correct data association; one wrong match corrupts the joint state permanently
Works with any sensors that have a measurement modelHeading uncertainty of the robot causes overconfidence: the filter gains spurious information about orientation

A worked example: cost of the joint filter

  1. State size. A map with N = 100 landmarks has 3 + 200 = 203 state variables. Σ has 203² = 41,209 entries: about 330 KB as 64-bit floats.
  2. One update. The Kalman gain is 203 × 2 and the covariance update (I − K H) Σ touches all 41,209 entries: about 80,000 multiplications. At a 20 Hz measurement rate that is 1.6 million operations per second: trivial.
  3. Scaling. With N = 10,000 landmarks, the state has 20,003 variables and Σ has 4.0 × 10⁸ entries: 3.2 GB. One update is 2 × 4 × 10⁸ ≈ 8 × 10⁸ operations: well over a second per measurement on a typical embedded computer, and the memory does not fit.
  4. Correlation at work. Two landmarks A and B seen from the same pose, each with σ = 0.1 m, have a correlation of about 0.9 if the pose error dominates their error. Re-observing A and finding it 0.15 m to the left of expected moves B left by about 0.9 × 0.15 = 0.14 m although B was not seen. That is the mechanism by which one loop closure corrects a whole map.
  5. Without the correlations (a diagonal Σmm, as in the “independent landmarks” shortcut), the update to A would not reach B, the map would be consistent only locally, and the filter would overstate its certainty about the relative positions of A and B.

Practical filters reduce the cost: submaps (many small filters), sparse information-form filters (SEIF, which keep the information matrix approximately sparse), or particle methods that factorise the map (FastSLAM, M3-T8). Graph-based methods (Module 4) take a different route: keep the whole history and optimise.

Engineering implications and limitations

Engineering implications

  • Keep the map small: prune landmarks that have not been seen for a long time and cap the number in the filter; maintain a separate, offline-optimised global map.
  • Guard consistency. Use the NEES/NIS tests from M2-L3. If the filter is overconfident, inflate noise rather than tuning a gate to hide it.
  • Delay landmark initialisation until a landmark has been seen a few times from different positions, to avoid spurious landmarks and to get a better first estimate.
  • Protect against wrong associations: strict gates, ratio tests and joint compatibility; one bad update cannot be undone in a filter.
  • Use a good linearisation point. The EKF linearises at the current estimate; keep the heading uncertainty small (good odometry or a compass), because it is the largest source of linearisation error.

Limits

Filtering SLAM cannot revise earlier linearisations or earlier decisions. Fixed-lag smoothers and graph optimisation (Module 4) trade memory for the ability to re-linearise, and have largely replaced EKF-SLAM for large maps. The filter remains the right tool for small maps, for the front end of many systems, and for fusing inertial data (Module 6).

Common misconceptions

“Landmarks are independent given the map.”

They are correlated through the shared pose error that placed them. Ignoring this makes the filter overconfident.

“The covariance of the robot pose shrinks to zero as the map improves.”

The pose uncertainty relative to the map shrinks; with no outside reference, the absolute uncertainty is bounded below by the initial pose uncertainty.

“A bigger state is just a bigger update.”

The update touches every entry of Σ, so the cost grows with the square of the number of landmarks.

Engineering takeaways

  1. Filter SLAM carries one Gaussian over the robot pose and all landmarks, with a (3 + 2N)-dimensional state.
  2. Prediction changes only the pose block and its cross-terms; an update with one landmark changes every entry through the correlations.
  3. The correlations between landmarks are what make a single loop closure correct the entire map.
  4. Memory and time grow with N², which limits the map to hundreds or a few thousand landmarks.
  5. Linearisation errors and wrong associations cannot be undone, leading to inconsistency.

References and further reading

Primary papers and authoritative documentation.

Knowledge check

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

Question 1

In filter SLAM the robot sees landmark A, drives on and sees landmark B, then returns to A and finds it displaced from the prediction. Why can the filter also correct B?

Explanation

Because A and B were placed using the same, uncertain, robot pose: their errors are correlated, and the filter stores the correlation in the covariance. The Kalman gain spreads the correction for A to the robot and to every landmark correlated with it, including B.

Question 2

A system has 5,000 landmarks and runs an EKF-SLAM with the full covariance at 10 Hz on an embedded board. Estimate the problem.

Explanation

The state has 10,003 variables, so Σ has about 10⁸ entries (≈ 0.8 GB) and each update costs about 2 × 10⁸ operations, which at 10 Hz is 2 × 10⁹ operations per second plus memory traffic: beyond most embedded boards. Use submaps, a sparse information filter, or graph-based SLAM.

Question 3

Why is the absolute pose uncertainty of the robot in filter SLAM never reduced below its initial value without an outside reference?

Explanation

The map is anchored to the robot’s first pose, which defines the frame. All later information is relative to that, so the uncertainty of where the whole map sits cannot drop below the uncertainty assigned to the initial pose.

What’s next

The next lesson writes out the EKF-SLAM equations with their Jacobians, including how a new landmark enters the state.

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-T7 — EKF-SLAM

Derive the EKF-SLAM prediction and update, with the joint covariance structure and linearisation error.

Nothing below is required to finish this lesson. The last 3 entries are lessons of other courses on this site that teach the background or go further.