Kalman Filtering: The Geometric Observer

The Kalman filter as Choset teaches it — a Luenberger observer whose gain is re-chosen every step so the correction is the shortest Mahalanobis step onto the most likely measurement hyperplane — plus observability, EKF localization, and Kalman SLAM, compressed to what a planner needs.

The problem: a planner that does not know where it is

Put Rusty in the Apartment corridor and ask it to drive a loop on encoders alone. Each tick the wheels report how far they turned; integrate that and you have a position. The orange track below is that integral. The gray dashed track is where the robot actually went.

Two things go wrong, and they are different in kind. The right wheel is a fraction of a percent larger than the odometry believes, so every straight run bends into an arc — a bias, invisible to any amount of averaging. On top of it the wheels slip by a random amount each tick — noise, which random-walks the heading and, through the heading, the position. Choset's Figure 8.1 shows the same thing on a B21: a rotational error approaching 45° after one lap. The command-integration estimate "is continuously losing information about its location," and nothing in the integration can tell you how much.

The cure is also in the widget. Tick landmark fixes. Whenever a mapped corner comes within range, one range-and-bearing sighting pulls the estimate back and collapses the ellipse. That pull is the subject of this chapter: how hard to tug, in which direction, and what the tug can and cannot fix.

One sentence of bookkeeping, said once for all of Part IV. For Rusty the planner's configuration $q \in \SEtwo$ and the filter's state $x_t$ are the same object; the sister book writes $x_t, u_t, z_t$ and this book keeps Choset's $x(k), u(k), y(k)$ because his geometry reads better in it. The notation table below sits the two side by side so nobody translates twice.

Building intuition

The knob the filter turns

Strip the problem to Choset's §8.2.6 toy: a robot on a rail with state $x = [x_r, v_r]\T$, a force input, Newton's law sampled at $T = 0.5$ s, and a noisy velocity sensor. An observer for it is a copy of the dynamics plus a correction proportional to the innovation — the difference between what the sensor said and what it would have said had the prediction been exact:

$K$ is a knob. Turn it to zero and you have dead reckoning; turn it up and the estimate chases the sensor, noise and all; turn it far enough and the estimate oscillates and diverges. The Observer Bench lets you turn it. Then it lets the filter turn it for you.

Watch three things. The ellipse in the phase plane grows orange on every predict and squishes purple on every update, always toward the green line of states consistent with the reading. The dots on the unit disc are the eigenvalues of the error dynamics; with the velocity sensor one of them sits at exactly $1$ no matter what you do, and the position variance climbs forever. And when you tick let the filter choose, the gain stops being a constant: it becomes a sequence $R(k)$ that starts large, settles, and the "equivalent knob" readout drifts to $1$ on its own. The Kalman gain is not a tuning constant. It is a trajectory, and the knob you were turning is what the filter computes.

The picture

Here is the one picture the chapter is built on, drawn from the numbers of Choset's example. The orange ellipse is the predicted belief. The green line is $\Omega$: every state that would produce the reading exactly. Where should the estimate go?

The naive answer is the gray foot: drop perpendicularly onto $\Omega$, correct the velocity the sensor measured and leave the position alone. The right answer is the purple foot: perpendicular in the metric of the covariance. Position and velocity are correlated in $P$ — a robot that was going faster than we thought has also gone farther — so correcting the velocity should drag the position with it, and the correlated, more uncertain direction moves more. Add sensor noise and the line itself moves to $\Omega^*$ through the most likely output $y^*$; the Kalman update is the Mahalanobis foot on that line. Three derivations, one picture.

The mathematics

Definitions

An observer for the system $x(k{+}1) = F x(k) + G u(k)$, $y(k) = H x(k)$ is a recursion that reconstructs $x(k)$ from the known signals $u(\cdot), y(\cdot)$ and the model $(F, G, H)$ when $H$ is not invertible — which it usually is not, being $p \times n$ with $p < n$ (§8.2.1; App. J.4). The Kalman filter is both an observer and a filter: it reconstructs the state and suppresses the noise.

The innovation $\nu = y(k{+}1) - H\hat x(k{+}1 \mid k)$ is what the sensor said minus what it would have said had the prediction been exact. The consistent set $\Omega = \{x \in \R^n : H x = y(k{+}1)\}$ is the affine subspace of states that reproduce the output; its dimension is $n - p$ when $H$ has full row rank, which Choset assumes throughout §8.2.

The Mahalanobis distance $d_M(x, \hat x) = \norm{x - \hat x}_M$ with $\norm{x}_M^2 = x\T P(k{+}1 \mid k)^{-1} x$ measures distance in standard deviations under the predicted covariance (§8.2.3, footnote 5). It comes from an inner product $\langle x_1, x_2 \rangle_M = x_1\T P^{-1} x_2$, so "orthogonal" has a meaning under it.

A linear system is observable when the initial state is determined by $u, y$ over a finite window; for LTI systems, iff $\rank Q = n$ (App. J.4, Thm. J.4.1). The Luenberger observer (eq. J.9) is $\dot{\hat x} = A\hat x + Bu + K(y - C\hat x)$, with error dynamics $\dot e = (A - KC)e$; its discrete cousin is what the Bench runs.

Data association is the choice of $a(i)$, the landmark that explains measurement $i$ (§8.3.2).

The simple observer is an orthogonal projection

Prediction grows the ellipse; the update is the Mahalanobis-closest point

The Kalman filter: most likely output, then project

The Kalman filter is a Luenberger observer with a scheduled gain

This is the chapter's lens, and Choset implies it across §8.2 and Appendix J without stating it.

Observability: the rank test

Two hand-checked steps

Choset's §8.2.6 robot, with his numbers: $T = 0.5$, $m = 1$, $F = \bigl[\begin{smallmatrix}1 & 0.5\\ 0 & 1\end{smallmatrix}\bigr]$, $G = [0,\, 0.5]\T$, $u = 0$, $V = \bigl[\begin{smallmatrix}0.2 & 0.05\\ 0.05 & 0.1\end{smallmatrix}\bigr]$, $H = [0\ \ 1]$, $W = 0.5$, $\hat x(k \mid k) = [2, 4]\T$, $P(k \mid k) = \diag(1, 2)$. He reports no measurement value; this chapter fixes $y(k{+}1) = 2.7$ and $y(k{+}2) = 2.0$. These two readings are ours, not his.

Step 1. Predict: $\hat x^- = F[2, 4]\T = [4, 4]\T$ and $P^- = FPF\T + V = \bigl[\begin{smallmatrix}1.7 & 1.05\\ 1.05 & 2.1\end{smallmatrix}\bigr]$. Update: $\nu = 2.7 - 4 = -1.3$, $S = 2.1 + 0.5 = 2.6$, $R = [1.05,\ 2.1]\T / 2.6 = [0.4038,\ 0.8077]\T$, so $\nu / S = -0.5$ exactly and

Step 2. Predict: $\hat x^- = [4.95,\ 2.95]\T$, $P^- = \bigl[\begin{smallmatrix}1.7788 & 0.4538\\ 0.4538 & 0.5038\end{smallmatrix}\bigr]$. Update: $\nu = -0.95$, $S = 1.0038$, $R = [0.4521,\ 0.5019]\T$, $\hat x^+ = [4.520,\ 2.473]\T$, $P^+ = \bigl[\begin{smallmatrix}1.5737 & 0.2261\\ 0.2261 & 0.2510\end{smallmatrix}\bigr]$.

Read the diagonal. The velocity variance goes $2 \to 0.404 \to 0.251$: the squish of Figure 8.7. The position variance goes $1 \to 1.276 \to 1.574$: growing, through two optimal updates, because the sensor never sees position. That is Theorem 8.2.2 in two numbers.

EKF for range-and-bearing localization

The algorithm

Choset numbers none of Chapter 8's algorithms; the names below follow his section titles.

Choset's reading of $R$: if $R$ is "large" the sensor is more believable than the prediction and the update leans on it; if "small", the reading barely moves the estimate. Line 5 says exactly how large — the ratio of predicted uncertainty to total innovation uncertainty, in matrix form.

Choset minimizes $\chi^2_{ij}$ alone. When the candidates' gates differ in size this is not quite maximum likelihood: the $\log\det S_{ij}$ term matters, and the sister's mlAssociate, which the range–bearing path here delegates to, keeps it. The range-only path uses Choset's rule verbatim because the sister's routine assumes a bearing row.

The ellipse that never shrinks

Now the question the Probe was built to answer. Rusty drives a square in room A of the Apartment while the EKF above localizes it against known landmarks. How many does it need?

With one range-only landmark the rank of the linearized pair is $2$. The single range row and its products with $F$ all share the same $(x, y)$ direction — radial from the landmark — so the direction tangent to the constant-range circle never appears in $Q$. The posterior ellipse stretches along that circle and stays stretched: after 240 steps its trace is $0.482$ against $0.034$ with two landmarks, an order of magnitude, and no number of further iterations closes the gap. Switch to two non-collinear landmarks and the rank is $3$; put both landmarks and the robot on one line and it falls to $2$ again (Exercise 4). Range and bearing from one landmark is observable while the robot moves — the third column of $HF$ carries $u_1$ — and loses a rank the moment it stops: two numbers cannot pin three. The NEES tile reads near $3$ for the observable cases, which is what a consistent 3-DOF filter should report (sister Ch. 6 on NEES and NIS).

Kalman SLAM

If the landmarks are unknown, put them in the state (§8.4; Smith, Self and Cheeseman). For an omnidirectional vehicle measuring the relative displacement to $n_\ell$ uniquely identified landmarks, the system is linear: $x = [x_r, y_r, x_{\ell 1}, y_{\ell 1}, \dots]\T$, $F = I$, $G = [I_2; 0]$, $V = \operatorname{blkdiag}(V_r, 0, \dots)$, and each $H_i$ has $-1$ in the robot columns and $+1$ in landmark $i$'s. The sister's plain Kf runs it unchanged. Its observability matrix has rank $4$ of $6$ for two landmarks: the two null directions shift robot and map together. The filter learns the map's shape — in the check, the covariance of $x_{\ell 1} - x_r$ falls monotonically from trace $0.079$ to $0.020$ — while the map's absolute covariance stalls at $0.059$ and goes no lower. No sighting can say where the world is.

For range and bearing on the unicycle (§8.4.2) the state is $[x_r, y_r, \theta_r, x_{\ell 1}, \dots]$ and $H_i$ is eq. (8.47): dense in the first three columns, two nonzero entries at landmark $i$, zero elsewhere. Association is eq. (8.48) against every landmark in the state, and a minimum $\chi^2$ above a high threshold founds a new one. The forget the map toggle in the Probe runs this: three landmarks founded from their first sightings, $588$ of the remaining $597$ sightings matched, the worst map error $0.12$ m at $1.6\sigma$. Honest caveats: the covariance update is $O(n_\ell^2)$ per step, and on long loops the linearization makes the filter inconsistent. The sister book's Chapter 14 shows the cost and the inconsistency, and its Chapter 15 shows what replaced it.

Implementation in Rust

The estimate crate is thin by design: every file is either an adapter over the sister book's vendored filters or a piece Choset has and the sister does not. No filter is re-implemented.

use nalgebra::{DMatrix, SMatrix};

/// An LTI system in Choset's letters. Every field names the Thrun symbol the
/// sister book uses, because the two conventions collide in the worst place:
/// Choset's `R` is the Kalman *gain*; Thrun's `R` is the process *covariance*.
pub struct Lti<const N: usize, const U: usize, const M: usize> {
    pub f: SMatrix<f64, N, N>, // A_t
    pub g: SMatrix<f64, N, U>, // B_t
    pub h: SMatrix<f64, M, N>, // C_t, assumed full row rank
    pub v: SMatrix<f64, N, N>, // R_t  (process covariance)
    pub w: SMatrix<f64, M, M>, // Q_t  (measurement covariance)
}

impl<const N: usize, const U: usize, const M: usize> Lti<N, U, M> {
    /// App. J.5: F = I + T A, G = T B, H = C.
    pub fn discretize(a: SMatrix<f64, N, N>, b: SMatrix<f64, N, U>, c: SMatrix<f64, M, N>, t: f64,
                      v: SMatrix<f64, N, N>, w: SMatrix<f64, M, M>) -> Self {
        Self { f: SMatrix::identity() + t * a, g: t * b, h: c, v, w }
    }

    /// Thm. 8.2.2: Q = [H; HF; …; HF^{N−1}]. Dynamic because N·M is not a
    /// const expression — the one place this crate leaves the stack.
    pub fn observability_matrix(&self) -> DMatrix<f64> {
        let mut q = DMatrix::zeros(N * M, N);
        let mut hf = self.h;
        for k in 0..N {
            q.view_mut((k * M, 0), (M, N)).copy_from(&hf);
            hf = hf * self.f; // Cayley–Hamilton says stopping at N−1 loses nothing
        }
        q
    }

    /// rank Q == N under a *relative* SVD tolerance — a parameter, not a magic
    /// number: a 0/1 matrix needs none, a linearized Jacobian mixing metres
    /// and radians needs a generous one.
    pub fn is_observable(&self, tol: f64) -> bool {
        self.observability_matrix().rank(tol) == N
    }
}

The observer is the knob. Its step is three lines; error_eigs is the unit-disc inset, and place_gain is Theorem J.3.2 transposed — Ackermann's formula on the pair $(F, HF)$, which is why it can only succeed when $(F, H)$ is observable.

use nalgebra::{Complex, SMatrix, SVector};
use crate::lti::Lti;

/// Fixed-gain Luenberger observer, App. J.4 discretized by J.5:
///   x̂⁺ = F x̂ + G u + K (y − H(F x̂ + G u)).
pub struct Observer<const N: usize, const U: usize, const M: usize> {
    pub sys: Lti<N, U, M>,
    pub k: SMatrix<f64, N, M>,
    pub xhat: SVector<f64, N>,
}

impl<const N: usize, const U: usize, const M: usize> Observer<N, U, M> {
    pub fn step(&mut self, u: &SVector<f64, U>, y: &SVector<f64, M>) {
        let predicted = self.sys.f * self.xhat + self.sys.g * u;
        let nu = y - self.sys.h * predicted; // the innovation: what the sensor saw minus what it should have
        self.xhat = predicted + self.k * nu;
    }

    /// e(k+1) = (I − K H) F e(k): inside the unit disc ⇔ the error decays (Thm. J.5.1).
    pub fn error_matrix(&self) -> SMatrix<f64, N, N> {
        (SMatrix::identity() - self.k * self.sys.h) * self.sys.f
    }

    pub fn error_eigs(&self) -> Vec<Complex<f64>> {
        self.error_matrix().complex_eigenvalues().iter().copied().collect()
    }

    /// The error covariance a fixed gain *actually* produces (Joseph form) —
    /// valid for any K, not only the optimal one. The Kalman gain is the
    /// argmin of its trace; see `kf::kalman_step`, which never calls this.
    pub fn covariance_update(&self, p: &SMatrix<f64, N, N>) -> SMatrix<f64, N, N> {
        let ikh = SMatrix::<f64, N, N>::identity() - self.k * self.sys.h;
        ikh * p * ikh.transpose() + self.k * self.sys.w * self.k.transpose()
    }
}

The Kalman step itself is an adapter. filters::kalman::Kf is the sister book's filter, vendored unchanged; this function only renames its inputs into Choset's letters and hands back $(\nu, S, R)$ because data association and the widgets need them.

pub use filters::kalman::{Gaussian, Kf}; // sister Ch. 6 — not re-derived, not re-implemented
use nalgebra::{SMatrix, SVector};
use crate::lti::Lti;

/// One Choset step, eqs. 8.26–8.32, through the sister's `Kf`.
pub fn kalman_step<const N: usize, const U: usize, const M: usize>(
    sys: &Lti<N, U, M>, bel: &mut Gaussian<N>, u: &SVector<f64, U>, y: &SVector<f64, M>,
) -> (SVector<f64, M>, SMatrix<f64, M, M>, SMatrix<f64, N, M>) {
    let mut kf = Kf::from(bel.clone());
    kf.predict(&sys.f, &sys.v, Some((&sys.g, u)));            // Thrun: A, R, (B, u)
    let info = kf.update(y, &sys.h, &sys.w);                   // Thrun: C, Q
    *bel = kf.belief();
    (info.innovation, info.s, info.k)                          // Choset: ν, S, R
}

/// §8.2.6 constants: T = 0.5, m = 1, V, W = 0.5, x̂ = [2, 4], P = diag(1, 2).
pub fn dead_reckoning() -> (Lti<2, 1, 1>, Gaussian<2>) {
    let (t, m) = (0.5, 1.0);
    let sys = Lti {
        f: SMatrix::<f64, 2, 2>::new(1.0, t, 0.0, 1.0),
        g: SMatrix::<f64, 2, 1>::new(0.0, t / m),
        h: SMatrix::<f64, 1, 2>::new(0.0, 1.0),                 // velocity sensor
        v: SMatrix::<f64, 2, 2>::new(0.2, 0.05, 0.05, 0.1),
        w: SMatrix::<f64, 1, 1>::new(0.5),
    };
    (sys, Gaussian { x: SVector::<f64, 2>::new(2.0, 4.0), p: SMatrix::from_diagonal(&[1.0, 2.0].into()) })
}

use estimate::{kf::{dead_reckoning, kalman_step}, lti::Lti};
use nalgebra::{SMatrix, SVector};

fn main() {
    let (sys, mut bel) = dead_reckoning();
    for (k, y) in [2.7, 2.0].into_iter().enumerate() {
        let (nu, s, r) = kalman_step(&sys, &mut bel, &SVector::zeros(), &SVector::<f64, 1>::new(y));
        println!("step {}: ν = {:+.3}  S = {:.4}  R = [{:.4}, {:.4}]", k + 1, nu[0], s[(0, 0)], r[(0, 0)], r[(1, 0)]);
        println!("        x̂⁺ = [{:.3}, {:.3}]   P⁺ = [[{:.4}, {:.4}], [{:.4}, {:.4}]]",
            bel.x[0], bel.x[1], bel.p[(0, 0)], bel.p[(0, 1)], bel.p[(1, 0)], bel.p[(1, 1)]);
    }
    let pos = Lti { h: SMatrix::<f64, 1, 2>::new(1.0, 0.0), ..sys };
    println!("rank Q = {} (velocity sensor), {} (position sensor)",
        sys.observability_matrix().rank(1e-9), pos.observability_matrix().rank(1e-9));
}

step 1: ν = -1.300  S = 2.6000  R = [0.4038, 0.8077]
        x̂⁺ = [3.475, 2.950]   P⁺ = [[1.2760, 0.2019], [0.2019, 0.4038]]
step 2: ν = -0.950  S = 1.0038  R = [0.4521, 0.5019]
        x̂⁺ = [4.520, 2.473]   P⁺ = [[1.5737, 0.2261], [0.2261, 0.2510]]
rank Q = 1 (velocity sensor), 2 (position sensor)

#[test]
fn reproduces_choset_8_2_6_two_steps() {
    let (sys, mut bel) = dead_reckoning();
    kalman_step(&sys, &mut bel, &SVector::zeros(), &SVector::<f64, 1>::new(2.7));
    assert_relative_eq!(bel.x, SVector::<f64, 2>::new(3.475, 2.95), epsilon = 1e-3);
    kalman_step(&sys, &mut bel, &SVector::zeros(), &SVector::<f64, 1>::new(2.0));
    assert_relative_eq!(bel.p[(0, 0)], 1.5737, epsilon = 1e-3);
    assert_relative_eq!(bel.p[(1, 1)], 0.2510, epsilon = 1e-3);
    assert_eq!(sys.observability_matrix().rank(1e-9), 1);
}

/// Derivation 4 made executable: an `Observer` whose gain is set to R(k) each
/// step reproduces the Kalman mean bit-for-bit.
#[test]
fn kf_is_observer_with_scheduled_gain() {
    let (sys, mut bel) = dead_reckoning();
    let mut obs = Observer { sys: sys.clone(), k: SMatrix::zeros(), xhat: bel.x };
    for y in [2.7, 2.0, 2.4, 1.9] {
        let (_, _, r) = kalman_step(&sys, &mut bel, &SVector::zeros(), &SVector::<f64, 1>::new(y));
        obs.k = r;
        obs.step(&SVector::zeros(), &SVector::<f64, 1>::new(y));
        assert_eq!(obs.xhat, bel.x);
    }
}

The landmark models carry Choset's conventions — the bearing is measured from the landmark, a quirk the translation into the sister's Feature type absorbs with one rotation by $\pi$.

use filters::nonlinear::MeasurementModel; // sister Ch. 7
use geom::SE2;
use nalgebra::{SMatrix, SVector};
use sim::Landmark;

/// h_j = [ρ, atan2(y_r − y_ℓ, x_r − x_ℓ) − θ_r]; rows of ∂h/∂x: [Δx/ρ, Δy/ρ, 0], [−Δy/ρ², Δx/ρ², −1].
pub struct RangeBearing<'m> { pub map: &'m [Landmark] }
/// §8.3.3: the same model with the bearing row removed.
pub struct RangeOnly<'m> { pub map: &'m [Landmark] }

impl MeasurementModel<SE2, 3, 2> for RangeBearing<'_> {
    fn h(&self, x: &SE2, j: usize) -> SVector<f64, 2> {
        let (dx, dy) = (x.x() - self.map[j].x, x.y() - self.map[j].y);
        SVector::new((dx * dx + dy * dy).sqrt(), geom::wrap(dy.atan2(dx) - x.theta()))
    }
    fn jacobian(&self, x: &SE2, j: usize) -> SMatrix<f64, 2, 3> {
        let (dx, dy) = (x.x() - self.map[j].x, x.y() - self.map[j].y);
        let q = (dx * dx + dy * dy).max(1e-12);
        let rho = q.sqrt();
        SMatrix::new(dx / rho, dy / rho, 0.0, -dy / q, dx / q, -1.0)
    }
    /// The innovation wraps: the one place where z − h(x) is wrong.
    fn residual(&self, z: &SVector<f64, 2>, zhat: &SVector<f64, 2>) -> SVector<f64, 2> {
        SVector::new(z[0] - zhat[0], geom::wrap(z[1] - zhat[1]))
    }
}

/// §8.3.2: a(i) = argmin_j ν_ijᵀ S_ij⁻¹ ν_ij. `None` above `chi2_new` ⇒ found a new landmark (§8.4.2).
pub fn associate_ml(y_i: &SVector<f64, 2>, bel: &Gaussian<3>, map: &[Landmark],
                    w: &SMatrix<f64, 2, 2>, chi2_new: f64) -> Option<usize> {
    let model = RangeBearing { map };
    let pose = SE2::from_vector(&bel.x);
    let best = (0..map.len()).map(|j| {
        let h = model.jacobian(&pose, j);
        let s = h * bel.p * h.transpose() + w;
        let nu = model.residual(y_i, &model.h(&pose, j));
        (j, (nu.transpose() * s.try_inverse().expect("S is PD") * nu)[(0, 0)])
    }).min_by(|a, b| a.1.total_cmp(&b.1))?;
    (best.1 <= chi2_new).then_some(best.0)
}

Putting it together

The integration lab is the two widgets above, run in sequence on the Apartment, and it ends with the object Chapter 16 inherits.

Dead reckoning. Rusty's encoders through odometryDelta give Choset's input $u = [\Delta s, \Delta\theta]$ each tick. ChosetEkf::predict alone is Figure 8.1: the orange track in the hook bends off the gray one and the ellipse grows without bound. Its claimed $\sigma$ tracks the actual error only as long as the noise model is honest — the wheel-radius bias is not in the model, and when you raise it the error outruns the ellipse. That gap is what the sister book's NEES instrument measures, and the reason Chapter 16 will care about beliefs that are not Gaussian.

One landmark. In the Probe with one range-only landmark the rank reads $2$ and the ellipse's major axis runs along the constant-range circle. After 240 steps the pose trace is $0.482$. The filter is doing nothing wrong; "the problem lies with the system." A planner handed this belief should read the long axis and plan a motion that changes the geometry — toward a second landmark, or around the first — because no amount of sitting still will shrink it.

Two landmarks. Rank $3$, trace $0.034$, mean NEES $3.8$ against the expected $3$. This is the belief a planner wants: a Gaussian $\bel(x_t)$ with a covariance it can read, in the same $\SEtwo$ coordinates its configuration lives in.

Forget the map. Kalman SLAM founds the three landmarks from their first sightings and matches $588$ of $597$ later ones, with the worst landmark $0.12$ m off at $1.6\sigma$. The covariance is now $9 \times 9$ and grows by two rows per landmark; the correlations between landmarks are what let a re-sighting of one improve all the others. The state is also what makes the filter's eventual inconsistency possible, which is the sister book's story, not this one.

What a planner takes from here is small and exact: a mean in $\SEtwo$, a $3 \times 3$ covariance, an innovation covariance $S$ for each candidate sighting, and the rank test to tell whether the next action can even reduce the uncertainty. Chapter 16 asks what happens when the belief is not Gaussian, and how to plan on it.

Exercises

1. Foundation exerciseFDifficulty 2 of 3The covariance update (Choset's problem 7)

   Starting from $P^+ = \E[(x - \hat x^+)(x - \hat x^+)\T]$ and $\hat x^+ = \hat x^- + R\nu$, derive $P^+ = P^- - RHP^-$. Follow Choset's three steps: expand into $P^- - 2\,\E[R\nu(x - \hat x^-)\T] + \E[R\nu\nu\T R\T]$; show the middle term is $2RHP^-$; show $\E[\nu\nu\T] = HP^-H\T$ when the output is noiseless and mark that step — it is where §8.2.3's assumption enters, and where $+W$ appears in the §8.2.4 version. Then check that with $R = P^-H\T(HP^-H\T + W)^{-1}$ the Joseph form $(I - RH)P^-(I - RH)\T + RWR\T$ collapses to the same expression.

2. Foundation exerciseFDifficulty 2 of 3Product of Gaussians, and two gains that are one

   Prove Theorem 8.2.1 via Choset's problem 8: write the product of the exponents, collect the terms quadratic in $z$ to get $C_3^{-1} = C_1^{-1} + C_2^{-1}$, the terms linear in $z$ to get $C_3^{-1}z_3 = C_1^{-1}z_1 + C_2^{-1}z_2$, and use the matrix identity $(C_1^{-1} + C_2^{-1})^{-1} = C_1 - C_1(C_1 + C_2)^{-1}C_1$ to reach $\mathcal{Q} = C_1(C_1 + C_2)^{-1}$. Then show the sister book's gain $K = \bar\Sigma C\T(C\bar\Sigma C\T + Q)^{-1}$ and Choset's $R$ coincide under the notation table's map — and say in one sentence why the sister's $Q$ is Choset's $W$, not his $Q$.

3. Conceptual exerciseCDifficulty 2 of 3Predict the schedule's limit
   Predict first

   In the Observer Bench with the default V and W and the velocity sensor, tick 'let the filter choose' and wait. Where do the two eigenvalues of (I − R(k)H)F settle?

   Both inside the disc, near 0.6

   One at exactly 1, the other near 0.64

   Both at 1: the filter stops correcting

   With the velocity sensor and Choset's V, W, what is the steady-state velocity gain R₂(∞)?

   Check Reveal
4. Conceptual exerciseCDifficulty 2 of 3The collinear trap
   Predict first

   In the Observability Probe, switch to range-only, show two landmarks, and drag them so that both landmarks and the robot's path lie on one line. What does the rank panel read while the robot is on that line?

   3 — two landmarks are always enough

   2 — the direction across the line is blind

   1 — nothing is observable
5. Practical exercisePDifficulty 2 of 3The steady-state gain

   Implement Lti::steady_state_gain(&self, p0, max_iters, tol) -> (SMatrix<f64, N, M>, bool) by iterating the Riccati recursion in Joseph form until $P(k{+}1 \mid k)$ stops moving, returning the gain and whether it converged. Test that an Observer with that gain and the full Kalman filter agree in their means to $10^{-6}$ after 50 steps on the §8.2.6 system with the position sensor, and that with the velocity sensor the recursion reports non-convergence while the gain itself still settles. The TypeScript riccatiSteadyState and gainSchedule in web/lib/estimate/kf.ts are the port.

6. Practical exercisePDifficulty 3 of 3Choset's problem 9 as a test (stretch)

   Implement the unicycle with a position-only sensor $y = x_1 + w$, $\Var w = 0.5$, no process noise, and run one EKF step from $\hat x(1 \mid 1) = [1, 0.5, \pi/4]\T$ with $u = [3, \pi]$, $T = 0.25$, $P(1 \mid 1) = I$, $y(2) = 1.7$. Print $\hat x(2 \mid 2)$ and $P(2 \mid 2)$, then run the same step through the sister's UKF and report the gap.

   What is x̂₁(2 | 2), the corrected x-position?

   Check Reveal

   Stuck? Get a hint