FastSLAM

This lesson formulates and implements feature-based SLAM using a Rao–Blackwellized Particle Filter (FastSLAM).

Particle representation

As in the previous lesson, particles are a set of weighted samples:

$$\mathcal{X}={\left\{⟨x^{[i]},w^{[i]}⟩\right\}}_{i=1,\dots ,N}$$

where each sample is a hypothesis about the state. For feature-based SLAM, the state is:

$$x=(x_{1:t},m_{1,x},m_{1,y},m_{2,x},m_{2,y},\dots ,m_{M,x},m_{M,y}{)}^T$$

where $x_{1:t}$ are the robot poses and $m_j$ are the landmark positions.

Dimensionality problem

Particle filters work well in low-dimensional spaces — likely regions of the state must be covered with samples. The state above is high-dimensional.

Can we exploit dependencies between dimensions of the state space?

If we know the robot poses, mapping is easy.

Key idea. Use particles only to model the robot's path. Each sample is a path hypothesis — and for each sample we can compute an individual map of landmarks.

Rao-Blackwellization

A trivial factorization that exploits dependencies between variables:

$$p(a,b)=p(b\mid a)\cdot p(a)$$

If $p(b\mid a)$ can be computed efficiently, then represent only $p(a)$ with samples and compute $p(b\mid a)$ for every sample. In our case, $a$ is the robot trajectory and $b$ is the map.

So instead of sampling $p(a,b)$ directly, we represent $p(a)$ with particles, then compute $p(b\mid a)$ — the map given the trajectory — for each particle. Because for grid maps, mapping is easy if we know the poses.

Rao-Blackwellization for SLAM

Applying the factorization to the SLAM posterior:

$$p(x_{0:t},m_{1:M}\mid z_{1:t},u_{1:t})=\underset{\text{path posterior}}{\underbrace{p(x_{0:t}\mid z_{1:t},u_{1:t})}}\cdot \underset{\text{map posterior}}{\underbrace{p(m_{1:M}\mid x_{0:t},z_{1:t})}}(1)$$

Note. First introduced for SLAM by Murphy, 1999.

Because we factor into path + map (given poses), landmarks are conditionally independent given the path:

$$p(x_{0:t},m_{1:M}\mid z_{1:t},u_{1:t})=p(x_{0:t}\mid z_{1:t},u_{1:t})\cdot \prod _{i=1}^{M}p(m_i\mid x_{0:t},z_{1:t})$$

With this factorization we use:

• MCL to estimate the robot poses.
• 2-D EKFs (one per landmark) to maintain the map.

Managing $M$ independent $2\times 2$ EKFs is far more efficient than a single joint filter over all landmarks.

Note. First exploited in FastSLAM by Montemerlo et al., 2002.

Modelling the robot's path

We use the sample-based representation for $p(x_{0:t}\mid z_{1:t},u_{1:t})$. Each sample is a path hypothesis: $x_0$ (typically $(0,0,0)$), $x_1$, $x_2$, ….

Since the particle filter does not revise past poses if a mistake is made, we don't need to maintain them in the sample set.

FastSLAM

This leads to the first efficient SLAM implementation, FastSLAM by Montemerlo et al., 2002.

• Each landmark is represented by a $2\times 2$ EKF.
• Each particle maintains $M$ independent EKFs for its landmarks.

Two updates:

• Action update. Sample a new pose using the proposal distribution (motion model).
• Sensor update. EKF on observed landmarks.

Key steps of FastSLAM 1.0

1. Extend the path posterior by sampling a new pose for each particle: $$x_t^{[k]}\sim p(x_t\mid x_{t-1}^{[k]},u_t)$$
2. Compute particle weights via the observation likelihood: $$w^{[k]}=|2\pi Q{|}^{-1/2}\exp \left(-\frac{1}{2}(z_t-{\hat{z}}^{[k]}{)}^T{Q}^{-1}(z_t-{\hat{z}}^{[k]})\right)$$
3. Update the belief of the observed landmarks with the EKF update rule.
4. Resample particles.

FastSLAM 1.0 algorithm

algorithmFastSLAM_1.0(𝒳_{t-1}, c_t, u_t, z_t)

1. for k = 1..N:# for each particle
2. let ⟨x_{t-1}^{[k]}, {⟨μ_{j,t-1}^{[k]}, Σ_{j,t-1}^{[k]}⟩}_j⟩ be particle k# unpack particle
3. x_t^{[k]} ~ p(x_t | x_{t-1}^{[k]}, u_t)# sample new pose
4. j = c_t# observed landmark id
5. if j never seen before:# if landmark j is new
6. μ_{j,t}^{[k]} = h⁻¹(z_t, x_t^{[k]})# init mean
7. H = h′(μ_{j,t}^{[k]}, x_t^{[k]})# Jacobian
8. Σ_{j,t}^{[k]} = H⁻¹ Q_t (H⁻¹)ᵀ# init covariance
9. w^{[k]} = p_0# default weight
10. else:# else (EKF update)
11. ẑ^{[k]} = h(μ_{j,t-1}^{[k]}, x_t^{[k]})# predicted measurement
12. H = h′(μ_{j,t-1}^{[k]}, x_t^{[k]})# Jacobian
13. Q = H Σ_{j,t-1}^{[k]} Hᵀ + Q_t# meas. covariance
14. K = Σ_{j,t-1}^{[k]} Hᵀ Q⁻¹# Kalman gain
15. μ_{j,t}^{[k]} = μ_{j,t-1}^{[k]} + K (z_t − ẑ^{[k]})# update mean
16. Σ_{j,t}^{[k]} = (I − K H) Σ_{j,t-1}^{[k]}# update covariance
17. w^{[k]} = |2π Q|⁻¹ᐟ² exp(−½ (z_t − ẑ^{[k]})ᵀ Q⁻¹ (z_t − ẑ^{[k]}))# importance weight
18. for all unobserved j′: keep ⟨μ_{j′}^{[k]}, Σ_{j′}^{[k]}⟩# leave unobserved unchanged
19. X_t = Resample(⟨x_t^{[k]}, …, w^{[k]}⟩_k)# resample
20. return X_t

The importance weight

As we mentioned earlier, the weight comes from the importance sampling principle:

$$w^{[k]}=\frac{\text{target}(x^{[k]})}{\text{proposal}(x^{[k]})}(2)$$

Mathematical derivation:

• Target distribution — the robot's path: $p(x_{1:t}\mid z_{1:t},u_{1:t})$
• Proposal distribution — all odometry information except the last observation: $p(x_{1:t}\mid z_{1:t-1},u_{1:t})$
• The proposal factors recursively: $$p(x_{1:t}\mid z_{1:t-1},u_{1:t})=\underset{{\mathcal{X}}_{t-1}\to {\bar{\mathcal{X}}}_t}{\underbrace{p(x_t\mid x_{t-1},u_t)}}\cdot \underset{{\mathcal{X}}_{t-1}}{\underbrace{p(x_{1:t-1}\mid z_{1:t-1},u_{1:t-1})}}$$

Plugging in:

$$\begin{array}{rl}{w}^{[k]}& =\frac{p(x_{1:t}^{[k]}\mid z_{1:t},u_{1:t})}{p(x_t^{[k]}\mid x_{t-1}^{[k]},u_t)p(x_{1:t-1}^{[k]}\mid z_{1:t-1},u_{1:t-1})}\\ & =\eta p(z_t\mid x_{1:t}^{[k]},z_{1:t-1})(4)\end{array}$$

(after applying Bayes' rule and cancelling the matching motion / past-path terms).

Integrating over the observed landmark position:

$$\begin{array}{rl}{w}^{[k]}& =\eta \int p(z_t\mid x_{1:t}^{[k]},z_{1:t-1},m_j)p(m_j\mid x_{1:t}^{[k]},z_{1:t-1})d{m}_j\\ & =\eta \int \underset{\mathcal{N}(z_t;{\hat{z}}^{[k]},Q_t)}{\underbrace{p(z_t\mid x_t^{[k]},m_j)}}\cdot \underset{\mathcal{N}(m_j;{\mu }_{j,t-1}^{[k]},{\Sigma }_{j,t-1}^{[k]})}{\underbrace{p(m_j\mid x_{1:t}^{[k]},z_{1:t-1})}}d{m}_j\end{array}$$

where:

• $p(m_j\mid x_{1:t}^{[k]},z_{1:t-1})$ is the landmark prior for particle $k$.
• $p(z_t\mid x_t^{[k]},m_j)$ is the measurement model with sensor noise $Q_t$.
• ${\hat{z}}^{[k]}\approx h(x_t^{[k]},{\mu }_{j,t-1}^{[k]})$ is the predicted measurement (first-order linearization w.r.t. the landmark).

The total measurement covariance is the projected landmark uncertainty plus the sensor noise:

$$Q=H{\Sigma }_{j,t-1}^{[k]}{H}^{\top }+Q_t$$

where $H$ is the Jacobian of the measurement model w.r.t. the landmark.

The marginal likelihood after integrating out $m_j$ is Gaussian, so:

$$w^{[k]}\approx |2\pi Q{|}^{-1/2}\exp \left(-\frac{1}{2}(z_t-{\hat{z}}^{[k]}{)}^{\top }{Q}^{-1}(z_t-{\hat{z}}^{[k]})\right)$$

Note. The weight tells us how consistent each sample's world is with what the system actually perceives.

Implementation sketch

The full FastSLAM 1.0 Pygame demo from the source notebook lives in three blocks:

1. FastSLAM core — a Rao-Blackwellized particle filter over the robot pose, where each particle carries an independent landmark map represented by tiny $2\times 2$ EKFs (position only).
2. Map and LiDAR handling.
3. Pygame demo app.

Assumptions / scope:

• Known data association — the front-end provides an integer landmark ID for each measurement; the core does not solve association.
• 2-D state — robot pose $(x,y,\theta )$; each landmark $m_j\in {\mathbb{R}}^2$.
• Measurements — range-bearing $z=(r,\phi )$ in the robot frame, with Gaussian noise $Q\in {\mathbb{R}}^{2\times 2}$.
• Motion model — odometry $(\Delta r_1,\Delta t,\Delta r_2)$ with Gaussian noise $R_x$.

Per-time-step flow:

1. Predict (particles). Sample each particle pose from the odometry proposal.
2. Correct (EKFs inside each particle).
   • Measurement model $h(x,m)$.
   • Jacobian $H=\partial h/\partial m\in {\mathbb{R}}^{2\times 2}$.
   • For a new landmark ID, initialize ${\mu }_j$ via inverse sensor model and ${\Sigma }_j$ from a local linearization.
   • For an existing landmark, run an EKF update — compute the Kalman gain via a numerically stable linear solve rather than explicit matrix inversion ($K=\Sigma H^T(H\Sigma H^T+Q{)}^{-1}$).
   • Accumulate the measurement log-likelihood to update the particle weight.
3. Normalize weights and compute the effective sample size $N_{\text{eff}}$.
4. Systematic resampling when $N_{\text{eff}}<\text{ratio}\times N$.

The full Python implementation is in notebooks/2_particle_filters/3_fast_slam.ipynb.

FastSLAM complexity

• Update robot particles based on the control: $O(N)$
• Incorporate an observation into the Kalman filters: $O(N)$
• Resample particle set: $O(N\cdot M)$

where $N$ is the number of particles and $M$ is the number of landmarks. So FastSLAM 1.0 is $O(N\cdot M)$.

From FastSLAM 1.0 to FastSLAM 2.0

FastSLAM 1.0 uses the motion model as the proposal distribution:

$$x_t^{[k]}\sim p(x_t\mid x_{t-1}^{[k]},u_t)$$

But is there a better distribution to sample from?

FastSLAM 2.0 also considers the measurements during sampling — especially useful with an accurate sensor (predictions from scan matching, compared to the motion noise). It samples from:

$$x_t^{[k]}\sim p\left(x_t\mid x_{t-1}^{[k]},u_{1:t},z_{1:t}\right)$$

The result is a more peaked proposal distribution — fewer particles required, more robust and more accurate, but more complex.

Summary

• Uses a particle filter to model the belief over robot paths.
• Factorizes the SLAM posterior into low-dimensional problems.
• Samples only the robot's path; for each particle, computes the landmarks (independent EKFs) given the path.
• Weights measure how consistent each sample's world is with what the system perceives.
• Per-particle data association.
• No robot-pose uncertainty in the per-particle data association.
• FastSLAM 1.0 and FastSLAM 2.0 differ in the proposal distribution.
• Complexity: $O(N\cdot \log M)$.
• Scales well (1 M+ features).
• Robust to data-association ambiguities.
• Advantages over the classical EKF approach (especially with nonlinearities).

---

Reading material

FastSLAM

• Thrun, Burgard, Fox. Probabilistic Robotics, §13.1–13.3, §13.8.
• Montemerlo, Thrun, Koller, Wegbreit. FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem, 2002.
• Montemerlo, Thrun. Simultaneous Localization and Mapping with Unknown Data Association Using FastSLAM, 2003.