EKF-SLAM

The first solution to simultaneous localisation and mapping, and still the clearest way to understand what makes the problem hard. Put the robot pose and every landmark into one state vector, carry one joint covariance, and run an extended Kalman filter. The insight that made SLAM a solved problem in principle is that the off-diagonal blocks — the correlations between landmarks — are not a nuisance but the entire mechanism by which a single observation can correct the whole map.

Symbols used on this page are defined in the Nomenclature → estimation & filtering.

The EKF itself is derived on the Extended Kalman filter page; this page is about what happens when the state grows with the map. FastSLAM and GraphSLAM are the two reactions to its cost.

The central result, due to Dissanayake et al. (2001). As the robot keeps observing, the map converges — landmark uncertainties decrease monotonically toward a lower bound set by the initial pose uncertainty, and in the limit the relative map becomes perfectly known. Along the way, the landmarks become fully correlated. That correlation is what makes SLAM work; discarding it to save computation is what makes a filter overconfident and eventually divergent.

1 · The augmented state

1.1 Structure

The state stacks the robot pose with all $n$ landmark positions. In 2-D, with a 3-DoF pose and point landmarks:

$$ \mathbf x = \begin{bmatrix} \mathbf x_r \\ \mathbf m \end{bmatrix} = \begin{bmatrix} x_r & y_r & \theta_r & \ell_{1x} & \ell_{1y} & \cdots & \ell_{nx} & \ell_{ny}\end{bmatrix}^\top \in \mathbb{R}^{3+2n} , $$ $$ \mathbf P = \begin{bmatrix} \mathbf P_{rr} & \mathbf P_{rm} \\ \mathbf P_{rm}^\top & \mathbf P_{mm} \end{bmatrix} . $$

Block	Meaning	Why it matters
$\mathbf P_{rr}$	pose uncertainty	Grows on motion, shrinks on observation
$\mathbf P_{mm}$	landmark uncertainties and their cross-covariances	The diagonal blocks shrink; the off-diagonal blocks are the map's internal consistency
$\mathbf P_{rm}$	pose–landmark correlation	The channel through which observing one landmark corrects the pose and every other landmark

Dimensions matter enormously here: a 3-DoF pose with 1000 point landmarks gives a state of 2003 and a covariance of $2003^2 \approx 4\times10^6$ entries. In 3-D with a 6-DoF pose it is worse. This is the cost problem that dominates the rest of the page.

1.2 Where the correlations come from

Landmarks are static, so nothing in the physics couples them. They become correlated because they are observed from a common, uncertain pose. If the robot's position is uncertain by some amount and it measures two landmarks from there, both landmark estimates inherit that same error — so their errors are shared, not independent.

The payoff comes later. Suppose the robot drives away, accumulating odometry drift, and then re-observes landmark A, sharply correcting its own pose. Because landmark B is correlated with the pose, the same update also corrects B — even though B was not observed. That is loop closure in the EKF, and it is purely a consequence of the off-diagonal blocks being maintained honestly.

2 · The cycle

2.1 Prediction

Only the robot moves, which gives the prediction step a very useful structure. With motion model $\mathbf x_r \leftarrow g(\mathbf x_r, \mathbf u)$ and $\mathbf G_r = \partial g/\partial\mathbf x_r$:

$$ \mathbf P^- = \begin{bmatrix} \mathbf G_r \mathbf P_{rr}\mathbf G_r^\top + \mathbf Q & \mathbf G_r \mathbf P_{rm} \\ \mathbf P_{rm}^\top \mathbf G_r^\top & \mathbf P_{mm} \end{bmatrix} . $$

Note what does not change: $\mathbf P_{mm}$ is untouched, because landmarks are static and the process noise does not reach them. So prediction costs $O(n)$, not $O(n^2)$ — the expensive $\mathbf P_{rm}$ product is a $3\times3$ times $3\times2n$, linear in the map size. Prediction is cheap; the update is not.

2.2 Observation model

A range-bearing sensor observing landmark $j$ predicts, with $\Delta x = \ell_{jx} - x_r$, $\Delta y = \ell_{jy} - y_r$, $q = \Delta x^2 + \Delta y^2$:

$$ \hat{\mathbf z}_j = \begin{bmatrix} \sqrt{q} \\ \operatorname{atan2}(\Delta y, \Delta x) - \theta_r \end{bmatrix} . $$

The Jacobian is the key to the whole algorithm's efficiency, because it is extremely sparse — it is nonzero only in the 3 pose columns and the 2 columns of landmark $j$:

$$ \mathbf H_j = \frac{\partial \mathbf h}{\partial \mathbf x} = \begin{bmatrix} -\dfrac{\Delta x}{\sqrt q} & -\dfrac{\Delta y}{\sqrt q} & 0 & \cdots & \dfrac{\Delta x}{\sqrt q} & \dfrac{\Delta y}{\sqrt q} & \cdots \\[3mm] \dfrac{\Delta y}{q} & -\dfrac{\Delta x}{q} & -1 & \cdots & -\dfrac{\Delta y}{q} & \dfrac{\Delta x}{q} & \cdots \end{bmatrix} . $$

A naive implementation that forms the full $2 \times (3+2n)$ matrix and multiplies densely wastes almost all its work on structural zeros. Every competent implementation exploits the sparsity by extracting the relevant $2\times5$ sub-block.

2.3 Update

Standard EKF, but note where the cost lands:

$$ \mathbf S_j = \mathbf H_j \mathbf P^- \mathbf H_j^\top + \mathbf R, \qquad \mathbf K_j = \mathbf P^- \mathbf H_j^\top \mathbf S_j^{-1}, $$ $$ \mathbf x^+ = \mathbf x^- + \mathbf K_j\left(\mathbf z_j - \hat{\mathbf z}_j\right), \qquad \mathbf P^+ = \left(\mathbf I - \mathbf K_j \mathbf H_j\right)\mathbf P^- . $$

Because $\mathbf H_j$ is sparse, $\mathbf S_j$ is only $2\times2$ — a trivial inverse. But $\mathbf K_j = \mathbf P^-\mathbf H_j^\top\mathbf S_j^{-1}$ is a full-height column block: it has an entry for every state. So the correction touches every landmark in the map, and the covariance update is $O(n^2)$. That is both the strength (the whole map gets corrected) and the fatal weakness (the cost).

Use the Joseph form $\mathbf P^+ = (\mathbf I - \mathbf K\mathbf H)\mathbf P^-(\mathbf I - \mathbf K\mathbf H)^\top + \mathbf K\mathbf R\mathbf K^\top$ in practice: it is symmetric by construction and far better behaved numerically over thousands of updates. The short form gradually loses symmetry and can lose positive-definiteness, a failure discussed on the EKF page.

3 · Landmark initialisation

A newly seen landmark must be added to the state — and doing it correctly is what preserves consistency. The inverse observation model places it:

$$ \hat{\boldsymbol\ell}_{n+1} = \begin{bmatrix} x_r + r\cos(\phi + \theta_r) \\ y_r + r\sin(\phi + \theta_r)\end{bmatrix} , $$

and the covariance must be augmented by propagating both the pose uncertainty and the measurement noise through the Jacobians $\mathbf J_r = \partial \boldsymbol\ell/\partial\mathbf x_r$ and $\mathbf J_z = \partial\boldsymbol\ell/\partial\mathbf z$:

$$ \mathbf P_{\ell\ell} = \mathbf J_r \mathbf P_{rr}\mathbf J_r^\top + \mathbf J_z \mathbf R \mathbf J_z^\top , \qquad \mathbf P_{\ell,\,\text{all}} = \mathbf J_r\, \begin{bmatrix}\mathbf P_{rr} & \mathbf P_{rm}\end{bmatrix} . $$

The new landmark must be correlated with everything immediately. The cross-covariance term $\mathbf P_{\ell,\text{all}}$ is not optional. A new landmark's position is computed from the current pose, so its error contains the pose error, which is already correlated with every existing landmark. Initialising it as independent — a very common shortcut — injects information the filter does not have, and the result is an overconfident covariance that never recovers. The filter will then reject good measurements as outliers and diverge.

A related decision is delayed initialisation. A single range-bearing measurement fully determines a 2-D point, but a bearing-only sensor (a camera) does not — it leaves depth unobservable, exactly as described on the SfM page. In that case the landmark must either be held in a provisional list until enough parallax accumulates, or parameterised in inverse depth so the linearisation stays valid.

4 · Data association

4.1 The gating test

Before any update, the filter must decide which landmark a measurement belongs to. The standard tool is the squared Mahalanobis distance of the innovation:

$$ d^2_{ij} = \boldsymbol\nu_{ij}^\top \mathbf S_{ij}^{-1}\boldsymbol\nu_{ij} , \qquad \boldsymbol\nu_{ij} = \mathbf z_i - \hat{\mathbf z}_j , $$

which under correct association is $\chi^2$ with $\dim(\mathbf z)$ degrees of freedom. Accept if $d^2 \lt \chi^2_{\nu,1-\alpha}$ — for a 2-D measurement at 99%, the gate is 9.21. Note that $\mathbf S$ includes the map uncertainty, so the gate automatically widens when the robot is lost and tightens when it is confident. That is the right behaviour and it comes for free.

4.2 Why nearest-neighbour is not enough

Method	Decision	Cost	Robustness
Nearest neighbour	closest gated landmark, independently per measurement	$O(nm)$	Poor when landmarks are dense or pose error is large
Joint compatibility (JCBB)	largest jointly consistent set of assignments	branch and bound	Much better — exploits the correlations
Multi-hypothesis	maintain several association hypotheses	expensive	Best, but the hypothesis tree must be pruned
Appearance-based	descriptor matching, then geometric verification	cheap	Sidesteps the geometry problem — the modern approach

JCBB's insight is worth stating because it is the same theme as §1.2: individual gating tests treat each assignment separately, but the predicted measurements are correlated through the shared pose. A set of assignments that are each individually plausible can be jointly impossible — the implied pose corrections contradict each other. Testing joint compatibility rejects those combinations and dramatically reduces false associations in cluttered maps.

A wrong association is unrecoverable. This is the sharpest practical difference between EKF-SLAM and the graph-based methods. The EKF fuses the measurement into the covariance and discards the measurement; there is no record from which to undo it. One confident bad association corrupts the pose, which corrupts the gating for subsequent measurements, which produces more bad associations — a positive feedback loop ending in divergence. Pose graphs keep the raw constraints and can therefore afford robust kernels and even edge removal; the EKF cannot.

5 · Complexity

Operation	Cost	Comment
Prediction	$O(n)$	Only the pose row/column changes
Single update	$O(n^2)$	The Kalman gain is full height
Memory	$O(n^2)$	The dense covariance
Data association	$O(n)$ to $O(n^2)$	Depends on the method

Landmarks $n$	State dim (2-D)	Covariance entries	Memory (double)
100	203	41 209	0.3 MB
1 000	2 003	4.0 M	32 MB
10 000	20 003	400 M	3.2 GB
100 000	200 003	40 G	320 GB

The practical ceiling is a few hundred to a couple of thousand landmarks. That is a small room, not a building — and it is the reason EKF-SLAM, despite being correct, was superseded for anything at scale. The standard mitigations are submapping (many small local maps with a coarse graph between them), sparse information-form filters, and simply accepting a bounded map with landmark culling.

5.1 The sim

The simulation runs a real EKF-SLAM — full state augmentation, sparse Jacobians, range-bearing updates — and draws the covariance matrix as a heat map beside the map. What to watch:

The off-diagonal blocks filling in. Landmarks start uncorrelated and become mutually correlated through the shared pose; the cross-correlation percentage climbs as the run proceeds.
Landmark ellipses shrinking as they are re-observed — the monotone map convergence described at the top of this page.
The state dimension and the $O(n^2)$ update cost growing as new landmarks enter.
Raising the odometry noise: the pose block inflates between observations, which widens the gates and makes the correlations build faster.

6 · Consistency and the linearisation problem

6.1 What inconsistency means

A filter is consistent if its reported covariance honestly reflects its actual error — formally, if the normalised estimation error squared matches its expected $\chi^2$ statistics. EKF-SLAM is known to become inconsistent: it reports a covariance smaller than its true error, i.e. it becomes overconfident. This is dangerous precisely because it is invisible from the inside; the filter's own uncertainty estimate says everything is fine.

6.2 Why

Two mechanisms, both structural rather than implementation bugs:

Linearisation about a wrong estimate. The Jacobians are evaluated at the current estimate, not the truth. Heading error is the worst offender: an error in $\theta_r$ rotates the whole predicted measurement geometry, so the Jacobian itself is wrong, and the resulting covariance update is not a valid bound. Errors accumulate in a biased direction rather than averaging out.
Spurious information gain. Because linearisation points differ between the prediction and update of the same quantity, the filter can manufacture information about directions that are genuinely unobservable. SLAM has three unobservable DoF in 2-D (the global position and heading of the whole map — the same gauge freedom as in bundle adjustment §5). A consistent filter must keep infinite uncertainty in those directions; the EKF slowly and incorrectly becomes certain about them.

6.3 Mitigations

Approach	Idea	Notes
Iterated EKF	re-linearise about the updated estimate and repeat	Cheap, helps meaningfully when innovations are large
First-estimates Jacobian (FEJ)	always linearise about the first available estimate of each state	Preserves the correct observability structure; a well-established fix, also used in VINS
Observability-constrained EKF	project the update to respect the unobservable directions	Directly addresses the spurious-information mechanism
Robocentric formulation	keep the map in the robot frame	Keeps heading errors small, so linearisation stays valid
UKF / sigma-point	avoid explicit Jacobians	Helps with linearisation but not with the $O(n^2)$ cost; see Filtering
Inflate $\mathbf Q$	add artificial process noise	The pragmatic hack. Buys consistency at the price of accuracy — and it is tuning, not a fix

7 · When EKF-SLAM is still the right choice

It is not obsolete — it is bounded. It remains a good fit when:

The map is genuinely small and bounded: a single room, a dock, a known set of beacons, a few dozen features on a target spacecraft.
A recursive, constant-time-per-step estimate is required — no batch optimisation, no growing history, predictable timing. This is a real advantage for embedded and safety-critical systems, where bounded worst-case execution matters more than scalability.
An explicit, up-to-date covariance is needed at every step for gating, fault detection or control. The EKF gives this naturally; a graph optimiser must do extra work to recover it.
The landmarks are well separated and unambiguous, so data association is reliable.

Conversely, for a large environment, for a system that must survive occasional bad associations, or where loop closures arrive long after the fact, the graph formulation is simply better — it keeps the measurements, re-linearises everything, and can reject a bad edge after the fact.

8 · Practical checklist

Correlate new landmarks on initialisation (§3). The most consequential correctness bug.
Exploit Jacobian sparsity. Never multiply the full $\mathbf H$ densely.
Use the Joseph form and re-symmetrise $\mathbf P \leftarrow (\mathbf P + \mathbf P^\top)/2$ periodically.
Gate with the $\chi^2$ test, and prefer joint compatibility over per-measurement nearest neighbour in cluttered maps.
Delay initialisation for bearing-only sensors until parallax suffices, or use inverse depth.
Monitor consistency with the normalised innovation squared, averaged over time. A running value persistently below its expectation means the filter is overconfident.
Cull landmarks that are rarely re-observed; every one costs $O(n)$ forever.
Consider FEJ or a robocentric formulation if consistency matters over long runs.
Bound the map size by design, and switch to submaps rather than letting $n$ grow unboundedly.

9 · Further reading

Smith, Self & Cheeseman (1990), "Estimating Uncertain Spatial Relationships in Robotics." The paper that introduced the joint state and the correlations.
Dissanayake et al. (2001), "A Solution to the Simultaneous Localization and Map Building (SLAM) Problem," IEEE T-RA 17(3). The convergence proofs and the lower bound on map uncertainty.
Durrant-Whyte & Bailey (2006), "Simultaneous Localization and Mapping: Part I," IEEE RAM 13(2). The best short introduction; part II covers recursive methods and data association.
Thrun, Burgard & Fox, Probabilistic Robotics, chapter 10. The clearest textbook derivation, with the full algorithm in pseudocode.
Neira & Tardós (2001), "Data Association in Stochastic Mapping Using the Joint Compatibility Test," IEEE T-RA. JCBB.
Julier & Uhlmann (2001), "A Counter Example to the Theory of Simultaneous Localization and Map Building." The paper that first demonstrated EKF-SLAM inconsistency.
Bailey et al. (2006), "Consistency of the EKF-SLAM Algorithm," IROS. A careful empirical study of how and when it goes wrong.
Huang, Mourikis & Roumeliotis (2010), "Observability-based Rules for Designing Consistent EKF SLAM Estimators," IJRR. The FEJ idea and the observability analysis.
Castellanos, Martínez-Cantín, Tardós & Neira (2007), "Robocentric map joining: Improving the consistency of EKF-SLAM."
Cadena et al. (2016), "Past, Present, and Future of Simultaneous Localization and Mapping," IEEE T-RO. The definitive survey placing all three approaches in context.