Kalman Filters

Two assumptions buy the Bayes filter a closed form. What comes back is the gain — not a tuning knob but a precision-weighted balance — a second coordinate system where the costs are reversed, and the first hint that waiting for the future beats recursion.

The problem with a grid

Rusty is on a rail. One degree of freedom, a beacon that pings its position, and a motor that never quite delivers the commanded step. The histogram filter of Chapter 5 will localize it perfectly well — at a thousand cells for centimetre resolution, recomputed every step, to describe a belief that is, every single time, one smooth bump.

That is the observation this chapter turns into an algorithm. If the belief is always going to be a single bump, stop storing the bump and store its mean and its width. Two numbers instead of a thousand, and the update becomes arithmetic.

Leave it running for a few cycles before reading on. Four things in that widget are the chapter.

The posterior is taller than both its parents — taller meaning narrower, since all four curves are densities. The green measurement blob and the orange prediction blob combine into a purple blob tighter than either. This is the sentence in the epigraph, and it is not a rounding artifact: two independent opinions genuinely beat one, and the algebra below says by exactly how much.

Prediction always widens; correction always narrows. Orange is never taller than the blue prior that preceded it. That is "moving smears" from Chapter 5, now in closed form: the covariance gains an additive $\mat{R}_t$ every step and there is nothing it can do about it.

The gain settles by itself. Watch the $K$ readout. It converges within a dozen steps and then holds, even though the measurements keep changing. Nothing told it to. We will prove why: the covariance recursion never looks at the data.

A confident filter is not an accurate one. Press divergent. The filter is told the cart obeys its model almost perfectly ($\mat{R} \approx 0$) while the cart in fact slips. Its covariance collapses, its 2σ band shrinks to a hairline, and the truth walks straight out of it. The NIS readout goes red well before the RMSE readout looks alarming — and NIS is the one you can compute on a real robot.

One implementation note, because it matters for Exercise 7. The bench carries two states — the cart's position and its velocity — while the beacon reports position only. The four blobs are the position marginal. Velocity is never measured at any point in the run, and the filter estimates it anyway, through the off-diagonal terms of $\Sigma$ that couple the two. That is the quiet superpower of a Gaussian belief: correlations turn one sensor into evidence about everything the sensor is correlated with.

Building intuition

Before any algebra, get the mechanism into your hands. The scalar case contains the entire idea, and the entire idea is a balance.

You have a prediction that says the cart is near $\bar\mu$, with uncertainty $\bar\sigma^2$. A beacon says it is near $z$, with uncertainty $\sigma_z^2$. Neither is right. The Kalman filter's answer is not to average them, and it is not to pick the more trustworthy one. It is to hang a weight at each claim whose mass is that claim's precision — the reciprocal of its variance — and take the centre of mass.

Two facts that widget makes physical, and that the derivations below make exact:

1. The posterior mean is always strictly between $\bar\mu$ and $z$. A Kalman filter can be wrong, but it cannot be wild.
2. The posterior precision is the sum of the two input precisions. Not the larger of them — the sum. That is why the purple curve is taller than both parents, and it is the whole reason fusing sensors is worth doing.

Everything else in this chapter is those two facts written in matrices.

The mathematics

Linear-Gaussian systems

The Kalman filter is the Bayes filter under three additional assumptions — linearity, Gaussian noise, and a Gaussian starting point — layered on top of the Markov assumption Chapter 5 already consumed.

Assumption 1 — linear motion with additive Gaussian noise.

so that

Assumption 2 — linear measurements with additive Gaussian noise.

Assumption 3 — Gaussian prior. $\htmlClass{term-prior}{\bel(x_0)} = \Normal(x_0; \mu_0, \Sigma_0)$.

Together these are exactly enough. Under them the posterior is Gaussian at every $t$ — not approximately, not asymptotically — so the filter never needs to represent anything but a mean and a covariance. That closure claim is what the next two derivations prove.

Prediction

The result, in the colors of the widget: the blue prior becomes the orange prediction by

Read the covariance line twice. $\mat{A}\Sigma\mat{A}\T$ is the old uncertainty dragged through the dynamics — for a contracting system it can shrink. The $+\mat{R}_t$ is the uncertainty this step creates, and since $\mat{R}_t$ is positive definite, that term only ever adds. In every system we will meet, prediction is a net loss.

Correction

Collecting the results, with the colors of the widget:

The quantity $z_t - \mat{C}_t\bar\mu_t$ is the innovation: the part of the measurement the filter did not already know. $S_t$ is how surprised the filter is entitled to be. Both names earn their keep later — Chapter 11 uses $S_t$ to gate outliers, and the consistency section below uses it to catch a filter lying about its own confidence.

The gain is precision-weighted trust

Specialize to one dimension with $\mat{C} = 1$. Then $S = \bar\sigma^2 + \sigma_z^2$ and

Substituting $K$ into the variance and inverting turns that last equation into the sentence the Gain Lever draws:

Precisions add. Not variances, not standard deviations — precisions. The posterior is more certain than either input, always, by an amount that does not depend on whether the two sources agreed. That is the epigraph, and it is also why the gain is not a knob: $K$ is nothing but the fraction of the total precision that the prediction contributed, so choosing it means choosing $\bar\sigma^2$ and $\sigma_z^2$, which are physical claims about a motor and a beacon.

The inversion lemma

Both derivations leaned on one identity. It is Thrun's Table 3.2, and it is more usually called the Sherman–Morrison–Woodbury formula.

Lemma. For invertible $\mat{U}$ and $\mat{V}$ and any $\mat{P}$ of compatible shape,

(Thrun states it with $\mat{R}$ and $\mat{Q}$ in those two roles; neutral letters here, since in this chapter those symbols are already taken.)

Verification. Multiply the right-hand side by $(\mat{U} + \mat{P}\mat{V}\mat{P}\T)$ and write $\Psi^{-1} = \mat{V}^{-1} + \mat{P}\T\mat{U}^{-1}\mat{P}$. The result is

using $\mat{P}\T + \mat{P}\T\mat{U}^{-1}\mat{P}\mat{V}\mat{P}\T = (\mat{V}^{-1} + \mat{P}\T\mat{U}^{-1}\mat{P})\mat{V}\mat{P}\T$. $\blacksquare$

The two places this chapter uses it: $\mat{U} = \mat{R}, \mat{P} = \mat{A}, \mat{V} = \Sigma$ in the prediction derivation, and $\mat{U} = \bar\Sigma^{-1}, \mat{P} = \mat{C}\T, \mat{V} = \mat{Q}^{-1}$ in the correction one.

The practical reading: it converts an $n \times n$ inversion into an $m \times m$ one. When a robot carries a 12-dimensional state and receives 2-dimensional range–bearing readings, that is the difference between inverting a 12×12 matrix and a 2×2 one, on every measurement, forever.

The algorithm

Lines 1–2 are prediction, lines 3–6 correction. This is Thrun's Table 3.1 with the innovation covariance given its own line, because every diagnostic in this chapter needs it. Line 6 is the form that appears in every textbook; the Rust below runs the Joseph variant of it instead, for the reason Step 7 of the derivation gives.

The other coordinate system

Step 3 of the correction derivation was an aside with a filter hidden in it. Written in the canonical parameters

the entire measurement update is

No gain. No innovation covariance. No inversion of anything the size of the state. Two additions.

The reason has a name: $\Omega$ is the Hessian of the negative log-belief, and log-densities of independent sources add. Fusing $k$ sensors in information form is a fold over $k$ matrix additions, in any order, with no intermediate normalization. That is why large multi-sensor systems are so often written this way.

The price appears on the other step. Prediction, which was a single matrix product in moments form, now requires going out to covariance and back:

The last three rows are the ones to remember, because they are the reason this section exists. In Chapter 14 a robot builds a map by keeping a covariance over its pose and every landmark, and that matrix becomes dense and enormous. Its inverse, it turns out, is nearly sparse: $\Omega_{ij}$ is non-zero only when landmarks $i$ and $j$ were seen from a common pose. Chapter 15 throws away the covariance entirely and works in the sparse information matrix forever, and the "expensive" prediction step stops being a problem because it is never performed — the whole trajectory is solved at once.

Is the filter telling the truth?

RMSE answers "how wrong is the estimate?" It cannot answer "does the filter know how wrong it is?" — and the second question is the one that predicts a crash. A filter that reports 5 cm of uncertainty while making 50 cm errors will reject the very measurements that could save it, because they fall outside its gate.

Two statistics answer it. Under a correct linear-Gaussian model, the normalized estimation error squared

is $\chi^2$ distributed with $n$ degrees of freedom, so $\E[\epsilon_t] = n$. And the normalized innovation squared

is $\chi^2$ with $m$ degrees of freedom, so $\E[\nu_t] = m$.

The difference between them is operational, and decisive. NEES needs $x_t$, so it exists only in simulation. NIS needs nothing the robot does not already have, so it runs on the real hardware, in flight, forever. Average either over $N$ steps and compare against the $\chi^2$ envelope with $Nn$ (or $Nm$) degrees of freedom, divided by $N$: outside the band, and the filter's covariance is a work of fiction. Above the band means overconfident, which is dangerous; below means conservative, which merely wastes information.

Go back to the tuning bench and run the three failure presets while watching the two meters. Each one moves a single slider away from balanced, and the three results are genuinely different diseases. Over a 400-step run, seeded and reproducible, they come out like this:

Sluggish claims the beacon is seven times noisier than it is, so it discounts every reading, lags every turn, and nearly triples its RMSE. NEES stays in band and NIS collapses to a seventh of its expected value: this filter is not lying, it is ignoring information, and NIS is the instrument that says so.

Jittery claims the cart slips nine times harder than it does. The prediction is worthless, so the filter follows the beacon almost exactly and inherits the sensor's noise — its RMSE is about what you would get by plotting the raw measurements. Yet both consistency statistics are fine. This is the case that matters most for how you tune: a filter can be honest and wasteful, and no amount of NIS-watching will find it. Only RMSE against known truth will.

Divergent is the dangerous one. Told the cart obeys its model almost perfectly, the covariance shrinks every step, the gain goes to zero, and the filter simply stops updating. Its own claimed σ ends the run at 0.05 m while it is 2 m from the truth. In the seeded run above, a sliding 40-step NIS average leaves its envelope at step 48; the same window's RMSE does not look alarming until step 66. Eighteen steps is not much, but it is the difference between a warning and a post-mortem — and unlike RMSE, NIS is available on the robot.

Implementation in Rust

The type is where the chapter's assumptions become enforceable. Three const generic parameters — state, control, and measurement dimension — mean that feeding a 4-state filter a 3×2 measurement matrix is a compile error, not a panic during a field trial.

use nalgebra::{SMatrix, SVector};
use pr_core::prob::Gaussian; // Ch. 2's moments-form belief — reused, not redeclared

/// A linear-Gaussian system and the belief it carries.
///
/// `N` = state dimension, `U` = control dimension, `M` = measurement dimension.
pub struct Kf<const N: usize, const U: usize, const M: usize> {
    pub a: SMatrix<f64, N, N>, // A_t  state transition
    pub b: SMatrix<f64, N, U>, // B_t  control
    pub c: SMatrix<f64, M, N>, // C_t  measurement
    /// R_t — **motion** noise, Thrun's convention. Most control texts call this Q.
    pub r: SMatrix<f64, N, N>,
    /// Q_t — **measurement** noise, Thrun's convention. Most control texts call this R.
    pub q: SMatrix<f64, M, M>,
    pub belief: Gaussian<N>,
}

/// Everything a correction learned, handed back for gating and diagnostics.
#[derive(Clone, Copy, Debug)]
pub struct Update<const N: usize, const M: usize> {
    pub innovation: SVector<f64, M>,
    pub s: SMatrix<f64, M, M>,
    pub gain: SMatrix<f64, N, M>,
    /// log p(z_t | z_{1:t-1}) = log N(innovation; 0, S) — Chapter 5's evidence,
    /// still free, and the same quantity Chapter 11 gates outliers with.
    pub log_evidence: f64,
}

impl<const N: usize, const U: usize, const M: usize> Kf<N, U, M> {
    /// Table 3.1, lines 1–2.
    pub fn predict(&mut self, u: &SVector<f64, U>) {
        self.belief.mu = self.a * self.belief.mu + self.b * u;
        // A Σ Aᵀ is the old uncertainty dragged through the dynamics — it can
        // shrink. + R is the uncertainty this step *creates*, and it cannot.
        self.belief.sigma = self.a * self.belief.sigma * self.a.transpose() + self.r;
        self.belief.symmetrize();
    }

    /// Table 3.1, lines 3–6, with the Joseph-form covariance.
    pub fn correct(&mut self, z: &SVector<f64, M>) -> Update<N, M> {
        let ct = self.c.transpose();
        let innovation = z - self.c * self.belief.mu;
        let s = self.c * self.belief.sigma * ct + self.q;
        let s_inv = s.try_inverse().expect("S_t singular: a measurement carries no noise");
        let gain = self.belief.sigma * ct * s_inv;

        self.belief.mu += gain * innovation;

        // Joseph form: (I − KC) Σ̄ (I − KC)ᵀ + K Q Kᵀ. Identical to (I − KC)Σ̄
        // at the optimal gain, but it is a *sum* of two positive-semidefinite
        // terms rather than a difference, so round-off cannot make it indefinite.
        let i_kc = SMatrix::<f64, N, N>::identity() - gain * self.c;
        self.belief.sigma =
            i_kc * self.belief.sigma * i_kc.transpose() + gain * self.q * gain.transpose();
        self.belief.symmetrize();

        // Ch. 2's Cholesky-based log-density, evaluated at the innovation.
        let log_evidence = pr_core::prob::log_normal_pdf(&innovation, &SVector::zeros(), &s);
        Update { innovation, s, gain, log_evidence }
    }
}

impl<const N: usize, const U: usize, const M: usize> bayes_core::BayesFilter for Kf<N, U, M> {
    type Belief = Gaussian<N>;
    type Control = SVector<f64, U>;
    type Measurement = SVector<f64, M>;

    fn predict(&mut self, u: &Self::Control) {
        Kf::predict(self, u)
    }

    fn correct(&mut self, z: &Self::Measurement) -> f64 {
        Kf::correct(self, z).log_evidence.exp()
    }

    fn belief(&self) -> &Self::Belief {
        &self.belief
    }
}

The information filter is the same struct with the belief stored the other way round. Notice how much shorter correct is than predict — the exact inverse of the shape above.

use nalgebra::{SMatrix, SVector};
use pr_core::prob::Gaussian;

/// The same belief in canonical coordinates: Ω = Σ⁻¹, ξ = Σ⁻¹µ.
pub struct InfoFilter<const N: usize, const U: usize, const M: usize> {
    pub xi: SVector<f64, N>,
    pub omega: SMatrix<f64, N, N>,
    pub a: SMatrix<f64, N, N>,
    pub b: SMatrix<f64, N, U>,
    pub c: SMatrix<f64, M, N>,
    pub r: SMatrix<f64, N, N>,
    pub q: SMatrix<f64, M, M>,
}

impl<const N: usize, const U: usize, const M: usize> InfoFilter<N, U, M> {
    /// Table 3.4, lines 1–2. Two n×n inversions: the expensive step *here*.
    pub fn predict(&mut self, u: &SVector<f64, U>) {
        let sigma = self.omega.try_inverse().expect("Ω must be positive definite");
        let mu = sigma * self.xi;
        let sigma_bar = self.a * sigma * self.a.transpose() + self.r;
        self.omega = sigma_bar.try_inverse().expect("Σ̄ must be positive definite");
        self.xi = self.omega * (self.a * mu + self.b * u);
    }

    /// Table 3.4, lines 3–4, with this filter's own sensor.
    pub fn correct(&mut self, z: &SVector<f64, M>) {
        let (c, q) = (self.c, self.q);
        self.add_information(z, &c, &q);
    }

    /// No gain, no S, nothing n×n inverted: the measurement's information is
    /// added in, and that is the entire update. Any sensor of any dimension `K`
    /// contributes through the same three lines, which is why fusion in
    /// information form is a fold rather than a special case.
    pub fn add_information<const K: usize>(
        &mut self,
        z: &SVector<f64, K>,
        c: &SMatrix<f64, K, N>,
        q: &SMatrix<f64, K, K>,
    ) {
        let ct_qinv = c.transpose() * q.try_inverse().expect("Q must be invertible");
        self.omega += ct_qinv * c;
        self.xi += ct_qinv * z;
    }

    pub fn to_moments(&self) -> Gaussian<N> {
        let sigma = self.omega.try_inverse().expect("Ω must be positive definite");
        Gaussian::new(sigma * self.xi, sigma)
    }

    /// Start from a moments-form belief and the same system matrices as a `Kf`.
    pub fn from_kf(kf: &Kf<N, U, M>) -> Self {
        let omega = kf.belief.sigma.try_inverse().expect("Σ must be positive definite");
        Self {
            xi: omega * kf.belief.mu,
            omega,
            a: kf.a,
            b: kf.b,
            c: kf.c,
            r: kf.r,
            q: kf.q,
        }
    }
}

The smoother needs the forward run kept rather than thrown away — which is the first time in this book that a filter's defining virtue, forgetting the past, is the thing standing in the way.

use nalgebra::{SMatrix, SVector};
use pr_core::prob::Gaussian;

/// One time step of a stored forward run: what the filter believed before the
/// measurement, after it, and the A_t that connected them.
#[derive(Clone, Copy)]
pub struct Step<const N: usize> {
    pub predicted: Gaussian<N>, // (µ̄_t, Σ̄_t)
    pub filtered: Gaussian<N>,  // (µ_t,  Σ_t)
    pub a: SMatrix<f64, N, N>,
}

/// Rauch–Tung–Striebel fixed-interval smoother.
///
/// One backward pass over a completed run. The middle line says everything:
/// correct each state by how wrong its own prediction of the future turned out
/// to be, scaled by how much that state was responsible for the prediction.
pub fn rts_smooth<const N: usize>(run: &[Step<N>]) -> Vec<Gaussian<N>> {
    let mut out: Vec<Gaussian<N>> = run.iter().map(|s| s.filtered).collect();

    for t in (0..run.len().saturating_sub(1)).rev() {
        let next = &run[t + 1];
        let l = run[t].filtered.sigma
            * next.a.transpose()
            * next.predicted.sigma.try_inverse().expect("Σ̄ must be invertible");

        out[t].mu = run[t].filtered.mu + l * (out[t + 1].mu - next.predicted.mu);
        out[t].sigma = run[t].filtered.sigma
            + l * (out[t + 1].sigma - next.predicted.sigma) * l.transpose();
    }
    out
}

/// NEES — needs ground truth, so it exists only in simulation. E[·] = N.
pub fn nees<const N: usize>(truth: &SVector<f64, N>, bel: &Gaussian<N>) -> f64 {
    let d = truth - bel.mu;
    let omega = bel.sigma.try_inverse().expect("Σ must be positive definite");
    (d.transpose() * omega * d)[(0, 0)]
}

/// NIS — needs only the innovation and S, so it runs on the real robot. E[·] = M.
pub fn nis<const M: usize>(innovation: &SVector<f64, M>, s: &SMatrix<f64, M, M>) -> f64 {
    let s_inv = s.try_inverse().expect("S must be invertible");
    (innovation.transpose() * s_inv * innovation)[(0, 0)]
}

A worked example you can check by hand

One dimension, one control, one measurement. The cart is commanded to move one metre per step, and a beacon reports its position.

Step one. The control $u_1 = 1$ gives $\bar\mu_1 = 0 + 1 = 1$ and $\bar\Sigma_1 = 1 + 0.25 = 1.25$. The beacon reports $z_1 = 1.7$, so $S_1 = 1.25 + 0.5 = 1.75$ and

The filter moves five sevenths of the way from its prediction to the reading: $\mu_1 = 1 + 0.714286 \times 0.7 = 1.5$, and $\Sigma_1 = (1 - 5/7)\times 1.25 = 2.5/7 \approx 0.357143$. That posterior variance is smaller than the prediction's 1.25 and smaller than the measurement's own 0.5 — the epigraph, in arithmetic you can do on paper.

Step two. $u_2 = 1$ gives $\bar\mu_2 = 2.5$, $\bar\Sigma_2 = 2.5/7 + 0.25 = 4.25/7 \approx 0.607143$. The beacon reports $z_2 = 2.1$, which is below the prediction, so $S_2 = 4.25/7 + 0.5 = 7.75/7$ and $K_2 = 4.25/7.75 \approx 0.548387$.

Notice the gain fell between the steps, from 0.714 to 0.548, without any instruction. The prediction got better — its variance dropped from 1.25 to 0.607 — so the measurement's share of the total precision shrank. Notice too that $\Sigma$ is converging: 1, 0.357, 0.274, heading for the fixed point of the Riccati recursion, which for these numbers is exactly $\Sigma_\infty = 0.25$ and $K_\infty = 0.5$. (Substitute: $\bar\Sigma = 0.25 + 0.25 = 0.5$, $S = 1$, $K = 0.5$, $\Sigma = 0.5 \times 0.5 = 0.25$. A fixed point you can verify in one line, and the reason the gain readout in the tuning bench stops moving.)

use approx::assert_relative_eq;
use ch06_kalman::{InfoFilter, Kf};
use nalgebra::{SMatrix, SVector};
use pr_core::prob::Gaussian;

/// The chapter's 1-D cart, built once so the example and the test cannot drift.
fn cart() -> Kf<1, 1, 1> {
    Kf {
        a: SMatrix::identity(),
        b: SMatrix::identity(),
        c: SMatrix::identity(),
        r: SMatrix::from_element(0.25), // motion noise      R
        q: SMatrix::from_element(0.50), // measurement noise Q
        belief: Gaussian::new(SVector::zeros(), SMatrix::identity()),
    }
}

fn main() {
    let mut kf = cart();
    for (t, (u, z)) in [(1.0, 1.7), (1.0, 2.1)].into_iter().enumerate() {
        kf.predict(&SVector::from_element(u));
        print!("step {}:  µ̄ = {:.6}  Σ̄ = {:.6}", t + 1, kf.belief.mu[0], kf.belief.sigma[(0, 0)]);
        let up = kf.correct(&SVector::from_element(z));
        println!(
            "   K = {:.6}   µ = {:.6}  Σ = {:.6}",
            up.gain[(0, 0)], kf.belief.mu[0], kf.belief.sigma[(0, 0)]
        );
    }
}

#[test]
fn worked_example_ch06_cart_1d() {
    let mut kf = cart();

    kf.predict(&SVector::from_element(1.0));
    assert_relative_eq!(kf.belief.mu[0], 1.0, epsilon = 1e-12);
    assert_relative_eq!(kf.belief.sigma[(0, 0)], 1.25, epsilon = 1e-12);

    let u1 = kf.correct(&SVector::from_element(1.7));
    assert_relative_eq!(u1.gain[(0, 0)], 5.0 / 7.0, epsilon = 1e-12);
    assert_relative_eq!(kf.belief.mu[0], 1.5, epsilon = 1e-12);
    assert_relative_eq!(kf.belief.sigma[(0, 0)], 2.5 / 7.0, epsilon = 1e-12);

    kf.predict(&SVector::from_element(1.0));
    assert_relative_eq!(kf.belief.mu[0], 2.5, epsilon = 1e-12);
    assert_relative_eq!(kf.belief.sigma[(0, 0)], 4.25 / 7.0, epsilon = 1e-12);

    let u2 = kf.correct(&SVector::from_element(2.1));
    assert_relative_eq!(u2.gain[(0, 0)], 4.25 / 7.75, epsilon = 1e-12);
    assert_relative_eq!(kf.belief.mu[0], 2.280_645_161_290_323, epsilon = 1e-9);
    assert_relative_eq!(kf.belief.sigma[(0, 0)], 0.274_193_548_387_096_8, epsilon = 1e-9);
}

/// The same two steps in canonical coordinates must land on the same posterior.
/// If this ever fails, one of the two derivations above is wrong.
#[test]
fn information_form_agrees() {
    let mut kf = cart();
    let mut inf = InfoFilter::from_kf(&kf);

    for (u, z) in [(1.0, 1.7), (1.0, 2.1)] {
        let (u, z) = (SVector::from_element(u), SVector::from_element(z));
        kf.predict(&u);
        kf.correct(&z);
        inf.predict(&u);
        inf.correct(&z);
    }

    let moments = inf.to_moments();
    assert_relative_eq!(moments.mu[0], kf.belief.mu[0], epsilon = 1e-9);
    assert_relative_eq!(moments.sigma[(0, 0)], kf.belief.sigma[(0, 0)], epsilon = 1e-9);
}

One more habit worth building here, because every later chapter reuses it. The book hand-rolls its filters — that is the point — but a hand-rolled filter with no external witness is a hand-rolled filter with an untested opinion. adskalman is a dev-dependency of ch06_kalman for exactly one purpose: to run the same matrices through somebody else's implementation and assert agreement to $10^{-9}$. It is not in the release build and never appears in a listing outside a #[cfg(test)] block. It is the cheapest available insurance against a transposed matrix — the class of bug that produces plausible numbers rather than a crash.

The failure gallery in the tuning bench is not hand-tuned for the widget either. It lives in the library, so the prose, the simulation, and the tests all cite the same four numbers.

/// A claim about how noisy the world is. The cart on the bench really has
/// `accel_sigma = 0.45` and `range_sigma = 0.32`; every other preset is a lie
/// of a particular shape.
#[derive(Clone, Copy, Debug)]
pub struct Tuning {
    /// R_t, as a white-acceleration σ in m/s².
    pub accel_sigma: f64,
    /// Q_t, as a range σ in m.
    pub range_sigma: f64,
}

pub const BALANCED: Tuning = Tuning { accel_sigma: 0.45, range_sigma: 0.32 };
/// Q enormous: the beacon is discounted, the estimate lags, NIS collapses.
pub const SLUGGISH: Tuning = Tuning { accel_sigma: 0.45, range_sigma: 2.40 };
/// R enormous: the model is discarded and the estimate inherits sensor noise.
/// Honest — both consistency tests pass — and wasteful.
pub const JITTERY: Tuning = Tuning { accel_sigma: 4.00, range_sigma: 0.32 };
/// R ≈ 0 while the cart really slips: Σ collapses and the filter stops listening.
pub const DIVERGENT: Tuning = Tuning { accel_sigma: 0.004, range_sigma: 0.32 };

#[test]
fn divergence_is_caught_by_nis_before_rmse() {
    let seeded = crate::bench::run(DIVERGENT, /* steps */ 400, /* seed */ 7);
    assert!(seeded.first_nis_out_of_band < seeded.first_rmse_alarm);
}

Putting it together: waiting for the future

A filter is causal by construction. Its estimate at time $t$ has seen $z_{1:t}$ and nothing more, because that is what "online" means. But once a run is over — a logged dataset, a mapping session, a post-flight analysis — the constraint is gone. The state at $t = 40$ can be re-estimated using the measurements from $t = 41$ onwards, and it should be, because those measurements are informative about it.