Localization I: Tracking with Gaussians

EKF localization is Chapter 7's filter with Chapters 9 and 10 plugged in — an assembly, not a new algorithm. The hard part is the discrete variable nobody writes down: which landmark am I looking at?

The problem

Here is a working localizer. Rusty drives laps of the Apartment corridor; the filter is the real EkfLocalizer from the library, the world generates noisy range–bearing detections of surveyed landmarks, and the purple ellipse is the posterior it reports.

Watch it for one lap before touching anything. Then do exactly one thing: press close pair.

Under the dense preset the filter is good. Position RMSE over four hundred steps is 0.094 m, the ellipse swells while the robot crosses a landmark-free stretch — which is 42% of them — and snaps tight when a feature comes back into the cone, and the NEES trace scatters around 3 the way a truthful three-degree-of-freedom filter should. Four hundred and thirteen features are matched and every one of them is matched correctly.

Under the close pair preset, two of the surveyed landmarks sit 0.30 m apart — the two edges of a doorframe, entered into the map as separate features. Nothing else changes: same filter, same noise, same trajectory, same seed. And the run falls apart — but not in the way readers usually expect. It does not get noisier. It gets confident and wrong, which is a strictly worse failure, because every number the filter publishes downstream still looks healthy.

Three claims follow from that one demonstration, and the rest of the chapter earns them.

The estimator is not the hard part. Everything in the prediction and correction steps is assembly of parts you already have. We will still derive it in full, because every implementer transcribes those Jacobians by hand at least once, but there is no new idea in it.

The hard part is a discrete variable. Deciding what you are looking at is not estimation; it is a combinatorial choice made under uncertainty, and the Gaussian machinery has nothing to say about it. What it does provide is a metric in which to make the choice.

Distance must be measured in the currency of your own uncertainty. Not metres. The correct question is never "which landmark is closest?" but "which landmark, given how wrong I might be, best explains this reading?" — and the answers differ, systematically and predictably.

Which localization problem is this?

Before deriving anything, it is worth being precise about what "localization" means, because it names at least six different problems of wildly different difficulty. Thrun's taxonomy splits them along three axes, and a Gaussian filter is honestly applicable to exactly one cell.

The three axes are worth stating in the book's own words.

Local versus global. Position tracking assumes the initial pose is known and the error stays small; the posterior is unimodal and a Gaussian is a fair summary of it. Global localization starts from a uniform prior — the robot is somewhere in the building, and after one door sighting the belief has one peak per door. Kidnapped robot is global localization plus a lie: the robot is confident about a pose it does not occupy. As the baseline puts it, in global localization the robot knows that it does not know where it is; a kidnapped robot does not even have that.

Static versus dynamic. In a static world the robot's pose is the only thing that changes. In a dynamic one, people walk and chairs move, and a fraction of every scan corresponds to nothing in the map. That is not extra noise: it is correlated extra noise, which is the one kind a Bayes filter cannot absorb (Chapter 5 showed why).

Passive versus active. A passive localizer estimates whatever the controller happens to drive past. An active one chooses where to go in order to learn — trading path length for certainty. That is a decision problem, and it waits for Chapter 22 and Chapter 24. But the option is worth naming here, because w11.4 below makes its value visible in a single button press.

This chapter lives in the top-left cell: tracking, static, passive. Chapter 12 takes the rest of the top row.

Markov localization

The formal umbrella over all six cells is one line of Chapter 5 with a map $m$ added to every conditional:

That is the whole content of "Markov localization": the Bayes filter, told about a map. Everything interesting is in the choice of representation for $\bel$, and in the initial condition — which is precisely the taxonomy above:

Notice that tracking and kidnapping have the same functional form. A filter cannot tell them apart by looking at its own belief. It can only tell them apart by looking at the evidence $\eta^{-1} = p(z_t \mid z_{1:t-1}, u_{1:t}, m)$ — the diagnostic Chapter 5 promised would come back. It comes back twice in this chapter: as the χ² gate, and as the mixture weight.

The mathematics

Notation this chapter adds

Prediction: the velocity model, differentiated

The state is a pose $x_t = (x, y, \theta)\T$, and the control is a commanded translational and rotational velocity $u_t = (v_t, \omega_t)\T$ held for $\Delta t$. Chapter 9 integrated that exactly, as an arc of radius $v/\omega$:

The covariance needs two Jacobians, because the uncertainty has two sources: the uncertainty already in the pose, and the uncertainty in what the wheels actually did.

The term $V_t M_t V_t\T$ deserves a sentence, because it is where the 2005 presentation improves on the earlier draft (which simply wrote an additive 3×3 $R_t$). Motion noise does not live in pose space. It lives in control space: the robot commanded $(v, \omega)$ and the wheels delivered something else. $M_t$ is 2×2, so $V_t M_t V_t\T$ is a 3×3 matrix of rank 2. There is one direction in pose space along which the velocity model claims perfect certainty — and Chapter 9 already told us which one, when it introduced the third noise term $\hat\gamma$: a final rotation that no $(v, \omega)$ pair can produce. If you do not add it back, a filter running this model believes it knows its heading better than it possibly can. The lab widget above adds it as an explicit floor $\diag(0, 0, (\alpha_5 v^2 + \alpha_6 \omega^2)\Delta t^2)$, which is exactly the variance the Chapter 9 sampler injects.

Correction: the landmark model, differentiated

Chapter 10's front end reports features $f_t^i = (r_t^i, \phi_t^i, s_t^i)\T$. Given a correspondence $c_t^i = j$, the predicted measurement of map landmark $m_j = (m_{j,x}, m_{j,y})$ from the predicted pose is pure trigonometry. Write $\delta = m_j - \bar\mu_{t,xy}$ and $q = \delta\T\delta$:

Read the two useful rows of $H_t^j$ before moving on; they explain most of what the widgets show.

The range row is the negative unit vector pointing from the robot to the landmark, and it has a zero in the $\theta$ column: turning in place does not change how far away something is. So a range measurement constrains position along the line of sight and nothing else.

The bearing row has $-1$ in the $\theta$ column whatever the range, and position terms that decay as $1/\sqrt{q}$. A distant landmark is therefore almost a pure compass: it pins heading tightly and position hardly at all. A near landmark is almost a pure position fix. This is why a sparse map of far-away beacons produces a filter with excellent heading and drifting position, and why the ellipse in the lab is long along the corridor rather than round.

The signature row is identically zero, which is Thrun's own observation: once the correspondence is known, the signature carries no information about the pose. It earns its place only in the next section, where the correspondence is not known.

Two deviations from Thrun's Table 7.2 are deliberate and both come from Chapter 7.

First, lines 11–12 apply each feature sequentially, re-linearizing $H_t$ against the updated mean, where the book sums $\sum_i K_t^i(z_t^i - \hat z_t^i)$ and $\big(I - \sum_i K_t^i H_t^i\big)$ at the end. The summed form is a first-order approximation to the sequential one and is only equivalent when the corrections are infinitesimal. Sequential costs nothing extra and is strictly better conditioned; Exercise 5 makes you measure the difference.

Second, $\bplus$ and $\bminus$ are not decoration. The measurement residual $z_t^i \bminus \hat z_t^i$ wraps its bearing component to $(-\pi, \pi]$; without that, a predicted bearing of $179°$ and an observed $-179°$ produce an innovation of $358°$, a gain-scaled correction that hurls the estimate across the map, and a filter that fails exactly once per lap at exactly the same place. The mean update likewise wraps $\theta$. This is the smallest possible instance of the manifold discipline, and it is the single most common bug in hand-written EKF localizers.

A worked example you can check by hand

Put Rusty at $\bar\mu_t = (2,\ 3,\ 0)$ with

and a single landmark at $m_j = (5, 3)$ — three metres straight ahead. The sensor has $\sigma_r = 0.2$ m and $\sigma_\phi = 0.05$ rad, so $Q_t = \diag(0.04,\ 0.0025)$.

Then $\delta = (3, 0)$, $q = 9$, $\sqrt q = 3$, and everything below is arithmetic you can do on paper.

$S_t$ came out diagonal, which is not a coincidence: with an isotropic position block the range and bearing rows of $H$ are orthogonal. The predicted spreads are $\sigma_{r,\text{pred}} = 0.539$ m and $\sigma_{\phi,\text{pred}} = 0.201$ rad — note that the predicted range spread is more than twice the sensor's own $0.2$ m, because most of it is the robot's ignorance about where it is, not the sensor's about what it saw.

Now suppose the robot measures $z_t = (3.2,\ 0.05)$: twenty centimetres farther than expected, and fifty milliradians to the left. The innovation is $\nu = (0.2,\ 0.05)$, so

comfortably inside any sane gate. The correction is $K_t \nu = (-0.1724,\ -0.1034,\ -0.01241)$:

Every number in that posterior tells you something.

The robot moved backwards in $x$, because it measured a longer range than expected and the landmark is straight ahead. $\sigma_x$ collapsed from 0.5 m to $\sqrt{0.03448} = 0.186$ m — the range measurement acted almost entirely along the line of sight, exactly as the first row of $H$ promised. $\sigma_y$ fell only to 0.279 m and $\sigma_\theta$ to 0.087 rad, because the bearing measurement had to explain a lateral offset and a heading error at once and cannot tell them apart: that inability is the $-0.0207$ correlation term, and it is negative because moving the robot down and turning it left produce the same bearing.

Data association: the actual problem

The algorithm above took $c_t$ as an input. Nothing supplies it.

The maximum-likelihood answer is to pick, for each observed feature, the landmark that makes the reading least surprising:

and then to accept it only if it survives a gate:

The set $\{z : d_M^2(z, \hat z^j) \le \gamma\}$ is the validation region — an ellipse in measurement space, drawn in orange in the widget below. It is the geometric object the whole section is about.

Spend a minute dragging the green dot before reading on, because the widget makes three points that are hard to make in prose.

The gate is not round, and its shape is a prediction. The robot's belief there is $\bar\Sigma = \diag(0.64,\ 0.04,\ 0.0025)$ — 0.8 m of uncertainty along the heading, 0.2 m across it, the shape a corridor always produces — so the validation ellipses come out long in range and thin in bearing. Landmark m0 sits exactly 4 m straight ahead, which makes it checkable by hand: $S^{m0} = \diag(0.64 + 0.04,\ 0.04/16 + 0.0025 + 0.0025) = \diag(0.68,\ 0.0075)$. Its 95% gate therefore extends $\sqrt{5.99 \times 0.68} = 2.02$ m along the line of sight and $\sqrt{5.99 \times 0.0075} = 0.212$ rad across it, which at 4 m is 0.85 m of arc. That is a 2.4 : 1 ellipse, and any two landmarks closer together than about two metres along the ray are candidates for the same reading.

Euclidean and Mahalanobis disagree over a large region. Toggle the metric and drag. Take the reading $z = (3.55\ \text{m},\ 0.05\ \text{rad})$, which the autoplay tour passes through. Its projected endpoint is 0.488 m from landmark m0 and 0.466 m from m1 — so a nearest-neighbour rule in metres picks m1. In the filter's own metric, $d_M^2(m0) = 0.631$ and $d_M^2(m1) = 1.872$, so maximum likelihood picks m0, by a factor of three. Both cannot be right, and only one of them used the information the filter has been carefully accumulating for the last hundred steps.

The $\ln\det S$ term is a real term, not a constant. It is dropped in Thrun's Table 7.3 and in most implementations, on the grounds that maximizing a Gaussian density and minimizing a Mahalanobis distance are the same thing. They are the same thing only when the candidates share a covariance. When they do not, the log-determinant penalizes candidates whose gates are wide — a landmark whose validation ellipse is twice as large in each axis pays $2\ln 4 \approx 2.77$, which outweighs a full standard deviation of mismatch. Toggling it in the widget moves the decision boundary between m0 and m1 by a visible margin. It rarely overturns a clear winner; it always moves the boundary, and the boundary is precisely where a filter with two close landmarks lives.

Line 9 is the one line Table 7.3 does not have, and it is the difference between a filter that survives a corridor and one that does not. Gaussians fall off exponentially, so a single outlier can dominate the entire correction; the baseline's own practical-considerations section is blunt about the remedy — "In practice, thresholding adds an important layer of robustness to the algorithm without which EKF localization tends to be brittle."

But notice what the gate can and cannot do. It rejects readings that are surprising. It is completely blind to readings that are unsurprising and wrong.

Why one wrong association is catastrophic

Here is the same worked example, poisoned. Everything is as before — Rusty at $(2, 3, 0)$ with $\bar\Sigma = \diag(0.25, 0.25, 0.01)$, a mapped landmark at $(5, 3)$ — except that the feature was generated by a different object at $(5.6, 3)$, which the surveyor never recorded. The robot measures a range of $3.6$ m and the filter attributes it to the landmark at $(5,3)$.

Step 1. $\nu = (0.6, 0)$, so $d_M^2 = 0.36 / 0.29 = 1.24$. The gate is 5.99. The wrong match is accepted, and it is not even close. The gate's range half-width here is $\sqrt{5.99 \times 0.29} = 1.32$ m; a landmark 0.6 m out of place is less than half of that. This is arithmetic, not bad luck.

The correction is $K\nu = (-0.517, 0, 0)$, so $\mu_x$ moves to $1.483$ while the truth is still $2$: an error of 0.517 m. And $\sigma_x$ shrinks from 0.5 m to 0.186 m. The filter is now wrong by 2.8 of its own standard deviations, and its NEES is 7.76 against an expectation of 3.

Step 2. The robot observes the same object again from the same place. Its predicted range to the landmark at $(5,3)$ is now $5 - 1.483 = 3.517$, and the measurement is again $3.6$. Innovation: $0.083$. Predicted spread: $\sqrt{0.0345 + 0.04} = 0.273$. So

The reading now looks better than a typical correct one. This is the mechanism, and it is worth saying out loud: the filter has moved to the pose that makes its own mistake look right. Error grows to 0.556 m, $\sigma_x$ falls to 0.136 m, NEES reaches 16.7.

Step 3. $d_M^2 = 0.034$. Error 0.570 m, $\sigma_x = 0.113$ m, NEES 25.6, and climbing.

Hedging: multi-hypothesis tracking

If the filter cannot decide safely, it can decline to decide. Represent the belief as a Gaussian mixture over association histories:

Each component is a full EKF localizer conditioned on one history of choices. When a feature has several landmarks inside its gate, the component branches, once per candidate, and each child takes its parent's weight scaled by how well that candidate explained the reading:

Line 7 matters more than it looks. A hypothesis that can explain nothing must pay for that, or it lives forever at its old weight and the mixture never collapses. Charging it a clutter density is the standard device, and it is what turns "no landmark fits" from silence into evidence.

MHT is where Gaussian localization runs out of road, and it is worth being blunt about why. It represents ambiguity by enumerating it, which works when the ambiguity is two landmarks and fails when it is "somewhere in this building". Its modern successors split the problem in two: for representation, Chapter 12's particle filters carry arbitrary multi-modality for a fixed cost; for association, Chapter 15's robust factor graphs keep the whole trajectory available so a bad match can be undone later rather than merely hedged against. Recent work makes the discrete variable a first-class citizen of the optimization — discrete-continuous factor graphs (Doherty et al.), or semidefinite relaxations that solve for pose and association simultaneously and certifiably (Korotkine et al.). Every one of those is a descendant of the score function in this chapter.

Implementation in Rust

The crate layout follows the chapter. localize owns the filter; motion and sensor are the Chapter 9 and 10 crates, imported rather than reimplemented.

crates/localize/src/
  lib.rs            // the Localizer trait, implemented three more times in Ch. 12
  ekf/mod.rs        // EkfLocalizer: predict, correct_known, correct
  ekf/jacobians.rs  // G_t, V_t, M_t, H_t — one function per equation
  ekf/associate.rs  // Mahalanobis, χ² gates, ml_associate
  ekf/mht.rs        // feature "mht": branch, weight, prune, merge

The Jacobians

use nalgebra::{Matrix2, Matrix2x3, Matrix3, Matrix3x2, Vector2};
use pr_core::geom::se2::SE2;
use motion::{Alphas, VelocityCmd};
use sensor::Landmark;

/// Below this |ω| the arc formulae are 0/0. A robot in a corridor commands
/// exactly this, so the limit is the common case, not the edge case.
const STRAIGHT: f64 = 1e-6;

/// M_t: control noise, in *control* space. Rank 2 by construction — which is
/// why it needs V_t to reach pose space, and why the γ̂ term of Chapter 9 has
/// to be added separately.
pub fn control_noise(u: &VelocityCmd, a: &Alphas) -> Matrix2<f64> {
    let (v2, w2) = (u.v * u.v, u.omega * u.omega);
    Matrix2::new(
        a.0[0] * v2 + a.0[1] * w2, 0.0,
        0.0,                       a.0[2] * v2 + a.0[3] * w2,
    )
}

/// (G_t, V_t) for the exact-arc velocity model, evaluated at μ_{t−1}.
pub fn motion_jacobians(mu: &SE2, u: &VelocityCmd) -> (Matrix3<f64>, Matrix3x2<f64>) {
    let (c, s) = (mu.theta().cos(), mu.theta().sin());
    let dt = u.dt;

    if u.omega.abs() < STRAIGHT {
        let g = Matrix3::new(
            1.0, 0.0, -u.v * dt * s,
            0.0, 1.0,  u.v * dt * c,
            0.0, 0.0,  1.0,
        );
        // The ∂/∂ω column needs the *second* order term: the first order of
        // (sin(θ+ωΔt) − sin θ)/ω cancels, leaving −½ v Δt² sin θ.
        let v_jac = Matrix3x2::new(
            dt * c, -0.5 * u.v * dt * dt * s,
            dt * s,  0.5 * u.v * dt * dt * c,
            0.0,     dt,
        );
        return (g, v_jac);
    }

    let nt = mu.theta() + u.omega * dt;
    let (cn, sn) = (nt.cos(), nt.sin());
    let r = u.v / u.omega;
    let w = u.omega;

    let g = Matrix3::new(
        1.0, 0.0, r * (-c + cn),
        0.0, 1.0, r * (-s + sn),
        0.0, 0.0, 1.0,
    );
    let v_jac = Matrix3x2::new(
        (-s + sn) / w,  u.v * (s - sn) / (w * w) + u.v * cn * dt / w,
        ( c - cn) / w, -u.v * (c - cn) / (w * w) + u.v * sn * dt / w,
        0.0,            dt,
    );
    (g, v_jac)
}

/// ẑ and H for one landmark. Returned together because they are always used
/// together and both need the same δ and q — computing them apart is how the
/// two drift out of sync during a refactor.
pub fn landmark_prediction(mu: &SE2, lm: &Landmark) -> (Vector2<f64>, Matrix2x3<f64>) {
    let dx = lm.x - mu.x();
    let dy = lm.y - mu.y();
    let q = dx * dx + dy * dy;
    let sq = q.sqrt();

    let z_hat = Vector2::new(sq, wrap_pi(dy.atan2(dx) - mu.theta()));
    let h = Matrix2x3::new(
        -dx / sq, -dy / sq,  0.0,
         dy / q,  -dx / q,  -1.0,
    );
    (z_hat, h)
}

/// Wrap to (−π, π]. Used on every bearing residual, without exception.
#[inline]
pub fn wrap_pi(a: f64) -> f64 {
    let x = (a + std::f64::consts::PI).rem_euclid(std::f64::consts::TAU);
    x - std::f64::consts::PI
}

Gates and association, with the dimension in the type

Association code is where measurement dimensions get mixed up: a range–bearing feature is 2-D, a range–bearing–signature feature is 3-D, and a gate built for one is silently wrong for the other. Const generics make that a compile error.

use nalgebra::{SMatrix, SVector};
use statrs::distribution::{ChiSquared, ContinuousCDF};

/// Everything the filter predicts about one landmark, before seeing anything.
pub struct Prediction<const Z: usize> {
    pub landmark: usize,
    pub z_hat: SVector<f64, Z>,
    pub s: SMatrix<f64, Z, Z>,
}

/// A χ² gate. The dimension is in the type, so a 2-D gate cannot be applied to
/// a 3-D innovation — the mistake that produces a filter which "works but
/// rejects too much" and takes a week to find.
#[derive(Clone, Copy, Debug)]
pub struct Gate<const Z: usize> {
    pub gamma: f64,
}

impl<const Z: usize> Gate<Z> {
    /// γ = χ²_{Z, p}. At Z = 2: 5.991 for p = 0.95, 9.210 for p = 0.99.
    pub fn at_confidence(p: f64) -> Self {
        let chi2 = ChiSquared::new(Z as f64).expect("Z ≥ 1");
        Self { gamma: chi2.inverse_cdf(p) }
    }

    #[inline]
    pub fn accepts(&self, d2: f64) -> bool {
        d2 <= self.gamma
    }
}

/// νᵀ S⁻¹ ν, via the Cholesky factor rather than an explicit inverse: same
/// answer, half the flops, and it fails loudly if S is not positive definite.
pub fn mahalanobis2<const Z: usize>(nu: &SVector<f64, Z>, s: &SMatrix<f64, Z, Z>) -> f64 {
    let chol = s.clone().cholesky().expect("S = H Σ Hᵀ + Q is positive definite");
    let y = chol.l().solve_lower_triangular(nu).expect("L is nonsingular");
    y.norm_squared()
}

/// ln det S, from the same factor: 2 Σ ln L_ii.
pub fn log_det<const Z: usize>(s: &SMatrix<f64, Z, Z>) -> f64 {
    let chol = s.clone().cholesky().expect("S is positive definite");
    2.0 * chol.l().diagonal().iter().map(|d| d.ln()).sum::<f64>()
}

#[derive(Clone, Copy, Debug, PartialEq)]
pub enum Association {
    Matched { landmark: usize, d2: f64 },
    /// Kept rather than discarded: the *closest* candidate and its distance are
    /// what you need to debug a filter that is rejecting everything.
    Outlier { nearest: usize, d2: f64 },
}

/// arg-min over landmarks of d²_M + ln det S, accepted only if it passes the
/// gate. `residual` is a parameter because a bearing must be wrapped and a
/// range must not — the caller owns that knowledge, not this function.
pub fn ml_associate<const Z: usize>(
    z: &SVector<f64, Z>,
    preds: &[Prediction<Z>],
    gate: &Gate<Z>,
    residual: impl Fn(&SVector<f64, Z>, &SVector<f64, Z>) -> SVector<f64, Z>,
) -> Association {
    let mut best = f64::INFINITY;
    let mut best_idx = usize::MAX;
    let mut best_d2 = f64::INFINITY;

    for p in preds {
        let nu = residual(z, &p.z_hat);
        let d2 = mahalanobis2(&nu, &p.s);
        // The log-determinant term is *not* optional when candidates have
        // different gate sizes. Dropping it is the most common silent bug in
        // nearest-neighbour association.
        let score = d2 + log_det(&p.s);
        if score < best {
            best = score;
            best_idx = p.landmark;
            best_d2 = d2;
        }
    }

    if gate.accepts(best_d2) {
        Association::Matched { landmark: best_idx, d2: best_d2 }
    } else {
        Association::Outlier { nearest: best_idx, d2: best_d2 }
    }
}

The dimension really is enforced. Feeding a 3-D feature to a 2-D gate does not compile:

let gate = Gate::<2>::at_confidence(0.95);
let z3: SVector<f64, 3> = feature.with_signature();
let a = ml_associate(&z3, &preds3, &gate, wrap_bearing_row);

error[E0308]: mismatched types
  --> src/ekf/associate.rs:98:37
   |
98 |     let a = ml_associate(&z3, &preds3, &gate, wrap_bearing_row);
   |             ------------              ^^^^^ expected `&Gate<3>`, found `&Gate<2>`
   |             |
   |             arguments to this function are incorrect
   |
   = note: expected reference `&Gate<3usize>`
              found reference `&Gate<2usize>`

That is the Chapter 7 tradition continued: the type system knows the difference between a 2-DOF χ² threshold and a 3-DOF one, and it should, because a human staring at 5.99 versus 7.81 will not.

The localizer

use nalgebra::{Matrix2, Matrix3, Vector2, Vector3};
use pr_core::geom::se2::SE2;
use motion::{Alphas, VelocityCmd};
use sensor::{Feature, Landmark};

use super::jacobians::{control_noise, landmark_prediction, motion_jacobians, wrap_pi};
use super::associate::{ml_associate, Association, Gate, Prediction};

/// A Gaussian over pose: mean on SE(2), covariance in (x, y, θ).
#[derive(Clone, Debug)]
pub struct GaussianBelief {
    pub mean: SE2,
    pub cov: Matrix3<f64>,
}

pub struct EkfLocalizer {
    pub bel: GaussianBelief,
    pub landmarks: Vec<Landmark>,
    /// Sensor noise diag(σ_r², σ_φ²).
    pub q: Matrix2<f64>,
    pub alphas: Alphas,
    pub gate: Gate<2>,
    /// The γ̂ floor of Chapter 9: heading noise that V M Vᵀ structurally cannot
    /// represent, because M has rank 2 and this direction is not in its range.
    pub gamma_alphas: (f64, f64),
}

impl EkfLocalizer {
    /// Table 7.2, lines 1–2.
    pub fn predict(&mut self, u: &VelocityCmd) {
        let (g, v) = motion_jacobians(&self.bel.mean, u);
        let m = control_noise(u, &self.alphas);

        let mut cov = g * self.bel.cov * g.transpose() + v * m * v.transpose();
        let gamma_var =
            (self.gamma_alphas.0 * u.v * u.v + self.gamma_alphas.1 * u.omega * u.omega)
                * u.dt * u.dt;
        cov[(2, 2)] += gamma_var;

        self.bel.mean = u.integrate(&self.bel.mean); // exact arc, Chapter 9
        self.bel.cov = symmetrize(cov);
    }

    /// Table 7.2, lines 4–13, for one feature whose correspondence is given.
    pub fn correct_known(&mut self, f: &Feature, j: usize) -> f64 {
        let (z_hat, h) = landmark_prediction(&self.bel.mean, &self.landmarks[j]);
        let nu = residual(&Vector2::new(f.r, f.phi), &z_hat);
        self.apply(&nu, &h)
    }

    /// Table 7.3: predict every landmark, choose by ML, gate, apply, repeat.
    /// Returns one verdict per feature so a caller can plot precision/recall.
    pub fn correct(&mut self, features: &[Feature]) -> Vec<Association> {
        let mut out = Vec::with_capacity(features.len());

        for f in features {
            // Re-linearised for *every* feature, so this feature's gate already
            // reflects the information the previous one contributed. It is also
            // why the order features arrive in stops mattering.
            let preds = self.predictions();
            let z = Vector2::new(f.r, f.phi);
            let assoc = ml_associate(&z, &preds, &self.gate, residual);
            out.push(assoc);

            if let Association::Matched { landmark, .. } = assoc {
                let (z_hat, h) = landmark_prediction(&self.bel.mean, &self.landmarks[landmark]);
                let nu = residual(&z, &z_hat);
                self.apply(&nu, &h);
            }
        }
        out
    }

    fn predictions(&self) -> Vec<Prediction<2>> {
        self.landmarks
            .iter()
            .enumerate()
            .map(|(k, lm)| {
                let (z_hat, h) = landmark_prediction(&self.bel.mean, lm);
                Prediction { landmark: k, z_hat, s: h * self.bel.cov * h.transpose() + self.q }
            })
            .collect()
    }

    /// K = Σ Hᵀ S⁻¹; μ ⊞ Kν; Σ ← (I − KH)Σ. Returns d²_M, the NIS of this update.
    fn apply(&mut self, nu: &Vector2<f64>, h: &nalgebra::Matrix2x3<f64>) -> f64 {
        let s = h * self.bel.cov * h.transpose() + self.q;
        let s_inv = s.try_inverse().expect("S is positive definite");
        let k = self.bel.cov * h.transpose() * s_inv;

        let dx: Vector3<f64> = k * nu;
        // ⊞, not +: the third component is an angle and lives on a circle.
        self.bel.mean = self.bel.mean.boxplus(&dx);
        self.bel.cov = symmetrize((Matrix3::identity() - k * h) * self.bel.cov);

        (nu.transpose() * s_inv * nu)[(0, 0)]
    }
}

/// z ⊟ ẑ: the range component subtracts, the bearing component wraps.
fn residual(z: &Vector2<f64>, z_hat: &Vector2<f64>) -> Vector2<f64> {
    Vector2::new(z[0] - z_hat[0], wrap_pi(z[1] - z_hat[1]))
}

#[inline]
fn symmetrize(m: Matrix3<f64>) -> Matrix3<f64> {
    0.5 * (m + m.transpose())
}