Bayesian Localization and Mapping

Recursive Bayesian localization derived once and run on a grid and as a particle cloud, Choset's physical sensor model, occupancy grids in log odds, Rao-Blackwellized SLAM as a statement — and the half neither Choset nor the sister book writes down, what a planner does when the start is a distribution.

The problem: switched on, told nothing

Rusty was last seen in room A. Someone carried it to room B and switched it on. The belief Chapter 15 would hand a planner — a tight Gaussian around the last known pose — is confident and wrong, and a confident, wrong belief is worse than no belief at all.

On the left the planner does exactly what Part II taught it: from the believed cell it follows the cost-to-go field Up, through room A's doorway, toward the goal. In room B that doorway is a wall. The wheels slip, the odometry reports the move, the predict-only belief sails into the corridor and the chassis stays where it was. On the right a uniform particle cloud over all free space knows nothing, which is the honest state. It moves where every hypothesis stays cheap, each scan kills the rooms that do not match what it sees, and it arrives. The check file pins both outcomes on the default seed: the Gaussian bumps and never reaches the goal; the cloud has its mode within half a metre of the truth inside twenty scans and arrives with at most three bumps.

Choset calls the three situations position tracking (initial configuration approximately known), global localization (unknown), and the kidnapped robot problem (believed wrongly, to be unlearned). The Kalman filter handles the first. This chapter is about the other two, and about what a planner can do while the belief is still a cloud.

One line of bookkeeping, carried over from Chapter 15: for Rusty the planner's configuration $q \in \SEtwo$ and the filter's state $x(k)$ are the same object, and a grid belief lives on the Chapter 6 lattice, so the cost-to-go $J$ and the belief share cells. The sister book writes $x_t, u_t, z_t$ and $\bel(x_t)$; this chapter keeps Choset's $P(x(k) \mid u(0{:}k{-}1), y(1{:}k))$ because his derivation reads better in it.

Building intuition

One recursion, two tables

Here is one recorded Apartment run — seeded odometry increments and 36-beam scans from the Chapter 2 simulator — replayed into two localizers that start uniform over free space. Left: Choset's Algorithm 16 on a grid over $\SEtwo$. Right: Algorithms 17–18 as a particle cloud. Same data, same sensor model, same recursion; only the way the posterior is written down differs.

Watch three things. First, the mode and the mean are different markers for a reason: during the bimodal phase, when rooms A and C look identical from inside, the mean (Choset's eq. 9.17, a weighted average of positions and a circular average of headings) sits in the corridor wall between them. "The mean of a bimodal distribution might lie within an obstacle so that no meaningful commands can be generated." The mode is always a possible location, but it can jump between peaks from one step to the next. Neither is what a planner should drive toward; that is the subject of the last section.

Second, the cost meters. At the default budget the grid is $30 \times 23 \times 18 = 12\,420$ cells and its one-byte expected-distance table cost $10\,782$ ray casts to build, once; the cloud is 600 particles at four doubles each, and every step costs it 600 ray casts per beam it uses. Slide the budget down and the two fail differently: the grid blurs — a 0.6 m cell cannot say which side of a doorway you are on — while the cloud starves — 150 particles over 90 m² of free space leave whole rooms unrepresented, and a hypothesis with no particle near it is gone for good. That is the misconception the widget kills: the particle filter is not a cheaper grid. It is a different approximation with a different failure mode.

Third, press kidnap. The $\tilde p$ strip under each panel is Choset's monitor from §9.1.4: the average observation likelihood $\tilde p = N^{-1}\sum_j P(y(i) \mid x_j)$, here on a per-beam scale so one threshold serves every budget. When the robot is carried to room C the grid's $\tilde p$ dives and, with auto-restart on, it does what Burgard et al. did — begins global localization again from uniform. The plain cloud has no particles in room C and nothing to resample toward. The remedies Choset lists — random particles in the motion model (Fox et al.), restart on a $\tilde p$ threshold (Burgard et al.), a random fraction scaled by $\tilde p$ (Lenser and Veloso), a smoothed $\tilde p$ (Gutmann and Fox) — are all switchable in the library, and the hook's cloud uses two of them.

Five cells, by hand

Everything the widget does is this figure done many times. Choset's Figure 9.1 is a corridor with doors; shrink it to five cells on a loop with doors at 1 and 3, and use the numbers of his problems 6 and 7: $P(\text{door} \mid \text{door cell}) = 0.8$, $P(\text{door} \mid \text{wall cell}) = 0.4$, and "move right one" succeeds with probability $0.8$ and stays put with $0.2$.

Start uniform, $0.2$ everywhere, entropy $\log_2 5 = 2.3219$ bits. The robot sees a door. Multiply each cell by the likelihood — $0.8$ at cells 1 and 3, $0.4$ elsewhere — to get the unnormalized $[0.08, 0.16, 0.08, 0.16, 0.08]$; the sum is $\eta^{-1} = 0.56$; divide and the posterior is $[\tfrac17, \tfrac27, \tfrac17, \tfrac27, \tfrac17] \approx [0.1429, 0.2857, 0.1429, 0.2857, 0.1429]$, entropy $2.2359$ bits. Sensing sharpened the belief, and it is bimodal: two doors look alike. Now move right. Each cell receives $0.8$ of its left neighbour and keeps $0.2$ of itself: $bel'(c) = 0.8\,bel(c{-}1) + 0.2\,bel(c)$, which gives $\tfrac17[1.0, 1.2, 1.8, 1.2, 1.8] \approx [0.1429, 0.1714, 0.2571, 0.1714, 0.2571]$ and entropy $2.2811$ bits. Moving smeared it. Every number here is produced by fiveCellSenseMove() in the library and asserted to $10^{-4}$ by the check file; the figure prints what the code returns.

The two steps are the whole chapter. Sensing is a multiplication by a likelihood and a renormalization; moving is a convolution with a stochastic kernel. A convolution with a kernel that has any spread can only increase entropy, so the belief sharpens only when the robot looks.

What a planner does with a belief

The metaphor for the whole chapter is one line: don't drive from where you probably are — drive so that wherever you are, the rest of the trip stays cheap.

Take a $3 \times 3$ grid, rows numbered top to bottom, four-connected, unit step cost, an obstacle at $(1, 0)$ and the goal at $(0, 1)$. Backward Dijkstra from the goal labels every free cell with its cost-to-go $J$: $0$ at the goal, $1$ at its three neighbours, $2$ at $(1, 2)$ and $(2, 1)$, $3$ at the bottom corners, and $\lambda$ at the obstacle. Suppose the belief says the robot is at $(2, 1)$ with weight $0.6$ — the mode — or at $(2, 0)$ with weight $0.4$. From the mode, Up is obviously best: $1 + J(1, 1) = 2$. But Up from $(2, 0)$ drives into the obstacle. Score every action by its expected cost under the belief:

At $\lambda = 10$ these are $5.2$, $3.6$, $6.4$, and Down is off-grid for both hypotheses at $10$. Right wins. The planner first moves sideways, into the column that is safe under both hypotheses, and only then Up. Set the collision penalty with

and the inset table in the last widget recomputes: Up and Right tie when $0.6 \cdot 2 + 0.4\lambda = 3.6$, at $\lambda^* = 6$; below it the planner gambles on the mode, and at $\lambda = 4$ Up costs $2.8$ and wins. The break-even is printed, not hidden, because the answer depends on it.

The mathematics

Definitions

Position tracking, global localization, kidnapped robot (§9.1). The initial configuration is known approximately, unknown, or believed wrongly and to be unlearned. Posterior representation. Mathematically $P : X \to \mathbb{R}$ lives in an infinite-dimensional space; a filter stores a finite approximation — a Gaussian $(\hat x, P)$, a grid over $\SEtwo$, or weighted samples. §9.1.4 treats all three; this chapter's Belief trait is what a planner needs from any of them: weighted samples on the lattice, the mode, the entropy.

Sensor model (§9.1.5). $P(y \mid x) = P(y \mid d(x))$ (9.22): the likelihood depends on the pose only through the ray-cast expected distance $d(x)$, and $P(y \mid d)$ is a mixture of four densities — hit, short, random, max-range (9.23) — binned at resolution $\Delta$ so each histogram $h_i$ sums to one (9.24).

Occupancy grid (§9.2.1). A map $m$ of binary cells $m_l$ assumed independent, $P(m) = \prod_l P(m_l)$ (9.40), each updated by the inverse model $P(m_l \mid x(k), y(k))$.

Belief-replanning policy (this book). $a^* = \arg\min_a \mathcal{J}_{bel}(a)$, recomputed after every sense–update. An expectation-of-cost rule with replanning — not a POMDP policy.

Recursive Bayesian localization, derived once

The normalizer is free

Three representations, two of them here

Kalman filters keep a Gaussian: efficient, floating-point resolution, high dimensions — and unable to say "one of three doors." Jensfeld and Christensen's mixture of Gaussians stretches it; this book stops at Chapter 15.

Grids (Burgard, Fox, and coworkers) store $P$ on a three-dimensional lattice over $(x_r, y_r, \theta_r)$; Choset reports 10–30 cm and 2–10° as sufficient. The prediction step is $O(N^2)$ if done as written on line 4 of Algorithm 16. Fox, Burgard, and Thrun's remedy: shift every $(x, y)$-plane by the offsets $\Delta x, \Delta y$ its heading implies, then convolve with a bounded separable kernel for the motion noise,

once per axis, $\tfrac23/\tfrac13$ at the borders — $O(NW)$ for kernel width $W$. The mean is a weighted average of cell centres, with the heading by the circular average $\hat\varphi = \operatorname{atan2}(\sum P(i)\sin\varphi(i), \sum P(i)\cos\varphi(i))$ (9.17); the mode is one max, sharpened to sub-cell accuracy by averaging around it.

Particles store $\mathcal{M} = \{(x_j, \omega_j)\}$ and run sequential importance sampling with resampling — "a survival of the fittest scheme." Motion is sampled, not convolved: Choset encodes an increment as $(\alpha, \beta, d)$ (Fig. 9.5) and perturbs each with Gaussians whose widths grow with both rotations and the translation,

then applies (9.18): $x' = x + d'\cos(\theta + \alpha')$, $y' = y + d'\sin(\theta + \alpha')$, $\theta' = \theta + \alpha' + \beta'$. With all $\sigma_i = 0$ this is the exact increment, and the check confirms the sampler's mean displacement is unbiased to a millimetre over 20 000 draws. Samples that would cross a wall are rejected, as Choset suggests. The sister's Chapter 8 owns resampling variance, KLD-adaptive sample sizes and the histogram filter's bookkeeping; its Chapter 12 owns Augmented MCL and the grid-versus-MCL comparison table. Both are vendored unchanged under web/lib/localize/, and nothing in this chapter re-implements a filter.

The sensor model, and why its σ is not the laser's

The four situations come straight from Choset's Figure 9.9, one ultrasound scan from a B21: most beams hit the nearest object (Gaussian); some are shorter — crosstalk, a person, a refrigerator not in the map (exponential); some pass through a bookshelf and return nonsense (uniform); some never return (max range). Fit the weights by log-likelihood maximization: the library's EM over $(\alpha, \beta, \gamma, \delta)$ recovers $(0.6, 0.15, 0.15, 0.1)$ from 6000 synthetic readings to within $0.03$. Two honesty items Choset insists on, and the code keeps:

• Beams are not independent. (9.25) is false and useful; neighbouring beams see the same unmodeled chair. Fox et al. used 60 of the 181 beams of a SICK scanner; the every parameter in scan_likelihood is that remedy, and the widgets default to every third beam.
• The variance encodes the map, not just the sensor. "The variances of the Gaussians do not depend solely on the accuracy of the sensor. They also encode the uncertainty of the map." A SICK laser's $\sigma$ is a few centimetres; the localizer uses $\sigma = 0.25$ m. The check file runs the same 600 particles both ways on the same log: at $0.25$ m the mode ends within $0.6$ m of the truth; at the raw $0.08$ m no particle ever lands within one $\sigma$ of the right answer and the cloud starves, six metres off. That is Choset's problem 9, and the sister's Chapter 10 is where beam intrinsics and the likelihood-field alternative are fitted properly.

The cost trick is also his: discretize distances to 256 values and $P(y \mid d)$ is "only 256 histograms with 256 bins each," 88 kB at double precision; precompute $d(x)$ for every grid cell and heading bin as one byte each, and the grid localizer's correction is a table lookup per beam. The widget's 0.4 m grid built that table from $10\,782$ ray casts and reads it for the rest of the run.

Log-odds occupancy mapping

Rao-Blackwellized SLAM, as a statement

Replanning on a belief as expected cost-to-go

This derivation is the book's addition; neither Choset nor the sister states it as a planning primitive.

The algorithm

Choset numbers these 16 through 20; the last two are this book's.

Implementation in Rust

The estimate crate of Chapter 15 gains a bayes module. Nothing in it re-implements a filter: the sister book's HistogramFilter, ParticleFilter, low_variance_resample, GridLocalizer, Mcl, AugmentedMcl, OccGrid, and GridRbpf are vendored unchanged. What the module adds is Choset's vocabulary around them, the Belief trait a planner consumes, and the one owned artifact.

pub use filters::nonparametric::{HistogramFilter, ParticleFilter, ParticleSet, low_variance_resample};
pub use filters::localize::{GridLocalizer, Mcl, AugmentedMcl};
pub use filters::occgrid::OccGrid;
use search::{Lattice, CellIdx};   // the Chapter 6 grid and its moves
use rand_pcg::Pcg64;

/// What a planner needs from any posterior: weighted samples on the planning
/// lattice plus the two statistics Choset discusses (§9.1.4). Implemented for the
/// grid belief, the particle set, and the Chapter 15 Gaussian, so
/// `replan_on_belief` never knows which representation it was handed.
pub trait Belief {
    /// K cells with weights summing to one. The grid returns its K heaviest
    /// marginal cells; the cloud draws K by the low-variance comb (Alg. 18).
    fn samples(&self, k: usize, lattice: &Lattice, rng: &mut Pcg64) -> Vec<(CellIdx, f64)>;
    /// Choset: "the mode has the advantage that it generally corresponds to a
    /// possible location of the vehicle" — unlike the mean (9.17).
    fn mode(&self, lattice: &Lattice) -> CellIdx;
    fn entropy_bits(&self) -> f64;
}

/// Choset's increment (Fig. 9.5): rotate α toward the target, drive d, rotate β.
/// The sister's (δ_rot1, δ_trans, δ_rot2) under other letters; `From` both ways.
#[derive(Clone, Copy, Debug)]
pub struct Increment { pub alpha: f64, pub d: f64, pub beta: f64 }

/// σ₁…σ₆ of (9.19)–(9.21). Widths grow with the rotations *and* the translation,
/// which is why a long straight run still fans out in heading.
pub type Sigmas = [f64; 6];

pub fn sample_motion(x: &SE2, u: Increment, s: &Sigmas, rng: &mut Pcg64) -> SE2 {
    let n = |sigma: f64| if sigma > 0.0 { Normal::new(0.0, sigma).unwrap().sample(rng) } else { 0.0 };
    let alpha = u.alpha + u.alpha * n(s[0]) + u.d * n(s[1]);
    let beta  = u.beta  + u.beta  * n(s[2]) + u.d * n(s[3]);
    let d     = u.d     + u.d     * n(s[4]) + (u.alpha + u.beta) * n(s[5]);
    SE2::new(x.x() + d * (x.theta() + alpha).cos(),       // (9.18)
             x.y() + d * (x.theta() + alpha).sin(),
             wrap(x.theta() + alpha + beta))
}

The five-cell corridor is the sister's one-dimensional histogram filter with Choset's problem 6–7 parameters, and nothing more. It exists so the §3 numbers have one source.

use filters::nonparametric::HistogramFilter;

/// Choset's Fig. 9.1 corridor on a loop of `N` unit cells. `sense` is the
/// filter's `correct`, `step` its `predict` with the two-point kernel of
/// problem 7 — no new filter code lives here.
pub struct Corridor<const N: usize> {
    pub filter: HistogramFilter<N>,
    pub doors: Vec<usize>,
    pub p_door_at_door: f64,   // 0.8
    pub p_door_at_wall: f64,   // 0.4
    pub p_move: f64,           // 0.8 succeeds, 0.2 stays
}

pub struct Sense { pub likelihood: [f64; 5], pub unnormalized: [f64; 5], pub evidence: f64, pub posterior: [f64; 5] }

impl<const N: usize> Corridor<N> {
    /// Update (9.4)–(9.6): multiply, sum, divide — and hand back the sum, because
    /// η⁻¹ is free and the kidnap monitors of §9.1.4 want it.
    pub fn sense(&mut self, saw_door: bool) -> Sense {
        let lik = |c: usize| {
            let p = if self.doors.contains(&c) { self.p_door_at_door } else { self.p_door_at_wall };
            if saw_door { p } else { 1.0 - p }
        };
        let prior = self.filter.belief();
        let unnormalized: Vec<f64> = (0..N).map(|c| prior[c] * lik(c)).collect();
        let evidence: f64 = unnormalized.iter().sum();                 // Alg. 16 lines 6–10
        self.filter.correct(|c| lik(c));                               // lines 11–13 inside
        Sense { likelihood: array(lik), unnormalized: array(|c| unnormalized[c]),
                evidence, posterior: self.filter.belief() }
    }

    /// Prediction (9.3) for "move `steps` places": land with p_move, stay otherwise,
    /// written on the displacement error the filter hands us, with the loop's wrap.
    pub fn step(&mut self, steps: i64) -> [f64; N] {
        let (n, p) = (N as i64, self.p_move);
        let wrap = |d: i64| d - n * ((d as f64 / n as f64).round() as i64);
        self.filter.predict(steps, |d| if wrap(d) == 0 { p } else if wrap(d + steps) == 0 { 1.0 - p } else { 0.0 });
        self.filter.belief()
    }
}

cargo run --example five_cells -p estimate builds the instance with doors $\{1, 3\}$, runs sense then move, and prints the table the figure above draws:

prior       0.2000 0.2000 0.2000 0.2000 0.2000   H = 2.3219 bits
sense door  0.1429 0.2857 0.1429 0.2857 0.1429   η⁻¹ = 0.56, H = 2.2359 bits
move right  0.1429 0.1714 0.2571 0.1714 0.2571   H = 2.2811 bits

#[test] fn reproduces_five_cell_sense_move() asserts every entry to $10^{-4}$; the TypeScript check does the same. Choset's problem 6 — ten places, landmarks at $\{0, 3, 6\}$, detect, move 3, detect, move 4, detect nothing — runs on the same type with $N = 10$ and gives $P(7) = 0.30$, $P(4) = 0.15$, $P(0) = 0.10$; with problem 7's $0.8/0.2$ motion the mode stays at 7 but drops to $0.2578$ (Exercise 1).

The sensor model is a cache first and a formula second.

/// Choset (9.23): hit N(y; d, σ) + short λe^{−λy} + random γ + max-range δ, binned
/// at Δ. The expected distance d(x) comes from the Chapter 2 ray cast — never
/// re-implemented here. σ is *not* the laser's: it also encodes the map's uncertainty.
pub struct ProximityModel {
    pub sigma: f64, pub lambda: f64,
    pub alpha: f64, pub beta: f64, pub gamma: f64, pub delta: f64,
    pub delta_bin: f64, pub max_range: f64,
    rows: Vec<OnceCell<Box<[f64]>>>,              // the 256 histograms of §9.1.5, lazily
}

impl ProximityModel {
    pub fn n_bins(&self) -> usize { (self.max_range / self.delta_bin).ceil() as usize + 1 }

    /// h_i: P(bin j | expected distance in bin i). The three continuous terms are
    /// each normalized on [0, d_max) and the row is scaled so Σ_j h_{i,j} = 1 (9.24),
    /// which is what Choset's problem 3(b) asks for by "appropriate values for δ_i".
    pub fn row(&self, i: usize) -> &[f64] {
        self.rows[i].get_or_init(|| {
            let d = self.bin_center(i.min(self.n_bins() - 2));
            let m = self.n_bins() - 1;
            let mut h = vec![0.0; m + 1];
            let hit_norm = normal_cdf(self.max_range, d, self.sigma) - normal_cdf(0.0, d, self.sigma);
            let short_norm = 1.0 - (-self.lambda * self.max_range).exp();
            for j in 0..m {
                let (lo, hi) = (j as f64 * self.delta_bin, ((j + 1) as f64 * self.delta_bin).min(self.max_range));
                let hit   = (normal_cdf(hi, d, self.sigma) - normal_cdf(lo, d, self.sigma)) / hit_norm;
                let short = ((-self.lambda * lo).exp() - (-self.lambda * hi).exp()) / short_norm;
                let rand  = (hi - lo) / self.max_range;
                h[j] = self.alpha * hit + self.beta * short + self.gamma * rand;
            }
            h[m] = self.delta;
            let s: f64 = h[..m].iter().sum();
            for v in &mut h[..m] { *v *= (1.0 - self.delta) / s; }
            h.into_boxed_slice()
        })
    }

    /// P(y | d) = h_{bin(d), bin(y)}, eq. (9.22) with the table doing the work.
    pub fn likelihood(&self, y: f64, d_expected: f64) -> f64 { self.row(self.bin(d_expected))[self.bin(y)] }

    /// log Π_k P(y_k | d_k) over every `every`-th beam — (9.25) with Fox et al.'s
    /// 60-of-181 remedy for its false independence assumption.
    pub fn log_scan_likelihood(&self, scan: &[f64], expected: &[f64], every: usize) -> f64 {
        scan.iter().zip(expected).step_by(every.max(1))
            .map(|(&y, &d)| self.likelihood(y, d).max(1e-300).ln()).sum()
    }
}

And the owned artifact. J is the Chapter 6 UniversalPlan with $\lambda$ outside the free set; the decision is a loop over samples and actions.

use search::{Lattice, CellIdx, Action, GridGraph, universal_plan, UniversalPlan};

/// K draws per decision; collision / off-grid penalty λ; information bonus weight (0 disables).
pub struct ReplanConfig { pub k_samples: usize, pub lambda: f64, pub beta_info: f64 }

/// Ch. 6 universal plan: exact cost-to-go to `goal`, with λ wherever the plan has
/// nothing to say. Computed once per goal; D* (Ch. 6) repairs it when the map changes.
pub struct CostToGo { plan: UniversalPlan<CellIdx>, pub lambda: f64 }

pub fn cost_to_go(graph: &GridGraph, goal: CellIdx, lambda: f64) -> CostToGo {
    CostToGo { plan: universal_plan(graph, goal), lambda }
}
impl CostToGo {
    pub fn j(&self, c: CellIdx) -> f64 { self.plan.cost_to_go.get(&c).copied().unwrap_or(self.lambda) }
    /// c(a) + J(f(x, a)), or λ when `a` leaves the free set from `x`.
    pub fn action_cost(&self, lattice: &Lattice, x: CellIdx, a: Action) -> f64 {
        match lattice.successor(x, a) {
            Some(next) if lattice.is_free(next) && !lattice.cuts_corner(x, a) => a.step_cost(lattice) + self.j(next),
            _ => self.lambda,
        }
    }
}

/// Expected one-step entropy reduction after `a` — a heuristic, named so.
pub trait InfoBonus { fn expected_entropy_drop(&self, after: &[(Option<CellIdx>, f64)], a: Action) -> f64; }

/// a* = argmin_a Σ_j ω_j [ c(a) + J(f(x_j, a)) ] − β_info · ΔH_a. Returns the per-action
/// table too: the widget's inset and the §3 micro-example test both need it.
pub fn replan_on_belief(
    j: &CostToGo, samples: &[(CellIdx, f64)], lattice: &Lattice,
    cfg: &ReplanConfig, info: Option<&dyn InfoBonus>,
) -> (Action, Vec<(Action, f64)>) {
    let table: Vec<(Action, f64)> = lattice.actions().iter().map(|&a| {
        let expected: f64 = samples.iter().map(|&(x, w)| w * j.action_cost(lattice, x, a)).sum();
        let bonus = match info {
            Some(b) if cfg.beta_info > 0.0 => {
                let after: Vec<_> = samples.iter().map(|&(x, w)| (lattice.successor(x, a).filter(|&n| lattice.is_free(n)), w)).collect();
                cfg.beta_info * b.expected_entropy_drop(&after, a)
            }
            _ => 0.0,
        };
        (a, expected - bonus)
    }).collect();
    let best = table.iter().min_by(|p, q| p.1.total_cmp(&q.1)).expect("at least one action").0;
    (best, table)
}

/// The loop every fielded robot runs: sense → update → replan → act. One loop for a
/// grid, a cloud, or the Chapter 15 Gaussian.
pub struct BeliefPlanner<B: Belief> { pub belief: B, pub j: CostToGo, pub cfg: ReplanConfig, pub rng: Pcg64 }

impl<B: Belief + Localizer> BeliefPlanner<B> {
    pub fn decide(&mut self, lattice: &Lattice) -> Action {
        let samples = self.belief.samples(self.cfg.k_samples, lattice, &mut self.rng);
        replan_on_belief(&self.j, &samples, lattice, &self.cfg, None).0
    }
    pub fn step(&mut self, lattice: &Lattice, rusty: &mut sim::Rusty, world: &sim::World) -> Action {
        let a = self.decide(lattice);
        let (u, bumped) = rusty.drive_lattice_step(a, world, &mut self.rng); // odometry reports the move either way
        self.belief.correct_contact(&u, bumped);          // the bumper is a sensor too (Ch. 3)
        self.belief.predict(&u, &mut self.rng);           // Alg. 17 lines 2–5
        self.belief.correct(&rusty.scan(world, &mut self.rng)); // lines 6–13, then Alg. 18 when N_eff is low
        a
    }
}

cargo run --example replan_3x3 builds the lattice with the obstacle at $(1, 0)$, prints $J$, then the four expected costs at $\lambda \in \{10, 6, 4\}$ with the chosen action:

J =  [ 1  0  1 ]
     [ λ  1  2 ]
     [ 3  2  3 ]
belief: (2,1) ω=0.6  (2,0) ω=0.4
λ = 10   Up 5.20   Right 3.60   Down 10.00   Left 6.40   → Right   (mode says Up)
λ =  6   Up 3.60   Right 3.60   Down  6.00   Left 4.00   → tie, Up by action order
λ =  4   Up 2.80   Right 3.60   Down  4.00   Left 3.20   → Up
break-even λ* = 6.0

#[test] fn replan_disagrees_with_mode_at_lambda_10() asserts $\{5.2, 3.6, 6.4, 10\}$, the tie at $6$, and the flip at $4$; #[test] fn point_mass_belief_equals_a_star() checks that one sample of weight one reproduces the Chapter 6 path step by step on fifty seeded Apartment queries.

Putting it together

The integration lab is the owned artifact running in the Apartment, with the belief that opened the chapter.

Rusty is in room B, flush under the corridor wall at $(5.0, 3.4)$. The cloud puts $0.6$ of its mass in room A under A's open doorway and $0.4$ in room B — the $3 \times 3$ example with the Apartment's walls. The goal is the west end of the corridor. The graph field is $J$ on the 0.4 m lattice inflated by Rusty's radius, computed once; every tick the planner draws $K = 24$ cells from the cloud, scores the eight lattice actions, executes the argmin, scans, folds the bumper reading in, and updates. The posterior belief is drawn as dots and filled cells; the route the mode alone would take is the thick haloed path stroke — two purple families on one canvas, kept apart by shape rather than hue, which is the book's rule for this chapter.

What happens, and what the check pins. With $\lambda = 20$ the first action is sideways: Up carries about $0.4$ of collision mass — pinned above $0.3$ — because it is a door under one hypothesis and a wall under the other. Within a few steps the corridor walls come into view, the cloud collapses onto room B, the entropy falls to under half its starting value, and the path straightens toward the goal. The belief planner arrives without a bump. Tick plan from the mode and the first action is Up; Rusty hits room B's wall at least once before the scans sort it out. Both runs are in the check file on the widget's seed, and the same scene holds on six seeds.

The λ slider. Every row of the inset table is affine in $\lambda$, so the break-even with the current winner is computed exactly and drawn under the slider. Drag below it and the belief planner gambles on the mode too; the $3 \times 3$ inset moves with the same number, and its $\lambda^* = 6$ never changes because its geometry never does.

The information bonus. Raise $\beta_{info}$ and the path detours toward geometry that distinguishes the hypotheses — the corridor's asymmetric north doorways — and the entropy sparkline drops sooner. It is a heuristic and the widget says so; it is also a preview of what a POMDP planner buys for its price.

Freeze the belief and the planner replans on stale information: it is still the expected-cost rule, still better than the mode when the two hypotheses disagree, but nothing it does can shrink the cloud. Replanning without sensing is the QMDP approximation with no observations to put back.

Three honesty items the chapter keeps, because the capstone inherits this loop. Assumption B of the derivation fails for large moves, and the loop takes small ones for that reason as much as for control. The belief planner is a one-step expectation with replanning, not an optimal POMDP policy, and its answer depends on $\lambda$ — a price the user sets, printed next to the decision it changes. And the five-cell numbers use Choset's problem parameters; they are not a figure from his text.

What Chapter 23 takes from here is exactly BeliefPlanner::step: AugmentedMcl fed by the proximity model and the odometry adapter, cost_to_go over the Chapter 6 lattice inflated by Chapter 7's brushfire clearance, and replan_on_belief every tick. Chapter 19's MPC consumes the same belief's mean and covariance; Chapter 22 learns heuristics on the same sampled loop.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Choset's problem 6, then problem 7

   Ten places on a circle, landmarks at $\{0, 3, 6\}$ that all look alike, $P(\text{landmark} \mid \text{landmark place}) = 0.8$ and $0.4$ elsewhere. The robot detects a landmark, moves 3 places counterclockwise and detects a landmark, moves 4 and detects none. Compute the posterior by hand with deterministic moves (problem 6). Then redo it with problem 7's motion — each unit step succeeds with $0.8$ and stays with $0.2$ — and explain why the mode's probability drops while the mode itself does not move.

   Problem 6: what is the posterior probability of place 7?

   Check Reveal
2. Foundation exerciseFDifficulty 2 of 3Problem 8: where the map hides in Assumption A

   Argue that once $x(k)$ and the map $m$ are known, $y(k)$ is independent of $y(1{:}k{-}1)$ — this is step 2 of the derivation. Then exhibit the dependence when $m$ is not given: construct two maps that agree at $x(k{-}1)$ and differ at $x(k)$, and show that $y(k{-}1)$ changes the predictive distribution of $y(k)$ through the posterior over maps. Relate the result to why Bayesian SLAM carries one map per particle rather than one map for all.

3. Conceptual exerciseCDifficulty 2 of 3Predict the first move and the flip
   Predict first

   In Plan-on-Belief with the default belief and λ = 20, what is the first action, and roughly where does the break-even marker sit?

   Up; break-even near λ = 2, since the mode is in room A

   A sideways step; break-even where 0.4·λ equals the extra path length of going around

   Down, away from the wall under both hypotheses
4. Conceptual exerciseCDifficulty 2 of 3Kidnap during the bimodal phase
   Predict first

   In Grid vs Particles, kidnap Rusty while the posterior is split between rooms A and C. Before the next frame is drawn, where does the mean marker land?

   On the true position in room C

   In the wall between the rooms, roughly midway

   On the mode, since the filter snaps the mean to the nearest peak
5. Practical exercisePDifficulty 3 of 3A Belief for the Chapter 15 Gaussian

   Implement Belief for Gaussian<3> by sigma points — seven for $n = 3$, weights from the unscented transform — instead of seeded draws, mapping each sigma point to its lattice cell and merging duplicates. Property-test that for a tight Gaussian ($\sigma \le 0.1$ m) replan_on_belief and A* from the mean agree on 100 seeded Apartment queries, and that for a wide one ($\sigma \ge 0.8$ m) they disagree at least once. The TypeScript consolidateCells is the merge step.

6. Practical exercisePDifficulty 3 of 3D* Lite under the belief planner (stretch)

   Replace the full backward Dijkstra in cost_to_go with Chapter 6's D* Lite so that an occupancy change repairs only the affected cells. Measure BeliefPlanner::step time, fixed seed, before and after on a run with twenty door events (a door closes, the lattice cell becomes occupied, $J$ must change). Report the speedup and the number of cells touched per repair.

   Stuck? Get a hint

References

1. Choset, H., Lynch, K. M., Hutchinson, S., Kantor, G., Burgard, W., Kavraki, L. E., and Thrun, S. (2005) Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press.link to Principles of Robot Motion: Theory, Algorithms, and Implementations (opens in a new tab)

   Chapter 9 is this chapter's source: the derivation with its two assumptions and the two-rooms caveat (§9.1.3), grids and particles with Algorithms 16–18 (§9.1.4), the sensor model (§9.1.5), occupancy grids and Algorithm 19 (§9.2.1), Rao-Blackwellized SLAM and Algorithm 20 (§9.2.2), and problems 6–9.

2. Fox, D., Burgard, W., and Thrun, S. (1999) Markov Localization for Mobile Robots in Dynamic Environments. Journal of Artificial Intelligence Research 11, 391–427.doi:10.1613/jair.616 (opens in a new tab)

   The grid localizer of Algorithm 16 as fielded on Rhino: shift-and-convolve prediction, the sensor model's treatment of unmodeled obstacles, and the 60-of-181 beam subsampling this chapter adopts.

3. Fox, D. (2003) Adapting the Sample Size in Particle Filters Through KLD-Sampling. International Journal of Robotics Research 22(12), 985–1003.doi:10.1177/0278364903022012001 (opens in a new tab)

   How many particles a cloud needs, and why the answer changes as it converges — the budget slider's missing axis, treated fully in the sister's Chapter 8.

4. Moravec, H. P. and Elfes, A. (1985) High Resolution Maps from Wide Angle Sonar. IEEE International Conference on Robotics and Automation.doi:10.1109/ROBOT.1985.1087316 (opens in a new tab)

   The occupancy grid. Choset's cone model (9.41)–(9.42) is an explicit approximation of Elfes's mixture of Gaussians and linear functions.

5. Grisetti, G., Stachniss, C., and Burgard, W. (2007) Improved Techniques for Grid Mapping with Rao-Blackwellized Particle Filters. IEEE Transactions on Robotics 23(1), 34–46.doi:10.1109/TRO.2006.889486 (opens in a new tab)

   Algorithm 20 made practical: scan-matched proposals and adaptive resampling, the recipe the vendored GridRbpf follows.

6. Kaelbling, L. P., Littman, M. L., and Cassandra, A. R. (1998) Planning and Acting in Partially Observable Stochastic Domains. Artificial Intelligence 101(1–2), 99–134.doi:10.1016/S0004-3702(98)00023-X (opens in a new tab)

   The POMDP, and the QMDP approximation (first proposed by Littman, Cassandra and Kaelbling in 1995) that this chapter's expected-cost-to-go rule is an instance of — with the honest warning that it never plans to gather information.

7. Prentice, S. and Roy, N. (2009) The Belief Roadmap: Efficient Planning in Belief Space by Factoring the Covariance. International Journal of Robotics Research 28(11–12), 1448–1465.doi:10.1177/0278364909341659 (opens in a new tab)

   Planning in the space of Gaussian beliefs by propagating covariance along roadmap edges: the step above the one-step expectation taken here, and below the full POMDP.

8. Macenski, S., Martín, F., White, R., and Ginés Clavero, J. (2020) The Marathon 2: A Navigation System. IEEE/RSJ International Conference on Intelligent Robots and Systems.link to The Marathon 2: A Navigation System (opens in a new tab)

   Nav2: the fielded instance of this chapter's loop — AMCL as the belief, costmaps and a planner server as the cost-to-go, replanned continuously.

9. Thrun, S., Burgard, W., and Fox, D. (2005) Probabilistic Robotics. MIT Press.link to Probabilistic Robotics (opens in a new tab)

   The depth this chapter links to rather than repeats: histogram and particle filters, beam models, grid localization and MCL, occupancy grids, FastSLAM, MDPs and POMDPs. Its chapters 4, 6, 8, 9, 13, 15 and 16 are the sister volume's 8, 10, 12, 13, 17, 21 and 22.

10. Shannon Dynamics (2026) Probabilistic Robotics via Rust — Chapters 5, 8, 10, 12, 13, 17, 21, 22. Sister volume, web edition.link to Probabilistic Robotics via Rust — Chapters 5, 8, 10, 12, 13, 17, 21, 22 (opens in a new tab)

    The induction proof of the recursion (Ch. 5), histogram and particle filters with KLD sizing (Ch. 8), sensor models (Ch. 10), grid localization and Augmented MCL with the comparison table (Ch. 12), occupancy grids and MAP mapping (Ch. 13), FastSLAM (Ch. 17), and the value function and POMDP this chapter's planner approximates (Chs. 21–22). The filters this chapter wraps are vendored from there.