FastSLAM

EKF-SLAM costs $O(n^2)$ per update because every landmark is correlated with every other through the shared, uncertain robot pose. FastSLAM's move is to attack that premise directly: if the robot's trajectory were known, the landmarks would be conditionally independent and each could be estimated separately. So sample the trajectory with a particle filter, and give every particle its own set of tiny independent landmark filters. The result is linear in the number of landmarks — and introduces a completely different failure mode.

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

Particle-filter fundamentals are on the Filtering page; the third approach to the same problem is GraphSLAM.

1 · Rao-Blackwellisation

1.1 The general principle

Rao-Blackwellisation is a variance-reduction technique with a simple recipe: if some variables can be integrated out analytically given the others, then sample only the rest. The estimator that marginalises analytically always has variance no greater than the one that samples everything — the Rao-Blackwell theorem. In SLAM the split is natural:

A naive particle filter over the whole SLAM state would need particles in a $3+2n$ dimensional space — utterly hopeless, since particle requirements grow exponentially with dimension. Rao-Blackwellisation reduces the sampled space to 3 dimensions per step and handles the other $2n$ in closed form.

1.2 The particle structure

Each particle carries a complete hypothesis about the world:

$$ \mathcal{P}^{[i]} = \Big\langle\; \mathbf x_t^{[i]}, \; \big(\boldsymbol\mu_1^{[i]}, \boldsymbol\Sigma_1^{[i]}\big), \; \big(\boldsymbol\mu_2^{[i]}, \boldsymbol\Sigma_2^{[i]}\big), \; \ldots, \; \big(\boldsymbol\mu_n^{[i]}, \boldsymbol\Sigma_n^{[i]}\big), \; w^{[i]} \;\Big\rangle $$

The critical point: each landmark filter is $2\times2$, not $2n \times 2n$. There are no cross-covariances between landmarks within a particle, because conditioned on that particle's trajectory there are none. That is where the $O(n^2)$ cost went.

2 · The algorithm

Step 2's weight is the likelihood of the observation under that particle's own map. A particle whose pose hypothesis explains the measurements well gains weight; one whose hypothesis is wrong loses it. That is the entire mechanism by which the trajectory posterior is shaped.

2.1 The per-landmark update

Because the pose is given for this particle, the landmark update is a textbook 2-D EKF with a $2\times2$ state:

$$ \mathbf Q_k = \mathbf H_k \boldsymbol\Sigma_k \mathbf H_k^\top + \mathbf R, \qquad \mathbf K_k = \boldsymbol\Sigma_k \mathbf H_k^\top \mathbf Q_k^{-1}, $$ $$ \boldsymbol\mu_k \leftarrow \boldsymbol\mu_k + \mathbf K_k\left(\mathbf z - \hat{\mathbf z}_k\right), \qquad \boldsymbol\Sigma_k \leftarrow \left(\mathbf I - \mathbf K_k\mathbf H_k\right)\boldsymbol\Sigma_k , $$

where $\mathbf H_k = \partial \mathbf h / \partial \boldsymbol\ell_k$ is only $2\times2$ — note that unlike EKF-SLAM there is no derivative with respect to the pose, because the pose is not being estimated here. Every matrix in sight is $2\times2$.

2.2 Complexity

The naive resampling cost is the practical bottleneck, not the update: duplicating a particle means duplicating its entire map. FastSLAM 2.0 solves this with a shared balanced binary tree over landmarks, where a resampled particle shares almost all its structure with its parent and only the modified path — $O(\log n)$ nodes — is copied. This is a persistent/immutable data structure, the same idea as structural sharing in functional programming, and it is what makes FastSLAM genuinely scale to large maps.

3 · FastSLAM 1.0 vs 2.0

The difference is the proposal distribution — where new pose samples are drawn from — and it matters a great deal.

The failure of 1.0 is worth understanding because it is a general lesson about particle filters. Sampling from the motion model scatters particles over the region odometry considers plausible. If the sensor is much more precise than the odometry, the measurement likelihood is a very narrow function, so nearly every particle lands where the likelihood is negligible. One particle gets essentially all the weight, the effective sample size collapses to 1, and the filter degenerates immediately. FastSLAM 2.0 folds the measurement into the proposal so particles are drawn where the data says they should be, producing far more uniform weights. A good proposal is worth orders of magnitude more particles.

4 · Resampling and degeneracy

4.1 Why resample at all

Without resampling, weight variance grows monotonically until one particle holds all the weight and the rest contribute nothing — all the computation is wasted on hypotheses with zero influence. The standard diagnostic is the effective sample size:

$$ M_{\text{eff}} = \frac{1}{\sum_{i=1}^{M}\left(w^{[i]}\right)^2} \;\in\; [1, M] . $$

Resample when $M_{\text{eff}}$ falls below a fraction of $M$ — commonly $M/2$. Resampling on a fixed schedule, or every step, is a mistake: it throws away diversity even when the weights are perfectly healthy.

Prefer systematic (low-variance) resampling over naive multinomial sampling: it draws a single random offset and steps through the cumulative distribution in $O(M)$, giving lower variance and a guarantee that any particle with weight above $1/M$ survives. Stratified and residual resampling are similar improvements.

4.2 Particle depletion — the defining weakness

This is precisely the problem for loop closure. Closing a large loop requires correcting poses from far back in time. But if every particle shares the same ancestor from that era, no particle carries the corrected past trajectory, and the correction is simply unrepresentable. The map cannot be fixed because the hypothesis that would fix it was discarded long ago. Compare GraphSLAM, which keeps all the constraints and can re-optimise the entire history at any time — this is the single strongest argument for the graph formulation.

Mitigations, none of which fully solve it:

• Resample only when $M_{\text{eff}}$ demands it. The cheapest and most important fix.
• Use a better proposal (FastSLAM 2.0), which keeps weights flat and so triggers resampling far less often.
• More particles. Delays the problem; does not remove it, and cost grows linearly.
• Regularisation / roughening. Add small jitter to resampled particles to restore some diversity. Statistically ad hoc but practically effective.
• Block / fixed-lag resampling. Restrict resampling to recent history so older trajectory diversity is preserved.

4.3 The sim

The simulation runs a Rao-Blackwellised filter with beacon range measurements and plots two diagnostics: the effective sample size, and the fraction of distinct ancestors remaining. Experiments:

• Set resample every 1–2 steps. The distinct-ancestor fraction crashes — depletion, visible directly.
• Make the measurement noise small while keeping odometry noise large. ESS collapses immediately: this is the FastSLAM 1.0 proposal failure of §3.
• Reduce particle count to 10–20 and watch the mean estimate lose the truth and fail to recover.

5 · The advantages that made it matter

5.1 Per-particle data association

This is FastSLAM's most underrated strength. In EKF-SLAM a single association decision is made and baked into the covariance permanently; if it is wrong, the filter is corrupted with no recourse. In FastSLAM, each particle makes its own association decision. Particles that choose correctly explain subsequent measurements well and gain weight; particles that choose wrongly are penalised and die. The filter therefore represents a distribution over associations and resolves ambiguity statistically over time rather than committing immediately. This makes it markedly more robust in cluttered maps.

5.2 Nonlinearity and multi-modality

The pose posterior is represented by samples, so it is not forced to be Gaussian. A robot that is genuinely unsure whether it is in corridor A or corridor B can carry both hypotheses, which a single-Gaussian EKF structurally cannot. Nor are there pose Jacobians in the sampled part, so the linearisation-induced inconsistency of EKF-SLAM §6 does not arise in the same way.

5.3 Where it is used

The most widely deployed descendant is grid-based rather than landmark-based: replace the per-landmark EKFs with a full occupancy grid per particle, and the measurement likelihood with a scan-matching score against that grid. This is GMapping, which was the default 2-D SLAM in ROS for many years and is still a reasonable choice for indoor LiDAR mapping. It inherits the same structure and the same depletion caveat, and the computation is embarrassingly parallel across particles — a natural GPU workload, since every particle's likelihood evaluation is independent.

6 · The comparison

Read the table as a progression of trades. FastSLAM buys linear scaling and robust association at the cost of an irreversible loss of trajectory history. GraphSLAM buys full reversibility by keeping all the measurements — paying in memory and in batch computation rather than in information.

7 · Practical checklist

• Use FastSLAM 2.0's proposal whenever the sensor is more precise than the odometry. This is the difference between working and not working.
• Resample adaptively on $M_{\text{eff}}$, never on a fixed schedule.
• Use systematic resampling, not multinomial.
• Instrument the distinct-ancestor count, not just ESS. ESS can look healthy immediately after a resampling that just destroyed the trajectory diversity.
• Use a shared tree for the maps if $n$ is large; naive map copying dominates runtime.
• Do not expect large loop closures to work. If the application needs them, pair FastSLAM with a pose graph or use the graph formulation outright.
• Parallelise across particles — the work is independent by construction.
• Do not confuse particle spread with uncertainty after heavy resampling; a depleted filter reports a small spread while being badly wrong, the same overconfidence trap as the EKF's inconsistency.

8 · Further reading

• Montemerlo, Thrun, Koller & Wegbreit (2002), "FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem," AAAI. The original.
• Montemerlo et al. (2003), "FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges," IJCAI. The improved proposal and the convergence result.
• Murphy (1999), "Bayesian Map Learning in Dynamic Environments," NIPS. The Rao-Blackwellised particle filter applied to mapping, predating FastSLAM.
• Doucet, de Freitas, Murphy & Russell (2000), "Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks," UAI. The general statistical framework.
• Thrun, Burgard & Fox, Probabilistic Robotics, chapter 13. The clearest textbook treatment, including the tree-based map representation.
• Grisetti, Stachniss & Burgard (2007), "Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters," IEEE T-RO 23(1). The GMapping paper — adaptive resampling and improved proposals in practice.
• Doucet & Johansen (2009), "A Tutorial on Particle Filtering and Smoothing: Fifteen Years Later." The best reference on proposals, degeneracy and resampling schemes.
• Bailey, Nieto & Nebot (2006), "Consistency of the FastSLAM Algorithm," ICRA. Shows that FastSLAM is also eventually inconsistent, through depletion rather than linearisation.
• Cadena et al. (2016), "Past, Present, and Future of Simultaneous Localization and Mapping," IEEE T-RO.

Related pages: EKF-SLAM · GraphSLAM & pose graphs · Filtering: KF · EKF · UKF · PF · ICP & registration · LiDAR