FastSLAM and Rao-Blackwellization

Don't estimate the joint. Sample the trajectory, solve every map in closed form, and let loop closure be natural selection among competing universes.

The map that never existed

Here is the naive thing, stated precisely enough to see it fail. The SLAM state is the pose plus the map, $(x_t, m)$. A particle filter represents a belief by samples. So: sample the whole state. Each particle is a pose and a complete map, weighted by how well it explains the scan.

Count the dimensions. Rusty's pose is 3. Twenty-four landmarks in the plane are 48 more. That is a 51-dimensional space, and the number of samples needed to cover a space to fixed resolution grows exponentially in its dimension — the curse of dimensionality from Chapter 8, arriving on schedule. Ten samples per axis would require $10^{51}$ particles. At a billion particles per second, drawing that population once takes about $10^{42}$ seconds, which is some $10^{24}$ times the current age of the universe. You get to do it at 10 Hz.

Worse, it is wasteful in a way that has nothing to do with dimension. Conditioned on a trajectory, the map has a closed-form posterior — it is a product of Gaussians, or a grid of log-odds counters. Sampling something you could have computed is throwing away accuracy for nothing, and there is a theorem later in this chapter that says exactly how much.

So here is the trade. Sample only the part that is genuinely hard — the path — and compute everything else. What the reader sees below is $M$ separate universes, each of which took a different guess about where Rusty went, and each of which built the map that guess implies.

Watch it for a lap before reading on. Three things are worth your attention, and each one is a section of this chapter.

There is no map. There are sixteen maps. They disagree, and the disagreement is the belief. Any "the map" you extract — the highest-weight tile, the weighted average, whatever — is a summary you chose, not a thing the filter holds. The reader who has been waiting since Chapter 13 for mapping and localization to be solved together should notice that this is what together actually looks like.

Weights are the referee, and the map is the referee's rulebook. Each universe scores the new scan against its own walls. A universe that drifted left builds walls that are shifted left, and its future scans keep agreeing with it — until the robot comes back to somewhere it has been, at which point the accumulated inconsistency has nowhere to hide, and the green border fades.

Loop closure is an extinction event, not an optimization step. In Chapter 16 closing a loop meant adding a constraint and running Gauss–Newton until the graph relaxed. Here it means some universes stop being paid and get deleted. No optimizer runs. Nothing is refined. The filter simply stops simulating the futures that turned out to be wrong.

If you knew the path

The intuition that unlocks all of this was already said out loud, twice, in earlier chapters.

Chapter 13 assumed known poses and built a map by counting. Chapter 11 assumed a known map and tracked a pose with an EKF. Each was easy; SLAM is hard because neither assumption holds. The Rao-Blackwellized move is to make one of them hold — not by knowing the path, but by conditioning on a guess about it, and then keeping many guesses around so that the conditioning is honest.

Formally, conditioning on the path does something stronger than making mapping convenient. It makes the landmarks independent of one another.

Look at the graphical model with the path marginalized out. Landmark $m_1$ was seen from poses $x_0, x_1, x_2$; landmark $m_3$ was seen from $x_3, x_4, x_5$. They were never observed together, and yet integrating over the trajectory makes them correlated: any evidence that shifts your belief about $x_2$ shifts $x_3$ through the motion model, which shifts $m_3$. Those induced couplings are precisely the off-diagonal blocks that made Chapter 14's covariance dense and its update $O(N^2)$.

Now condition on the path — click the toggle. The pose nodes become observed, the induced arcs vanish, and what is left is three separate little estimation problems that cannot talk to each other. Each is a $2 \times 2$ Gaussian updated by its own sightings.

Two consequences follow immediately, and both are worth stating before the algebra.

The first is a cost argument. Chapter 14's update touched every entry of a $(3+2N) \times (3+2N)$ covariance. Here, observing one landmark updates one $2 \times 2$ matrix inside one particle, and touches nothing else — $O(M)$ per observation regardless of how large the map has grown.

The second is a representational argument, and it is the one that survived into 2026. Because each particle owns its map, each particle may also own its data association. Universe 3 can believe the doorway it just saw is landmark 7 while universe 9 believes it is a new landmark, and neither has to commit for everyone. The correspondence variable $c_t$, which Chapter 11 had to guess once and live with forever, becomes a sampled variable like any other.

The mathematics

Notation

The SLAM factorization theorem

Everything rests on one identity. With known correspondences $c_{1:t}$,

Read it right to left. The product on the right is $N$ independent two-dimensional Gaussians, each of which a Kalman filter can maintain in closed form. The factor on the left is a distribution over trajectories, which is exactly the kind of thing a particle filter is good at. The theorem says this split is exact — no approximation has been made yet.

Rao-Blackwellization is a theorem, not a trick

Why should sampling paths and computing maps beat sampling both? Because of a variance inequality that predates robotics by fifty years.

Let $\varphi(X, Y)$ be any estimator of a quantity of interest, where $X$ is what we sample and $Y$ is what we could integrate out. The Rao–Blackwell estimator is the conditional expectation $\E[\varphi \mid X]$, and it satisfies

Identify $X$ with the path and $Y$ with the map, and the statement becomes: a filter that samples trajectories and computes map posteriors is never worse, in variance, than one that samples both — and is strictly better whenever the map genuinely varies given the path.

The FastSLAM 1.0 weight

Now make it concrete. Particle $i$ holds a pose $x_{t-1}^{[i]}$ and, for each landmark, a Gaussian $\Normal(\mu_{j}^{[i]}, \Sigma_{j}^{[i]})$. It draws a new pose from the motion model,

using sample_motion_model_odometry from Chapter 9 — the proposal is the motion prior, which is the same choice Monte Carlo localization made in Chapter 12, for the same reason: everything cancels except the measurement likelihood. Here that likelihood must be marginal over the landmark, and the result is

That $\mat{S}_t$ should look familiar: it is the innovation covariance from Chapter 11, the same matrix that defines the Mahalanobis gate. Here it does double duty — it weights the particle and gates its data association.

Why 1.0 wastes particles, and what 2.0 does about it

FastSLAM 1.0 proposes from the motion model and then judges the result with the measurement. When the wheels are vague and the sensor is precise — which describes every robot built since about 1995 — this is a bad plan. The proposal spreads samples over a region tens of centimetres wide; the likelihood concentrates them in a region a few centimetres wide; and almost every particle lands somewhere the measurement considers absurd.

The counters underneath make the cost quantitative. At the default $\sigma_r = 5$ cm the motion-prior proposal puts about 43% of its samples inside the posterior's 90% region and lands an effective sample size of $N_{\mathrm{eff}}/M \approx 0.18$; the measurement-aware proposal puts exactly 90% of them there — as it must, since it is the posterior — with $N_{\mathrm{eff}}/M = 1.00$. In round numbers, one FastSLAM 2.0 particle is doing the work of five of FastSLAM 1.0's, and the ratio widens as the sensor sharpens: at $\sigma_r = 2$ cm it is six, at $\sigma_r = 40$ cm it is under two.

The fix is to fold $z_t$ into the proposal itself:

This is the optimal proposal in Doucet's sense — among proposals that use the information available at time $t$, it minimizes the variance of the importance weights. Linearizing the measurement in the pose makes it a Gaussian you can compute with one EKF-shaped step.

Grids: the gmapping recipe

Landmarks are a luxury. In a bare corridor there is nothing to call a landmark and the map is an occupancy grid, so the closed forms of the last section evaporate: there is no $\mat{H}_m$, no $\Sigma_j$, no integral you can do by hand.

Grisetti, Stachniss and Burgard's answer — the algorithm the world knew as gmapping for fifteen years — is to keep the shape of FastSLAM 2.0 and replace algebra with local sampling.

1. Find the mode. Scan-match $z_t$ against the particle's own map $m^{[i]}_{t-1}$, starting from the odometry-predicted pose, giving $\hat x_t^{[i]}$. Any matcher from Chapter 16 does; ours is a greedy hill climb on the map-match score, which is what gmapping itself used.
2. Probe around it. Draw $K$ poses $\{x_k\}$ in a small neighbourhood of $\hat x_t^{[i]}$.
3. Weight the probes by observation × motion, $\;\omega_k = p(z_t \mid m^{[i]}, x_k)\, p(x_k \mid x_{t-1}^{[i]}, u_t)$.
4. Fit and sample. Compute the weighted mean and covariance of the probes, and draw the new pose from that Gaussian. This is Step 3 of the previous derivation, done numerically.
5. Pay the normalizer. Multiply the particle's weight by $\eta^{[i]} = \sum_k \omega_k$ — the Monte Carlo estimate of the same predictive likelihood integral that FastSLAM 2.0 evaluated in closed form.
6. Map, then select. Integrate the scan into $m^{[i]}$ at the sampled pose with Chapter 13's log-odds update, and resample only if $N_{\mathrm{eff}} < M/2$.

Two details in that list are load-bearing, and both are visible in the widgets.

The scan matcher must be allowed to fail. On a fresh map the score surface is flat — every pose explains an unexplored grid equally well — and the "optimum" it returns is numerical noise. The implementation compares the matched score against the seed score and falls back to plain motion sampling when the improvement is not real. Without that guard, the filter confidently follows garbage for the first ten steps and never recovers.

Resampling is a cost, not a service. Chapter 8 said this about localization; here it decides whether there are still competing universes left when the loop finally closes.

The three policies expose two different failure modes, and the instructive part is that the meter everyone watches only sees one of them.

Never resample and the weights degenerate: $N_{\mathrm{eff}}/M$ slides to about $0.1$, meaning that of sixteen universes being simulated and paid for, one holds essentially all the probability. The filter has quietly become a single-hypothesis estimator with a fifteen-fold overhead, and the error curve shows it — depending on the seed, the lap error grows by anywhere from 20% to a factor of three.

Resample every step and $N_{\mathrm{eff}}$ looks fine forever, because the weights are wiped clean each step and the statistic only ever reports one step of evidence. What dies instead is ancestry: the lineage gauge falls to 1, meaning every surviving universe is a copy of one great-grandparent and the population can no longer disagree about anything. This is Chapter 8's particle deprivation wearing a different hat, and $N_{\mathrm{eff}}$ is structurally blind to it.

Resample when $N_{\mathrm{eff}} < M/2$ buys the middle — and be honest about how little it buys here. Even the selective policy fires on roughly half the steps of this lap, and the lineage gauge still collapses inside about twenty steps; on error alone the always-resample policy is indistinguishable from it on this benign out-and-back. The difference shows up in the cases this lap does not contain: an ambiguous junction, a symmetric wing, a loop closed after ten minutes of drift, where the hypothesis that turns out to be right must still exist when the evidence arrives. Selective resampling slows the loss of diversity; nothing on that panel restores it, which is why the recovery machinery of Chapter 12 is not optional in a deployed system.

The algorithms

FastSLAM 2.0 changes exactly two lines. Line 3 samples from the measurement-aware proposal, and line 9 uses the predictive covariance $\mat{H}_x \mat{R}_t \mat{H}_x\T + \mat{Q}_j$ instead of $\mat{S}$. Everything else — the per-landmark EKF, the untouched landmarks, the resampler — is identical.

Line 8 is the line that matters, and it is worth pausing on. The maximum-likelihood association is computed per particle, against that particle's own map and its own pose. Different particles reach different conclusions, and the ambiguity is resolved not by a decision but by survival: whichever association was right leads to consistent future scans and its universe multiplies. Chapter 11's EKF had to pick one and could never take it back.

Implementation in Rust

Before the code, one observation about the dependency list. This chapter uses nalgebra for $2\times2$ and $3\times3$ blocks, rand + rand_distr with a seeded Pcg64, statrs in the tests to cross-check the Gaussian densities, and rayon behind a native-only feature for the per-particle loop. What it does not use is faer or factrs — and that absence is the architectural point. Chapter 15 needed a sparse linear solver because it was inverting one enormous coupled system. Here there is no system to invert: the factorization theorem has already broken the problem into $M \times N$ pieces of size 2. The largest matrix anyone factors in this entire chapter is $3 \times 3$.

Three types carry the rest: a landmark's EKF, a particle that owns a path and a bank of them, and a copy-on-write grid for the map-based variant.

use nalgebra::{Matrix2, Matrix2x3, Vector2};
use pr_core::geom::SE2;

/// One landmark's posterior inside one particle: a 2-D Gaussian, and nothing
/// else. There is no pose block and no cross-covariance with any other
/// landmark, because conditioned on this particle's path there is nothing for
/// them to correlate through. That absence *is* the factorization theorem.
#[derive(Clone, Debug)]
pub struct LandmarkEkf {
    pub mu: Vector2<f64>,
    pub sigma: Matrix2<f64>,
    /// Sightings so far — FastSLAM's cheap stand-in for existence evidence.
    pub hits: u32,
}

/// A range–bearing feature, Thrun's f = (r, φ, s) without the signature.
#[derive(Clone, Copy, Debug)]
pub struct Feature {
    pub r: f64,
    pub phi: f64,
}

impl LandmarkEkf {
    /// Initialize from a first sighting: invert the measurement, then push the
    /// sensor covariance through the inverse Jacobian.
    ///
    /// Σ carries *no* pose uncertainty. Inside this particle the pose is known
    /// exactly — that is the whole point of sampling the path.
    pub fn from_first_sighting(x: &SE2, z: Feature, q: &Matrix2<f64>) -> Self {
        let bearing = x.theta() + z.phi;
        let mu = Vector2::new(x.x() + z.r * bearing.cos(), x.y() + z.r * bearing.sin());
        let h = landmark_jacobian(x, &mu);
        let h_inv = h.try_inverse().expect("range–bearing Jacobian is singular only at r = 0");
        Self { mu, sigma: h_inv * q * h_inv.transpose(), hits: 1 }
    }

    /// Innovation ν and its covariance S = H Σ Hᵀ + Q — Chapter 11's gate,
    /// unchanged, and the one matrix this chapter uses for three jobs at once:
    /// weighting, gating, and the Kalman gain.
    pub fn innovation(&self, x: &SE2, z: Feature, q: &Matrix2<f64>) -> (Vector2<f64>, Matrix2<f64>) {
        let (r_hat, phi_hat) = predict(x, &self.mu);
        let h = landmark_jacobian(x, &self.mu);
        let nu = Vector2::new(z.r - r_hat, wrap_angle(z.phi - phi_hat));
        (nu, h * self.sigma * h.transpose() + q)
    }

    /// FastSLAM 1.0's importance weight: the measurement's *marginal*
    /// likelihood, with the landmark integrated out (Derivation 3).
    pub fn weight_1_0(&self, x: &SE2, z: Feature, q: &Matrix2<f64>) -> f64 {
        let (nu, s) = self.innovation(x, z, q);
        let s_inv = s.try_inverse().expect("S is positive definite whenever Q is");
        let det = (std::f64::consts::TAU * s).determinant();
        det.powf(-0.5) * (-0.5 * (nu.transpose() * s_inv * nu)[0]).exp()
    }

    /// Standard EKF correction of this landmark at the sampled pose.
    pub fn update(&mut self, x: &SE2, z: Feature, q: &Matrix2<f64>) {
        let (nu, s) = self.innovation(x, z, q);
        let h = landmark_jacobian(x, &self.mu);
        let k = self.sigma * h.transpose() * s.try_inverse().unwrap();
        self.mu += k * nu;
        self.sigma = (Matrix2::identity() - k * h) * self.sigma;
        self.hits += 1;
    }
}

/// ∂h/∂m for the range–bearing model.
fn landmark_jacobian(x: &SE2, mu: &Vector2<f64>) -> Matrix2<f64> {
    let d = Vector2::new(mu.x - x.x(), mu.y - x.y());
    let q = d.norm_squared().max(1e-12);
    let r = q.sqrt();
    Matrix2::new(d.x / r, d.y / r, -d.y / q, d.x / q)
}

/// ∂h/∂x. Only FastSLAM 2.0 needs this one — 1.0 never differentiates the
/// measurement with respect to the pose, which is precisely its weakness.
fn pose_jacobian(x: &SE2, mu: &Vector2<f64>) -> Matrix2x3<f64> {
    let d = Vector2::new(mu.x - x.x(), mu.y - x.y());
    let q = d.norm_squared().max(1e-12);
    let r = q.sqrt();
    Matrix2x3::new(-d.x / r, -d.y / r, 0.0, d.y / q, -d.x / q, -1.0)
}

The particle is where the theorem becomes a data structure. Note the path field: a particle is a trajectory, and the reason it must be is in the collapsible proof above.

use nalgebra::{Matrix2, Matrix3, Vector2, Vector3};
use rand::SeedableRng;
use rand_distr::{Distribution, StandardNormal};
use rand_pcg::Pcg64;
use pr_core::geom::SE2;
use pr_core::models::OdomDelta;

pub struct FsParticle {
    pub pose: SE2,
    /// The particle IS a path. Dropping this field would break the
    /// factorization theorem, not merely lose a visualization.
    pub path: Vec<SE2>,
    pub landmarks: Vec<LandmarkEkf>,
    pub log_w: f64,
}

/// FastSLAM 1.0 vs 2.0, as a choice of proposal rather than a fork of the code.
#[derive(Clone, Copy, PartialEq, Eq, Debug)]
pub enum Proposal {
    MotionPrior,
    MeasurementAware,
}

pub struct FastSlam {
    particles: Vec<FsParticle>,
    proposal: Proposal,
    /// Measurement noise Q = diag(σ_r², σ_φ²).
    q: Matrix2<f64>,
    /// Chapter 9's odometry α's, used for both the sampler and R_t.
    alphas: [f64; 4],
    /// p₀ from `FastSLAM_unknown_correspondence`: the likelihood credited to a
    /// brand-new landmark, and therefore the bar an old one has to clear.
    p0: f64,
    rng: Pcg64,
}

impl FastSlam {
    pub fn step(&mut self, u: &OdomDelta, z: &[Feature], c: Option<&[usize]>) -> FsReport {
        for p in &mut self.particles {
            let r = odometry_pose_covariance(&p.pose, u, &self.alphas);
            // 2.0 folds one already-mapped feature into the proposal. An
            // unmapped one carries no pose information, so there is nothing to
            // fold and we fall back to 1.0's motion prior for this step.
            let anchor = (self.proposal == Proposal::MeasurementAware)
                .then(|| z.first().and_then(|f| self.associate(p, u, f, c)))
                .flatten();

            let pose = match anchor {
                Some(j) => {
                    let (x, log_w) = self.propose_2_0(p, u, &r, z[0], j);
                    p.log_w += log_w;
                    x
                }
                None => sample_motion_model_odometry(u, &p.pose, &self.alphas, &mut self.rng),
            };

            for (k, f) in z.iter().enumerate() {
                match self.associate_at(p, &pose, f, c.map(|cs| cs[k])) {
                    Some(j) => {
                        // 1.0 pays here; 2.0 already paid for the anchor above.
                        if !(k == 0 && anchor.is_some()) {
                            p.log_w += p.landmarks[j].weight_1_0(&pose, *f, &self.q).ln();
                        }
                        p.landmarks[j].update(&pose, *f, &self.q);
                    }
                    None => {
                        p.landmarks.push(LandmarkEkf::from_first_sighting(&pose, *f, &self.q));
                        p.log_w += self.p0.ln();
                    }
                }
            }

            p.pose = pose;
            p.path.push(pose);
        }
        self.select()
    }

    /// FastSLAM 2.0's proposal (Derivation 4): add precisions, let the
    /// measurement pull the mean, and return the predictive log-likelihood as
    /// the weight increment.
    fn propose_2_0(
        &mut self,
        p: &FsParticle,
        u: &OdomDelta,
        r: &Matrix3<f64>,
        z: Feature,
        j: usize,
    ) -> (SE2, f64) {
        let lm = &p.landmarks[j];
        let x_hat = p.pose.apply_odom(u);
        let (r_hat, phi_hat) = predict(&x_hat, &lm.mu);
        let hm = landmark_jacobian(&x_hat, &lm.mu);
        let hx = pose_jacobian(&x_hat, &lm.mu);

        let qj = hm * lm.sigma * hm.transpose() + self.q;
        let qj_inv = qj.try_inverse().unwrap();
        let nu = Vector2::new(z.r - r_hat, wrap_angle(z.phi - phi_hat));

        // Σ_x = [Hxᵀ Qj⁻¹ Hx + R⁻¹]⁻¹ — information matrices add.
        let sigma_x = (hx.transpose() * qj_inv * hx + r.try_inverse().unwrap())
            .try_inverse()
            .expect("R is positive definite, so the sum is too");
        let mu_x = Vector3::from(x_hat) + sigma_x * hx.transpose() * qj_inv * nu;

        let l = sigma_x.cholesky().expect("Σ_x ≻ 0").l();
        let noise: Vector3<f64> =
            Vector3::from_fn(|_, _| StandardNormal.sample(&mut self.rng));
        let sample = SE2::from_vector(mu_x + l * noise);

        // The weight uses the *predictive* covariance and is evaluated at the
        // motion mean — so it does not depend on where the sample landed.
        let s = hx * r * hx.transpose() + qj;
        (sample, log_mvn_pdf(&nu, &s))
    }
}

The grid variant needs one more idea. Resampling clones particles, most clones die before they ever write a cell, and copying a 40×30 grid per clone per step is pure waste. FastSLAM's classic answer was a shared balanced tree over landmarks; the idiomatic Rust answer is Arc plus make_mut, which gives the same asymptotic win with none of the pointer bookkeeping.

use std::sync::Arc;
use pr_core::mapping::OccGrid;

pub struct GridParticle {
    pub pose: SE2,
    pub path: Vec<SE2>,
    /// Shared until written. `Arc::make_mut` performs the deep copy on the
    /// first mutation after a clone — and only if someone else is still
    /// reading, which after a resample is usually nobody.
    pub map: Arc<OccGrid>,
    pub log_w: f64,
}

impl Clone for GridParticle {
    fn clone(&self) -> Self {
        // O(1). The cells are not touched.
        Self { pose: self.pose, path: self.path.clone(), map: Arc::clone(&self.map), log_w: 0.0 }
    }
}

pub struct GridRbpf {
    particles: Vec<GridParticle>,
    /// K in the improved proposal: how many poses to probe around the mode.
    k_samples: usize,
    /// Resample below this fraction of M. Grisetti et al. use 1/2.
    neff_threshold: f64,
    alphas: [f64; 4],
    angles: Vec<f64>,
}

impl GridRbpf {
    /// One `gmapping_step`. On native builds the per-particle loop is a rayon
    /// `par_iter_mut`; on WASM it is the same code, single-threaded, because
    /// every particle is independent until the weights are normalized.
    pub fn step(&mut self, u: &OdomDelta, scan: &[f64], rng: &mut Pcg64) -> RbpfReport {
        for p in &mut self.particles {
            let predicted = p.pose.apply_odom(u);
            let matched = scan_match(&p.map, &predicted, scan, &self.angles);

            let (pose, increment) = if matched.score - matched.seed_score > SCAN_MATCH_GAIN {
                self.improved_proposal(p, u, matched.pose, scan, rng)
            } else {
                // Flat score surface: the map has nothing to say here yet, and
                // trusting the "optimum" would be trusting numerical noise.
                let x = sample_motion_model_odometry(u, &p.pose, &self.alphas, rng);
                let w = log_map_match_score(&p.map, &x, scan, &self.angles);
                (x, w)
            };

            p.pose = pose;
            p.path.push(pose);
            // Tempered: beams are not independent (Chapter 10), so the raw
            // product over-counts the evidence badly enough to collapse the
            // population on the first resample.
            p.log_w += WEIGHT_TEMPER * increment;

            // The copy happens here, or not at all.
            Arc::make_mut(&mut p.map).integrate_scan(&pose, scan, &self.angles);
        }

        let weights = normalize_log_weights(&self.particles);
        let neff = effective_sample_size(&weights);
        let resampled = neff < self.neff_threshold * self.particles.len() as f64;
        if resampled {
            // Clones are O(1) here; the deep copies happen lazily, above.
            self.particles = low_variance_sampler(&self.particles, &weights, rng);
        }
        RbpfReport { neff, resampled }
    }
}

/// N_eff = 1 / Σ w̃². M when the weights are uniform, 1 when a single particle
/// carries everything.
pub fn effective_sample_size(w: &[f64]) -> f64 {
    let s: f64 = w.iter().map(|x| x * x).sum();
    if s > 0.0 { 1.0 / s } else { 0.0 }
}

A worked example you can check by hand

Two particles, one landmark, one sighting. Everything below is arithmetic you can do on paper, and the numbers are pinned by the test that follows.

Particle $i$ sits at the origin facing along $+x$. Its EKF for landmark $j$ says $\mu_j = (4, 0)$ with $\Sigma_j = \diag(0.21,\, 0.12)$. The sensor has $\mat{Q} = \diag(0.2^2,\, 0.05^2) = \diag(0.04,\, 0.0025)$.

Step 1 — the Jacobian. With $\delta = (4, 0)$, $r = 4$ and $q = 16$:

Step 2 — the innovation covariance. $\mat{H}_m \Sigma_j \mat{H}_m\T = \diag(0.21,\, 0.0625 \cdot 0.12) = \diag(0.21,\, 0.0075)$, so

Bearing uncertainty is dominated by the sensor, range uncertainty by the map — read straight off the two terms.

Step 3 — the normalizer. $|2\pi\mat{S}|^{1/2} = 2\pi\sqrt{0.25 \cdot 0.01} = 2\pi (0.05) = 0.31416$, so the leading factor is $1/0.31416 = 3.1831$.

Step 4 — the weights. Particle 1 gets $z = (4.1,\, 0.05)$, hence $\boldsymbol{\nu}_1 = (0.1,\, 0.05)$:

Particle 2 gets $z = (4.3,\, 0.15)$, hence $\boldsymbol{\nu}_2 = (0.3,\, 0.15)$ — exactly three times as wrong:

Step 5 — normalize, and check the resampling rule. $\tilde w = (0.7613,\, 0.2387)$, so

Three lessons in one number. Tripling the innovation cost particle 2 a factor of $3.19$ in weight, not a factor of 3 — the exponential is unforgiving, and it is unforgiving in the bearing channel (which contributed 2.25 of the 2.61) because that is where $\mat{S}$ is small. And with $N_{\mathrm{eff}} = 1.571 > M/2 = 1$, gmapping's rule says do not resample: a 3:1 weight ratio between two particles is not yet degeneracy.

#[cfg(test)]
mod tests {
    use super::*;
    use approx::assert_relative_eq;

    #[test]
    fn worked_example_ch17_two_universes() {
        let q = Matrix2::new(0.04, 0.0, 0.0, 0.0025);
        let lm = LandmarkEkf {
            mu: Vector2::new(4.0, 0.0),
            sigma: Matrix2::new(0.21, 0.0, 0.0, 0.12),
            hits: 1,
        };
        let x = SE2::new(0.0, 0.0, 0.0);

        let (_, s) = lm.innovation(&x, Feature { r: 4.1, phi: 0.05 }, &q);
        assert_relative_eq!(s[(0, 0)], 0.25, epsilon = 1e-12);
        assert_relative_eq!(s[(1, 1)], 0.01, epsilon = 1e-12);
        assert_relative_eq!(s[(0, 1)], 0.0, epsilon = 1e-12);

        let w1 = lm.weight_1_0(&x, Feature { r: 4.1, phi: 0.05 }, &q);
        let w2 = lm.weight_1_0(&x, Feature { r: 4.3, phi: 0.15 }, &q);
        assert_relative_eq!(w1, 2.753_451_476_665, epsilon = 1e-9);
        assert_relative_eq!(w2, 0.863_168_987_665, epsilon = 1e-9);

        // Two particles, a 3.19:1 weight ratio — and still above the M/2 line.
        let total = w1 + w2;
        let normalized = [w1 / total, w2 / total];
        assert_relative_eq!(normalized[0], 0.761_332_714_843, epsilon = 1e-9);
        assert_relative_eq!(effective_sample_size(&normalized), 1.570_870_837_6, epsilon = 1e-9);
        assert!(effective_sample_size(&normalized) > normalized.len() as f64 / 2.0);
    }
}

The TypeScript port reproduces the same five digits, from the same landmarkInnovation the widgets call. If the prose and the code ever disagree, the test settles it.

Putting it together

What the numbers say

Here is the Apartment lap from w17.1 — out along the corridor, turn, and back over ground already mapped — run to completion under both proposals, at five particle counts, on seeds 3, 42, 11 and 7. Numbers are the RMS position error of the highest-weight particle over the whole lap, in metres. Raw odometry on the same log has an RMS error of 3.37 m.

Three readings, in order of how much they matter.

The proposal is worth particles. On the median, eight measurement-aware particles match sixteen motion-prior ones, and four of them beat eight. This is Grisetti et al.'s headline claim reproduced in a browser: the reason gmapping could map a building with thirty particles when FastSLAM 1.0 needed hundreds is not a better resampler, it is that the scan matcher tells each particle roughly where it is before the particle has to guess. Note the median-versus-worst columns, though — at small $M$ a good proposal lowers the typical error without doing much for the tail, because the tail is dominated by runs where every particle was wrong together.

More particles is not monotone. At $M = 40$ one of the four seeds diverged to 3.9 m. Nothing about a large $M$ prevents every particle from being descended from one universe that mapped a doorway in the wrong place; once the map is wrong and self-consistent, the scan matcher agrees with it enthusiastically. Particle count buys coverage of the proposal's uncertainty, not insurance against a confidently wrong map.

The floor is the grid. Every configuration plateaus near 0.16 m, which is about half a cell. That is not a coincidence and not a bug: the map-match score cannot distinguish poses that put the scan endpoints in the same cells. If you want centimetres, you need Chapter 16's continuous correspondences, not more universes.

The road not taken: EM mapping

The verdict, in 2026

It would be dishonest to end this chapter without saying what happened next.

gmapping lost. In production 2-D SLAM the default is a pose graph — Chapter 16's architecture, shipped as slam_toolbox and its relatives. The reasons are not subtle. A pose graph stores one map and $O(T)$ poses; an RBPF stores $M$ maps, and while copy-on-write hides much of that, a large building at 5 cm resolution times thirty particles is real memory. A pose graph can revisit and correct an old decision when a loop closes; an RBPF can only fail to have deleted the universe that was right. And a pose graph gives you lifelong mapping — load yesterday's graph, add today's — where a particle filter's ancestry is gone the moment it is resampled away.

Rao-Blackwellization won. The theorem is permanent, and it is everywhere: sample the part that is genuinely nonlinear, integrate the rest. Chapter 18 marginalizes landmarks out of a visual-inertial window rather than sampling them. Chapter 22 builds POMCP on particles that carry world states because the value function given a state is computable. Chapter 23 samples control sequences and integrates the dynamics analytically along each one. Every one of those is the same inequality from Derivation 2.

Per-particle data association survived too, and it is still the cleanest answer to genuine ambiguity. A corridor of identical doors, a symmetric building, a warehouse of identical racks: a single-hypothesis front end must guess, and a pose graph with a wrong loop closure produces a map that is confidently, catastrophically folded. Modern systems fight this with robust kernels and switchable constraints, which are ways of softening a commitment. FastSLAM never had to commit. That is why multi-hypothesis loop closing keeps being reinvented, and why the ideas in this chapter are worth knowing even though you will probably ship the pose graph.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Prove the factorization, and break it

   Reproduce the induction of Derivation 1 with every normalizer tracked explicitly (the sketch quietly absorbs one). Then construct a measurement model that observes two landmarks in a single reading — say a stereo pair whose baseline is known — and show exactly which line of the proof fails and what the map posterior factors into instead. Finally: a laser scan observes hundreds of grid cells at once. What is this chapter's grid-based filter implicitly assuming, and which chapter already told you that assumption is false?

2. Foundation exerciseFDifficulty 2 of 3The variance you did not have to pay

   Starting from the law of total variance, show that the Rao-Blackwellized estimator's variance deficit is exactly $\E[\Var[\varphi \mid X]]$. Then estimate that quantity for landmark SLAM: if each of $N$ landmarks has posterior covariance $\Sigma_j$ and you sampled them instead of integrating them, roughly how does the weight variance scale with $N$? Use your answer to explain why the naive $(3+2N)$-dimensional particle filter of the opening section fails at $N = 24$ rather than at $N = 2$.

3. Foundation exerciseFDifficulty 3 of 3Derive the 2.0 proposal for range–bearing

   Fill in every step of Derivation 4 for the range–bearing model: write $\mat{H}_x$ explicitly, complete the square in $x_t$, and obtain $(\mu_x, \Sigma_x)$. Then answer two questions the algebra makes easy. (a) What happens to $\Sigma_x$ as $\mat{Q} \to 0$ with a single landmark observed — is the pose fully determined, and if not, which direction is left free? (b) What happens when two landmarks are observed at once, and why does FastSLAM 2.0 as published only use one of them in the proposal?

4. Conceptual exerciseCDifficulty 1 of 3Predict the depletion, then measure it

   In w17.3, before touching anything, predict what happens to the "samples in 90%" counter for FastSLAM 1.0 when you halve $\sigma_r$ — will it halve, stay flat, or something else? Now do it, and read the counter. Then find the value of $\sigma_r$ at which 1.0's $N_{\mathrm{eff}}/M$ stops falling, and explain what is limiting it there. (Hint: what else is in $\mat{Q}_j$ besides $\mat{Q}$?)

5. Conceptual exerciseCDifficulty 2 of 3Find the smallest sufficient universe count

   In w17.1, turn the measurement-aware proposal off. Find the smallest $M$ that completes the lap with the tiles still recognizable as the Apartment, on at least four of five re-rolled seeds. Now turn the proposal back on and repeat. Report both numbers and the ratio. Then explain, using w17.2, why raising $M$ further stops helping — and identify, from the lineage gauge, the moment at which extra particles stopped being independent hypotheses.

6. Practical exercisePDifficulty 2 of 3Per-particle data association in the corridor of identical doors

   Implement FastSLAM_unknown_correspondence (the second algorithm box) against the landmark filter: Mahalanobis gating at $\chi^2_{2,0.99}$, maximum-likelihood association, and new-landmark creation below $p_0$. Then build the experiment the chapter keeps promising: a corridor with six identical doors spaced identically, entered at an unknown offset. Show that the particle population holds multiple association hypotheses simultaneously, and that the Chapter 11 EKF on the same log commits to one and never recovers. Report the fraction of seeds each approach gets right.

7. Practical exercisePDifficulty 3 of 3Copy-on-write versus the classic tree

   Replace Arc::make_mut with FastSLAM's original data structure: a balanced binary tree over landmarks, shared between particles, where an update creates $O(\log N)$ new nodes and shares the rest. Benchmark both against $M \in \{10, 100, 1000\}$ and $N \in \{50, 500, 5000\}$, on native and on WASM. Report where the constant factors cross, and how much of the difference is allocation rather than copying. Then argue whether the tree is worth it for the grid variant, where the shared object is 1200 cells rather than $N$ small structs.