X0.3.4

Lesson 0.3.4 — Implementing the Kalman Filter in PyTorch: Forms, Numerics, Gating, and the Bridge to Nonlinear

Week 4 · Prerequisites: 0.3.1, 0.3.2, 0.3.3, 0.1.1, 0.1.2, 0.1.3.

Prior results used. This lesson uses 0.3.1’s Gaussian conditioning and Schur complement, 0.3.2’s log-likelihood and precision, 0.3.3’s KF predict/update and RTS smoother, 0.1.1’s tensor shapes and broadcasting, 0.1.2’s autograd and no_grad, and 0.1.3’s nn.Module registration and state_dict. Each is restated at the point of use.

Hands forward. This lesson hands forward one capability: the learner can implement a numerically robust, outlier-gated Kalman filter in PyTorch, choose between the covariance and information forms, verify the filter’s covariance stays symmetric and positive-definite, and state the EKF’s linearization as the KF’s natural nonlinear extension — the capability that the entire Phase 1 state-estimation curriculum (and the SLAM, tracking, and control lessons in Phases 2–4) assumes.

Running case. The same 6-DOF MuJoCo arm from 0.2.1–0.3.3; the state is the 2D x_t = [q₂(t), q̇₂(t)]ᵀ, the dynamics are the A = [[1, dt], [0, 1]] constant-velocity model, the observation is the noisy joint-2 encoder, and one extra complication is added: the encoder occasionally reports gross outliers (a stuck bit, a transient electrical fault, or a missed reading). The lesson’s central result is that the covariance form’s naive update drifts from Σ = Σᵀ ⪰ 0 in finite precision, the Joseph form preserves it, and Mahalanobis gating rejects the outliers before they corrupt the estimate.

§L.0 — Lesson map, objectives, and misconceptions

§L.0.1 — Lesson map

§L.0.2 — Objectives

By the end of this lesson you will be able to do eight things:

1. Write the KF’s predict and update in both the covariance and information forms, and state the conditions under which each is preferred (state dimension, measurement dimension, sparsity, initial uncertainty).
2. Derive the information form’s update Y = Y⁻ + HᵀR⁻¹H from 0.3.1’s precision-sum, and state the rank-|y| structure that makes it cheap for sparse H.
3. State why the covariance form’s (I − KH)Σ⁻ drifts from symmetry and positive-definiteness in finite precision, and write the Joseph form (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ, which is symmetric and PSD by construction.
4. Diagnose the three numerical failure modes — a non-symmetric Σ (drifting from Σᵀ), a non-PSD Σ (negative eigenvalue), and a singular S (from R = 0 with a rank-deficient HΣ⁻Hᵀ) — from the symptom each produces.
5. Apply the Mahalanobis gate d² = νᵀS⁻¹ν to reject outliers, compute the chi-squared threshold for a given confidence level and measurement dimension, and state the two failure modes of the gate (too loose, too tight).
6. State the EKF’s linearization (A_t = ∂f/∂x, H_t = ∂h/∂x evaluated at the current estimate), and state the two ways it fails (linearization error at large deviations, divergence when the estimate is far from the truth).
7. State the unscented transform’s sigma points and the particle filter’s sampling, and choose the estimator for a given nonlinearity’s severity (linear → KF; mild → EKF; moderate → UKF; severe/multimodal → PF).
8. Produce a numerically robust, outlier-gated KF implementation in PyTorch — a KalmanFilter nn.Module with state_dict-serializable parameters — and verify that Σ remains symmetric, PSD, and bounded over a 1,000-step run with outliers.

§L.0.3 — Misconception inventory

Each misconception is displaced in the section named.

Everything is self-contained. Where a prerequisite lesson is used, the result is restated at the point of use.

§L.0.5 — Retrieval warm-up

This is the twelfth lesson. The prompt requires drawing from lessons 2–5 steps back; 0.3.1–0.3.3 are 1–3 steps back and are excluded. The warm-up draws from 0.2.4 (four steps back), 0.2.1 (seven steps back), and 0.1.2 (eleven steps back, but a prerequisite of this lesson).

Warm-up 1 (from 0.2.4, the frame audit). A multi-frame pipeline has transforms T_A^B, T_B^C, and their composition. What is the composition’s rule, and how does the “effect in the code” pattern state it? Answer: T_A^C = T_A^B · T_B^C, with the intermediate frame B canceling; the “effect in the code” pattern writes p_A = T_AB @ p_B and p_B = T_BC @ p_C, and the composition is p_A = T_AB @ (T_BC @ p_C) = (T_AB @ T_BC) @ p_C. How this is used: §L.2’s information form and §L.3’s Joseph form are frame-agnostic matrix operations, but the state’s vector space and the measurement’s vector space must be consistently defined — the H matrix is the “effect” of the state on the measurement, and the wrong H is the frame audit’s failure mode.

Warm-up 2 (from 0.2.1, the inverse of SE(3)). For T = (R, t), what is T⁻¹, and how is the inverse’s translation’s frame different from t’s? Answer: T⁻¹ = (Rᵀ, −Rᵀt), and the inverse’s translation is in the child frame, while t is in the parent frame. How this is used: §L.5’s EKF’s A matrix is the Jacobian of the state transition (the f function), and the Jacobian’s frame must match the state’s frame; a frame mismatch in A is the EKF’s “wrong A” bug (not in the four canonical bugs but adjacent to them).

Warm-up 3 (from 0.1.2, autograd). A scalar loss L(μ, Σ) depends on a matrix Σ that is a leaf with requires_grad=True. Does .backward() populate Σ.grad? Answer: Yes, if Σ is a leaf with requires_grad=True and the operations from Σ to L are differentiable. Σ.grad is the gradient of the scalar with respect to the matrix, and it has the same shape as Σ. How this is used: §L.3’s Joseph form’s Σ can be a leaf if the learner wants to learn the filter’s Q or R (a differentiable KF), and the gradient flows through the Joseph form’s (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ chain. The Σ’s gradient has the same shape, and the chain’s differentiability is what makes the differentiable filter possible.

Warm-up 4 (from 0.1.1, the shape of a matrix product). A (2, 2) matrix A multiplied by a (2,) vector x gives a (2,) result. What is the shape of Aᵀ? Of x A? Answer: Aᵀ is (2, 2); x A is ill-shaped (a (2,) row times a (2, 2) matrix is (2,), but the semantic is xᵀ A, which is (1, 2) @ (2, 2) = (1, 2)). How this is used: §L.3’s Joseph form’s KRKᵀ is a (2, 1) @ (1, 1) @ (1, 2) = (2, 2) product, and the shape must match Σ’s (2, 2); the K is (2, 1), the R is (1, 1), and the Kᵀ is (1, 2) — the outer product is the (2, 2) correction.

Part A — The Covariance and Information Forms

§L.1 — Objects: the two forms, their matrices, and their equivalent updates

The covariance form (the standard KF) stores (μ, Σ) and updates:

Predict: μ⁻ = Aμ + Bu, Σ⁻ = AΣAᵀ + Q. Update: S = HΣ⁻Hᵀ + R, K = Σ⁻HᵀS⁻¹, μ = μ⁻ + K(y − Hμ⁻), Σ = (I − KH)Σ⁻.

The information form stores (ŷ, Y) where Y = Σ⁻¹ (the information matrix) and ŷ = Yμ (the information vector), and updates:

Predict: Y⁻ = (A Y⁻¹ Aᵀ + Q)⁻¹, ŷ⁻ = Y⁻ A Y⁻¹ ŷ — the same cost as the covariance form’s predict (both require a matrix inversion of the A Y⁻¹ Aᵀ + Q). Update: Y = Y⁻ + HᵀR⁻¹H, ŷ = ŷ⁻ + HᵀR⁻¹y — no S⁻¹; the update is the sum of the prior’s information and the measurement’s information, and the HᵀR⁻¹H is a rank-|y| n × n matrix (the measurement’s dimension is |y|).

The key difference is the update:

• Covariance form: inverts S (an |y| × |y| matrix), then multiplies by Σ⁻Hᵀ (n × |y|). Cost: O(|y|³ + n|y|²).
• Information form: adds HᵀR⁻¹H (n × n but rank |y|), then computes Y⁻¹ for the next predict. Cost: O(n|y|²) for the update, plus the inversion at the next predict.

The choice depends on:

The information form’s Y₀ = 0 is the “uninformative prior” — the state is completely unknown, and the first measurement fully determines it. The covariance form cannot represent Σ₀ = ∞ without a regularization (a large but finite Σ₀), which introduces a bias (the prior is not truly flat). The information form handles the uninformative prior exactly, which is why SLAM (where the initial map is unknown) often uses the information form.

§L.1.1 — Result: the two forms are algebraically equivalent; the choice is the sparsity structure and the initial uncertainty

Claim. For the linear-Gaussian model, the covariance form’s (μ, Σ) and the information form’s (ŷ, Y) satisfy Y = Σ⁻¹ and ŷ = Yμ at every step, and the two forms’ updates produce the same (μ, Σ) when converted. The choice between them is computational, not algebraic.

Proof sketch. The information form’s update Y = Y⁻ + HᵀR⁻¹H is the matrix version of 0.3.1’s precision-sum 1/σ² = 1/σ₀² + 1/σ_y²; substituting Y⁻ = (Σ⁻)⁻¹ and Y = Σ⁻¹ and multiplying by Σ on both sides gives I = Σ(Σ⁻)⁻¹ + ΣHᵀR⁻¹H, which rearranges to Σ = Σ⁻ − Σ⁻Hᵀ(HΣ⁻Hᵀ + R)⁻¹HΣ⁻ (the Woodbury identity), which is the covariance form’s (I − KH)Σ⁻ with K = Σ⁻Hᵀ(HΣ⁻Hᵀ + R)⁻¹. The mean’s equivalence is the same algebra applied to ŷ, and the two forms are the same filter in different coordinates.

Corollary. The information form’s Y⁻¹ at the predict step is the only inversion; the update is inversion-free. For a system with many measurements per state (|y| ≫ n), the information form’s update is O(n|y|²) vs. the covariance form’s O(|y|³) — a factor of |y|/n in the information form’s favor. For sparse H (each measurement observes one state, as in SLAM’s landmark observations), the information form’s update preserves the sparsity and the total cost is linear in the number of measurements, while the covariance form’s S becomes dense and the cost is cubic.

Corollary. The uninformative prior (Y₀ = 0, Σ₀ = ∞) is representable exactly in the information form and not in the covariance form. A learner who uses the covariance form with a very large Σ₀ (e.g., 1e10) approximates the uninformative prior with a finite one, and the approximation introduces a bias in the first estimate (the prior’s mean μ₀ is weighted by the tiny precision 1e-10, so the bias is negligible but non-zero). The information form’s Y₀ = 0 is exact, and its first estimate is the pure measurement (no prior).

§L.1.2 — Worked-example ladder

WE-L.1.a — Fully worked: the same one-step update in both forms

Input. Scalar model A = 1, H = 1, Q = 0.01, R = 0.04. Prior μ = 0.5, Σ = 0.01. Measurement y = 0.7.

Trace (covariance form).

1. Σ⁻ = A·Σ·Aᵀ + Q = 1·0.01·1 + 0.01 = 0.02, μ⁻ = 1·0.5 = 0.5.
2. S = H·Σ⁻·Hᵀ + R = 0.02 + 0.04 = 0.06.
3. K = Σ⁻·Hᵀ/S = 0.02/0.06 ≈ 0.3333.
4. μ = 0.5 + 0.3333·(0.7 − 0.5) = 0.5667.
5. Σ = (1 − K·H)·Σ⁻ = (1 − 0.3333)·0.02 ≈ 0.01333.

Trace (information form).

6. Y⁻ = 1/Σ⁻ = 1/0.02 = 50, ŷ⁻ = Y⁻·μ⁻ = 50·0.5 = 25.
7. Y = Y⁻ + Hᵀ·R⁻¹·H = 50 + 1/0.04 = 50 + 25 = 75.
8. ŷ = ŷ⁻ + Hᵀ·R⁻¹·y = 25 + 1/0.04·0.7 = 25 + 17.5 = 42.5.
9. Σ = 1/Y = 1/75 ≈ 0.01333, μ = ŷ/Y = 42.5/75 ≈ 0.5667.

Output and interpretation. The two forms give the same (μ, Σ) = (0.5667, 0.01333). The covariance form inverts S = 0.06 (1×1); the information form adds HᵀR⁻¹H = 25 to Y⁻ (a scalar addition). For a 1×1 problem, the costs are identical; for n = 1000 and |y| = 10, the information form’s 10² update is 10⁵ operations vs. the covariance form’s 10³ for S⁻¹ plus 10³·10 = 10⁴ for the gain — a factor of ~10 in the information form’s favor for the update.

Self-explanation 1. Before reading on: in step 7, the information form’s Y = 75 is the sum of Y⁻ = 50 and HᵀR⁻¹H = 25. What does this sum’s relative size tell you about the prior’s and the measurement’s precisions?

Answer. The prior’s precision (Y⁻ = 50) is twice the measurement’s (25), so the prior is more precise; the posterior’s precision (75) is the sum, and the posterior’s variance (1/75) is the harmonic combination of 1/50 and 1/25. The ratio 50/25 = 2 is the prior’s relative precision, and the posterior’s mean is closer to the prior (the gain is 0.333 < 0.5), which is consistent with the prior being more precise.

Self-explanation 2. Before reading on: the covariance form’s step 5 Σ = (1 − K)Σ⁻ is a subtraction, while the information form’s step 7 Y = Y⁻ + HᵀR⁻¹H is an addition. Are the two forms numerically equivalent?

Answer. Algebraically yes, numerically no — the subtraction in step 5 is catastrophic cancellation when K ≈ 1 (the measurement is very precise), because 1 − K and Σ⁻ are both O(1) and the difference Σ is O(1/K) or smaller. The information form’s addition of positive terms is stable. This is one reason the information form is preferred for precise measurements, and it is the numerical difference that the algebraic equivalence hides.

WE-L.1.b — Completion problem

Input. A 2D state with a 1×2 H = [1, 0] (position-only observation), Σ⁻ = [[0.01, 0.005], [0.005, 0.04]], R = 0.01. Measurement y = 0.7, predicted μ⁻ = [0.5, 0.1].

Trace (final 25% as prompts).

1. S = H·Σ⁻·Hᵀ + R. ⟨your turn: compute⟩
2. K = Σ⁻Hᵀ/S. ⟨your turn⟩
3. μ = μ⁻ + K(y − Hμ⁻). ⟨your turn⟩
4. The information form’s HᵀR⁻¹H. ⟨your turn: what is its shape, and what is its rank?⟩

Answers.

1. HΣ⁻Hᵀ = Σ⁻[0, 0] = 0.01. S = 0.01 + 0.01 = 0.02.
2. Σ⁻Hᵀ = [0.01, 0.005]ᵀ. K = [0.5, 0.25]ᵀ.
3. y − Hμ⁻ = 0.7 − 0.5 = 0.2. μ = [0.5 + 0.5·0.2, 0.1 + 0.25·0.2] = [0.6, 0.15].
4. HᵀR⁻¹H = [1, 0]ᵀ·(1/0.01)·[1, 0] = [[100, 0], [0, 0]] — a (2, 2) matrix with rank 1 (the measurement observes only the first component, so the update is rank-1 in the information form).

The lesson. The information form’s update’s HᵀR⁻¹H is rank-|y| (1 here), and the covariance form’s K is the same rank (1 column) — the two are the same update in different coordinates, and the rank is the measurement’s dimension. The rank-1 update is why the information form is cheap for a small number of measurements per state.

WE-L.1.c — Faded: the information form with an uninformative prior

Setup. The same 2D model, but Σ₀ = ∞ (the prior is flat).

Question. What is Y₀? What is the first update’s Y and μ? Can the covariance form represent Σ₀ = ∞?

You must produce the intermediate steps. The setup and the answer are given.

Answer. Y₀ = 0 (the information matrix is zero, because ∞⁻¹ = 0). The first update’s Y = Y₀ + HᵀR⁻¹H = 0 + [[100, 0], [0, 0]] = [[100, 0], [0, 0]], and ŷ = 0 + HᵀR⁻¹y = [100·0.7, 0] = [70, 0]. Σ = Y⁻¹ — but Y is singular (rank 1), so the covariance does not exist (∞ in the unobserved direction). The information form’s μ = ŷ/Y is undefined in the unobserved direction (0/0), and the state is only observed in the first component. The covariance form cannot represent Σ₀ = ∞ without a regularization (Σ₀ = 1e10), and the regularized version’s Σ is [[100, 0], [0, 0]]⁻¹-like, which is ill-conditioned.

The lesson. The information form handles the uninformative prior exactly (the Y = 0 is well-defined, and the update’s Y is the rank-|y| HᵀR⁻¹H), while the covariance form cannot represent ∞. The cost is that the information form’s Y can be singular (rank-deficient), and the mean is only partially determined — the null space of Y is the unobserved state’s directions. For full observability (the measurements eventually observe every state), the accumulated Y becomes non-singular after enough updates, and the covariance form becomes representable. The choice is the observability: the information form handles the transient unobservability, and the covariance form needs a finite prior.

WE-L.1.d — Contrastive near-miss: the covariance form with a huge but finite prior

Correct solution. The information form with Y₀ = 0 (the uninformative prior) gives the exact first update.

Near-miss. The covariance form with Σ₀ = 1e10·I approximates the uninformative prior, and the first update’s μ is the precision-weighted average of μ₀ (the prior’s mean, arbitrary) and y.

Why the near-miss produces a plausible but incorrect number. The huge-Σ₀ prior is not flat — it is a very broad Gaussian with the arbitrary mean μ₀. The first update’s μ is μ₀ + K(y − Hμ₀) with K ≈ (HΣ₀Hᵀ + R)⁻¹HΣ₀ ≈ H⁻¹ (in the square case), so μ ≈ H⁻¹y — the measurement — but the residual μ₀ − H⁻¹y is weighted by the tiny Σ₀⁻¹, and the bias is O(1/Σ₀) — small but non-zero. For Σ₀ = 1e10, the bias is 1e-10, which is negligible for a single update but accumulates over many updates, and the accumulated bias is the difference between the huge-Σ₀ and the true uninformative prior. The symptom is a tiny systematic error that is invisible in a single step and visible over a long trajectory.

The discrepancy, stated. Σ₀ = 1e10 and Σ₀ = ∞ are not the same prior: the former is a broad Gaussian with a mean, the latter is a flat distribution with no mean. The information form represents the latter exactly, and the covariance form represents it only approximately. The error is small per step and systematic, and the fix is either the information form or a very large Σ₀ with the awareness that it is an approximation.

Self-explanation 3. Before reading on: in the near-miss, if the prior’s mean μ₀ is exactly H⁻¹y (the measurement’s implied state), does the huge-Σ₀ prior give the same first update as the uninformative prior?

Answer. Yes — if μ₀ = H⁻¹y (the prior’s mean equals the measurement’s estimate), the update’s μ = μ₀ + K(y − Hμ₀) = μ₀ (the innovation is zero), and the huge-Σ₀ prior’s mean is unchanged. The uninformative prior’s first update is H⁻¹y (the measurement), which equals μ₀ by assumption, so the two coincide. The coincidence is a special case (the prior’s mean happens to match the measurement), and the generic case has a non-zero innovation, which makes the huge-Σ₀ and the uninformative priors different.

§L.1.3 — Comparative table: the covariance and information forms

Design principle (P12.6). Choose the form by the sparsity of H and the prior’s information, not by the state’s dimension. The two forms are algebraically the same filter, so the choice is a statement about structure: a measurement that observes one landmark out of thousands leaves HᵀR⁻¹H sparse and Y sparse, while the covariance form’s S fills in and its cost grows with the state. Conversely, a state whose covariance is the wanted output — a tracking filter feeding a controller, a gate that needs S — pays an inversion for every step it spends in the information form. The rule is mechanical and checkable before implementation: write down the density of H and whether Σ or Y is the quantity the next consumer reads.

§L.1.4 — Concept check

Concept check (displaces M1). A learner says: “The information form is just the covariance form inverted, so the two are interchangeable — I’ll pick whichever has fewer lines of code.” What has the learner failed to model, and what does the resulting bug look like?

Answer. The learner has failed to model that algebraic equivalence is not computational equivalence: the two forms produce the same (μ, Σ) when converted, and the conversions are where the costs and the failure modes live. Y = Σ⁻¹ is a full n × n inversion, so an implementation that stores Y and inverts it every step to get Σ — and then inverts back for the next update — has paid the inversion twice and gained nothing. The bug class is equivalence read as interchangeability: the code is correct, the numbers match the reference, and the cost is a factor of n worse than either form’s design intends, which shows up only when the state grows. The second half of the same error is representational: the information form’s Y₀ = 0 and its ability to be singular are not properties the covariance form shares, so a learner who treats the forms as interchangeable will regularize a prior that did not need regularizing and will invert a Y that is rank-deficient by design.

§L.1.5 — Consolidation checkpoint 1

(a) You should now be able to: (i) write both forms’ predict and update, and state where the S⁻¹ does and does not appear; (ii) count each form’s update cost in terms of n and |y|; (iii) state which form handles Σ₀ = ∞ exactly and why the other cannot; (iv) state the numerical difference between a subtraction and an addition of positive terms.

(b) Transfer probe. A SLAM back-end tracks n = 10,000 landmarks with a camera that observes about |y| = 200 of them per frame, each observation touching one landmark. The engineer implements the covariance form and finds the step time grows as the map grows. Answer sketch: the covariance form inverts S (200 × 200, manageable) but forms HΣ⁻Hᵀ, and with Σ⁻ dense — which it becomes as soon as the landmarks are correlated through the shared trajectory — that product fills in and the update’s cost tracks the state’s density rather than the measurement count. The information form’s update adds HᵀR⁻¹H, a rank-|y| matrix that touches only the observed landmarks’ entries, so the per-step cost is set by |y| and the sparsity of Y survives the update. The cost of the switch is that Y’s predict requires an inversion, so the form is chosen for the update-heavy loop the back-end actually runs — and the diagnosis is to print nnz(Σ⁻) and nnz(Y) after a hundred frames and watch which one stays sparse.

(c) Remediation rule. If you cannot do (b), return to §L.1.2 and redo WE-L.1.a, then re-attempt (b) by writing out what HᵀR⁻¹H touches when H selects one landmark.

Part B — Numerical Stability: The Joseph Form and Its Companions

§L.2 — Objects: the Joseph form, symmetrization, and the singular-S case

The Joseph form of the covariance update is:

It is algebraically identical to the naive (I − KH)Σ⁻ for the exact optimal K, but it is structurally different:

• The naive form’s (I − KH)Σ⁻ is not necessarily symmetric (the product of (I − KH) and Σ⁻ is not symmetric even if both are), and the computed K’s drift from the exact optimal makes the product non-symmetric.
• The Joseph form’s (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ is symmetric by construction (a matrix times its transpose, plus a positive-semidefinite KRKᵀ), and it is PSD for any K (not just the optimal one). If K is the optimal gain, the two forms are equal; if K is any other gain, the Joseph form is a valid covariance (symmetric, PSD) and the naive form is not.

The symmetrization is the simpler fix: Σ = (Σ + Σᵀ)/2. It enforces symmetry but does not enforce PSD — a non-PSD Σ (negative eigenvalue) remains non-PSD after symmetrization (the negative eigenvalue is preserved). Symmetrization is a cheap (one n × n transpose and add) and partial fix; the Joseph form is the complete fix.

The singular-S case arises when R = 0 (a noiseless measurement) and HΣ⁻Hᵀ is rank-deficient (the measurement does not constrain all the dimensions it “observes” — for example, a sensor that reports a 2D position with one component frozen). The S⁻¹ is undefined, and the K is infinite. The fixes:

• Pseudoinverse: K = Σ⁻Hᵀ S⁺, where S⁺ is the Moore–Penrose pseudoinverse. The constrained directions get the full update, and the unconstrained directions get no update. This is the mathematically correct fix for a rank-deficient S.
• Regularization: S + εI with a tiny ε (e.g., 1e-8). The ε makes S invertible, and the unconstrained directions get a tiny update (the ε⁻¹ is large, but the Σ⁻Hᵀ for those directions is zero, so the product is finite). The regularization is simpler than the pseudoinverse and adequate for the small ε if the Σ⁻Hᵀ is not exactly zero.

The square-root filtering is the most numerically stable form, storing the Cholesky factor L (with Σ = LLᵀ) rather than Σ. The predict and update are QR decompositions (or Cholesky updates) of the factors, and the PSD property is preserved by construction (a product LLᵀ is always PSD). The cost is twice the standard form’s (the QR is O(n³) vs. the O(n³) of the standard update’s matmuls), and the stability is better (the condition number of L is the square root of Σ’s, which halves the dynamic range). The square-root form is used in aerospace (where the Σ’s condition number can be 10^{10}) and is overkill for most robotics applications (where the Σ’s condition number is 10^6 or less).

§L.2.1 — Result: the Joseph form is symmetric and PSD for any K; the naive form is symmetric and PSD only for the exact optimal K; in finite precision, the naive form drifts and the Joseph form does not

Claim. For any K (not necessarily the optimal gain), the Joseph form’s Σ_J = (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ is symmetric and PSD (assuming Σ⁻ ⪰ 0 and R ⪰ 0). The naive form’s Σ_N = (I − KH)Σ⁻ is symmetric and PSD if and only if K is the exact optimal K = Σ⁻HᵀS⁻¹.

Proof sketch. The Joseph form’s symmetry: Σ_Jᵀ = (I − KH)Σ⁻ᵀ(I − KH)ᵀ + KRᵀKᵀ = Σ_J (since Σ⁻ᵀ = Σ⁻ and Rᵀ = R). PSD: for any vector v, vᵀΣ_Jv = ‖Σ⁻^{1/2}(I − KH)ᵀv‖² + ‖R^{1/2}Kᵀv‖² ≥ 0 (a sum of squared norms). The naive form’s symmetry: Σ_Nᵀ = Σ⁻ᵀ(I − KH)ᵀ = Σ⁻(I − HᵀKᵀ), which equals Σ_N only if Σ⁻KH = HᵀKᵀΣ⁻, i.e., if Σ⁻HᵀS⁻¹HΣ⁻ = Σ⁻HᵀS⁻¹HΣ⁻ (the optimal K’s identity). PSD: for the optimal K, Σ_N = Σ⁻ − Σ⁻HᵀS⁻¹HΣ⁻ is the Schur complement of [[Σ⁻, Σ⁻Hᵀ], [HΣ⁻, S]], which is PSD; for non-optimal K, Σ_N is not a Schur complement, and it can have negative eigenvalues.

Corollary. The Joseph form’s cost is 2n³ (two n × n matmuls) vs. the naive form’s n³, a factor of 2 — cheap for the guarantee of symmetry and PSD. In most robotics applications, the Joseph form is the default because the guarantee is worth the cost.

Corollary. The Joseph form’s PSD is contingent on R ⪰ 0 and Σ⁻ ⪰ 0. If the input Σ⁻ is non-PSD (from a previous step’s drift), the Joseph form’s output is not necessarily PSD (the (I − KH)Σ⁻(I − KH)ᵀ term is PSD only if Σ⁻ is). The complete fix is to enforce Σ⁻ ⪰ 0 at every step (by eigenvalue clipping or symmetrization), and the Joseph form is the first line of defense, not the only one.

§L.2.2 — Worked-example ladder

WE-L.2.a — Fully worked: the naive form vs. the Joseph form, with a perturbed K

Input. Scalar model A = 1, H = 1, Σ⁻ = 0.02, R = 0.04. The optimal K = 0.3333, and a perturbed K = 0.34 (2% off).

Trace (naive form, optimal K).

1. Σ_N = (1 − 0.3333)·0.02 = 0.01333.
2. Symmetric? For a scalar, yes (trivially). PSD? 0.01333 > 0, yes.

Trace (naive form, perturbed K).

3. Σ_N = (1 − 0.34)·0.02 = 0.0132.
4. Symmetric? Trivially yes. PSD? 0.0132 > 0, yes — but the value is different from the optimal’s, and the error is 0.00013 (a 1% difference from the optimal).

Trace (Joseph form, optimal K).

5. (1 − K)²·Σ⁻ + K²·R = (0.6667)²·0.02 + (0.3333)²·0.04 = 0.4444·0.02 + 0.1111·0.04 = 0.00889 + 0.00444 = 0.01333. ✓ Matches the naive form’s optimal.

Trace (Joseph form, perturbed K).

6. (1 − 0.34)²·0.02 + 0.34²·0.04 = 0.4356·0.02 + 0.1156·0.04 = 0.00871 + 0.00462 = 0.01333. Same as the optimal’s!

Output and interpretation. For the scalar case, the Joseph form’s perturbed K = 0.34 gives the same Σ = 0.01333 as the optimal, while the naive form’s perturbed K gives 0.0132 (a 1% error). The Joseph form’s cancellation — the (1 − K)² decreasing and the K²R increasing as K moves away from optimal — is the structural property that makes it insensitive to the gain’s perturbation, and the naive form lacks it. For a matrix Σ, the perturbation’s effect is larger (the (I − KH)Σ⁻ is non-symmetric by O(ε) where ε is the K’s error), and the Joseph form’s (I − KH)Σ⁻(I − KH)ᵀ is symmetric by construction.

Self-explanation 4. Before reading on: in step 6, the Joseph form’s K = 0.34 gives the same Σ as the optimal K = 0.3333. Is this a coincidence of the scalar case, or a general property?

Answer. It is not a coincidence, and it is not exact either. The Joseph form (1−K)²Σ⁻ + K²R is a quadratic in K whose vertex sits exactly at K* = Σ⁻/(Σ⁻ + R), the optimal gain — so its first derivative vanishes there and a small perturbation δ moves the value by O(δ²) rather than O(δ). At K = 0.34, δ = 0.00667, the exact value is 0.013336 against 0.013333 at the optimum: a difference of 2.7 × 10^{-6}, which is why the two print the same to five decimals. The naive form (1−K)Σ⁻ is linear in K, so the same perturbation moves it by δΣ⁻ = 1.3 × 10^{-4} — a 1% relative error, fifty times larger. That is the whole argument for the Joseph form: at the optimal gain the two agree exactly, and away from it the Joseph form is flat to first order while the naive form is not. The property is general, not a scalar artifact; in the matrix case the naive form’s asymmetry adds a second, separate defect.

WE-L.2.b — Completion problem

Input. 2D model, Σ⁻ = [[0.01, 0.005], [0.005, 0.04]], H = [1, 0], R = 0.01, optimal K = [0.5, 0.25]ᵀ. Perturb K to [0.51, 0.25]ᵀ (1% off in the first component).

Trace (final 25% as prompts).

1. The naive form’s Σ_N = (I − KH)Σ⁻ with the perturbed K. ⟨your turn: compute I − KH with K = [0.51, 0.25]ᵀ and multiply by Σ⁻⟩
2. Is Σ_N symmetric? ⟨your turn⟩
3. The Joseph form’s Σ_J = (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ with the same K. ⟨your turn: compute and check symmetry⟩
4. Which is PSD? ⟨your turn⟩

Answers.

1. KH = [0.51, 0.25]ᵀ·[1, 0] = [[0.51, 0], [0.25, 0]]. I − KH = [[0.49, 0], [-0.25, 1]]. (I − KH)Σ⁻ = [[0.49, 0], [-0.25, 1]]·[[0.01, 0.005], [0.005, 0.04]] = [[0.0049, 0.00245], [0.0025, 0.03875]].
2. Symmetric? The (0, 1) entry is 0.00245 and the (1, 0) entry is 0.0025 — not symmetric (they differ by 5 × 10⁻⁵). The naive form drifts from symmetry even for a 1% perturbation.
3. (I − KH)Σ⁻(I − KH)ᵀ = [[0.49, 0], [-0.25, 1]]·[[0.01, 0.005], [0.005, 0.04]]·[[0.49, -0.25], [0, 1]] = [[0.0049, 0.00245], [0.0025, 0.03875]]·[[0.49, -0.25], [0, 1]] = [[0.00240, 0.00123], [0.00123, 0.03813]] (symmetric, since (0, 1) = (1, 0) by construction). Then KRKᵀ = [0.51, 0.25]ᵀ·0.01·[0.51, 0.25] = [[0.00260, 0.00128], [0.00128, 0.00063]]. Σ_J = [[0.00500, 0.00251], [0.00251, 0.03876]] — symmetric, and close to the optimal’s [[0.0050, 0.0025], [0.0025, 0.0388]].
4. Both are PSD in this example, but the naive form’s non-symmetry is a drift that, accumulated over many steps, can produce a negative eigenvalue (a non-PSD Σ). The Joseph form’s symmetry is structural and does not drift.

The lesson. The Joseph form is symmetric by construction (the (I − KH)Σ⁻(I − KH)ᵀ is a matrix times its transpose), while the naive form’s symmetry is a consequence of the exact optimal K and breaks under the finite-precision drift. The structural guarantee is what makes the Joseph form robust, and its 2× cost is the price.

WE-L.2.c — Faded: the singular S case

Setup. With a 1D measurement S is 1×1, so it is singular only when HΣ⁻Hᵀ + R = 0, and with R ≥ 0 that needs R = 0 and HΣ⁻Hᵀ = 0. Take H = [0, 1] (observe the zero-variance component) and Σ⁻ = [[0.01, 0], [0, 0]] with R = 0. Then S = 0, singular. Compare H = [1, 0] with the same Σ⁻: S = 0.01, non-singular — the singularity is about which component is observed, not about the dimensions.

Question. With H = [0, 1] (observe the zero-variance component), Σ⁻ = [[0.01, 0], [0, 0]], R = 0, what is S, and what does the update do?

You must produce the intermediate steps. The setup and the answer are given.

Answer. S = HΣ⁻Hᵀ = 0 (the second component’s variance is zero, and the observation is noiseless). S⁻¹ is undefined. The pseudoinverse S⁺ = 0 (the 0 × 0 matrix’s pseudoinverse is 0), so K = Σ⁻Hᵀ S⁺ = 0, and the update is μ = μ⁻ + 0 = μ⁻ and Σ = Σ⁻. The measurement is ignored — because the observed component has zero variance and zero noise, the measurement is always exactly the state, and the update cannot change anything. The regularization S + ε with ε = 1e-8 gives S⁻¹ = 1e8, which is huge, and K = Σ⁻Hᵀ·1e8 = [0, 1e8·0] = 0 (the Σ⁻Hᵀ for the second component is 0, so the K is still 0). The measurement is still ignored. The pseudoinverse and the regularization give the same result for this case.

The lesson. The singular S is the rank-deficient measurement case: the measurement does not constrain all the dimensions it purports to observe, and the unconstrained dimensions get no update. The pseudoinverse is the exact fix when the deficiency is genuine; the regularization is the approximate one; the symptom in the covariance form is a singular S that makes S⁻¹ undefined — which torch.linalg.inv reports by raising, not by returning NaN. The information form avoids the issue: HᵀR⁻¹H with R = 0 is infinite (the R⁻¹ is ∞), which is the exact representation of a noiseless measurement — but the infinite HᵀR⁻¹H needs the regularization or the constraint formulation, and the pseudoinverse is the finite version.

WE-L.2.d — Contrastive near-miss: the symmetrization as the only fix

Correct solution. The Joseph form (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ guarantees symmetry and PSD.

Near-miss. The symmetrization Σ = (Σ_N + Σ_Nᵀ)/2 fixes the symmetry but not the PSD.

Why the near-miss produces a plausible but incorrect number. The symmetrized Σ is symmetric (the obvious check passes), but its eigenvalues can be negative (a non-PSD matrix’s symmetrization is still non-PSD — the eigenvalues are the same as the original’s, since the symmetrization is a projection onto the symmetric matrices, not a PSD projection). The negative eigenvalue is the symptom of a non-PSD Σ, and the downstream operations (the Σ⁻HᵀS⁻¹’s S is the HΣ⁻Hᵀ + R, which is smaller than R if Σ has a negative eigenvalue, and the K can be wrong or undefined) are corrupted. The symmetrization is a necessary first step (the Σ must be symmetric for the eigendecomposition) but not sufficient (the Σ must be PSD).

The discrepancy, stated. The symmetrization (Σ + Σᵀ)/2 and the Joseph form (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ both produce symmetric matrices, but only the Joseph form’s is PSD by construction (the (I − KH)Σ⁻(I − KH)ᵀ is MΣ⁻Mᵀ, which is PSD for Σ⁻ ⪰ 0, and the KRKᵀ is PSD for R ⪰ 0). The symmetrized naive form’s PSD is contingent on the input’s PSD and the K’s optimality, and the drift in either can produce a negative eigenvalue that the symmetrization cannot fix.

Self-explanation 5. Before reading on: in the near-miss, if Σ_N has a negative eigenvalue λ = -1e-8, is the symmetrized Σ’s smallest eigenvalue also -1e-8?

Answer. Yes — the symmetrization (Σ_N + Σ_Nᵀ)/2 is a projection onto the symmetric matrices, and the eigenvalues of a symmetric matrix are the same as the eigenvalues of its symmetric part if the original is “close to symmetric” (the asymmetric part’s eigenvalues are imaginary and cancel in the symmetrization). For a non-PSD Σ_N, the symmetric part inherits the negative eigenvalue, and the symmetrization does not make it PSD. The only fixes are the Joseph form (prevention) or the eigenvalue clipping (correction: Σ = U max(Λ, 0) Uᵀ), and the clipping is a post-hoc fix that loses information (the negative eigenvalue’s direction becomes zero variance, which is overconfident), while the Joseph form prevents the negative eigenvalue from appearing.

§L.2.3 — Comparative table: the covariance-update forms and their guarantees

Design principle (P12.7). Prefer a form whose guarantee is structural over a fix that repairs the output. The Joseph form and the square-root form are symmetric and PSD because of how they are written — a matrix times its transpose, a product LLᵀ — so no input perturbation can produce a matrix that violates the guarantee while the arithmetic is exact. Symmetrization and eigenvalue clipping act on the result: they can restore the property a drifting computation destroyed, and they cannot tell a genuine zero eigenvalue from a numerical artifact, so clipping converts a lost direction into a confident zero variance. The price of the structural forms is a constant factor of two, and the price of the repairs is an unverifiable claim about which eigenvalues were real.

§L.2.4 — Concept check

Concept check (displaces M2). A learner says: “Σ = (I − KH)Σ⁻ is the textbook update, and the textbook’s Σ is symmetric and positive-definite, so my implementation is fine.” What has the learner failed to model, and what does the resulting bug look like?

Answer. The learner has failed to model the gap between the formula and the computed value: (I − KH)Σ⁻ is symmetric and PSD when K is the exact optimal gain, and the K a finite-precision implementation computes differs from the exact one by O(ε). The product of a nearly-symmetric I − KH with a symmetric Σ⁻ is not symmetric, and the asymmetry accumulates over thousands of steps rather than staying at the rounding level. The bug class is slow drift with a passing spot check: a single-step test at K = [0.51, 0.25] shows a 5 × 10⁻⁵ asymmetry, which reads as rounding; a thousand steps later the smallest eigenvalue is negative, the gate’s S is smaller than R, and the gain is wrong. The Joseph form is flat to first order in the gain’s error — its value at K = 0.34 differs from the optimum’s by 2.7 × 10⁻⁶ where the naive form’s differs by 1.3 × 10⁻⁴ — which is why the fix is the form, not a tighter tolerance.

§L.2.5 — Consolidation checkpoint 2

(a) You should now be able to: (i) write the Joseph form and state which term supplies symmetry and which supplies PSD; (ii) state the condition under which the naive form’s symmetry holds, and why the computed K violates it; (iii) distinguish what symmetrization fixes from what it cannot; (iv) state the two fixes for a singular S and when each applies.

(b) Transfer probe. A learner’s filter runs for 1,000 steps and then a step raises inside torch.linalg.cholesky with “The factorization could not be completed because the input is not positive-definite.” They add Σ = (Σ + Σᵀ)/2 after every update and the raise moves to step 4,000. Answer sketch: symmetrization is a projection onto the symmetric matrices, and it leaves the eigenvalues where they were — the negative one included. The raise is the negative eigenvalue arriving, and halving the drift’s rate does not remove its source. The Σ that reaches the Cholesky factor is the naive update’s output, and the naive update is only PSD for the exact optimal gain, so the repair must be at the update: the Joseph form’s (I − KH)Σ⁻(I − KH)ᵀ + KRKᵀ is PSD for any K, including the computed one. Symmetrization is still worth keeping as a cheap first line of defense, but its role is to prepare Σ for an eigendecomposition, not to make it PSD — and the “it moved from 1,000 to 4,000” observation is the tell that a rate was reduced and a cause was not.

(c) Remediation rule. If you cannot do (b), return to §L.2.2 and redo WE-L.2.b, then re-attempt (b) by asking which eigenvalues the symmetrization changes.

Part C — Outlier Rejection and the Mahalanobis Gate

§L.3 — Objects: the innovation’s Mahalanobis distance, the chi-squared test, and the gate

The innovation is ν_t = y_t − Hμ_t⁻, with covariance S_t = HΣ_t⁻Hᵀ + R. The Mahalanobis distance of the innovation is:

The chi-squared test: under the null hypothesis that the model is correct (the measurement is a sample from N(Hμ⁻, S)), the squared Mahalanobis distance is distributed as χ²(m), where m = |y| is the measurement’s dimension. The gate rejects the measurement if:

where χ²_{1-α}(m) is the 1 - α quantile of the chi-squared distribution with m degrees of freedom. For α = 0.01 (a 1% false-rejection rate) and m = 1, χ²_{0.99}(1) ≈ 6.63; for m = 2, χ²_{0.99}(2) ≈ 9.21; for m = 3, χ²_{0.99}(3) ≈ 11.34.

The robust update (after the gate) has two variants:

• Hard rejection: if d² > threshold, skip the update (the filter’s μ and Σ are unchanged). This is the simplest robust update, and it is adequate when the outliers are isolated (a single bad measurement, followed by good ones).
• Soft rejection (Huber-style): down-weight the measurement by w = min(1, threshold/d²), and use the inflated R' = R/w in the update. This is smoother (the estimate transitions gradually) and better for many outliers, but it is more complex to implement.

The gate’s two failure modes:

• Too loose (α too small, threshold too large): the gate accepts outliers, and the filter’s μ is corrupted (the outlier pulls the estimate toward it, and the covariance shrinks, making the filter overconfident). The gate is ineffective.
• Too tight (α too large, threshold too small): the gate rejects good measurements, and the filter diverges (the Σ grows without the update’s reduction, and the estimate freezes). The gate is too aggressive.

The choice of α is a trade-off: a small α (e.g., 0.001) is conservative (rarely rejects good data, but also rarely catches outliers); a large α (e.g., 0.1) is aggressive (catches more outliers, but also rejects more good data). The standard is α = 0.01 (1% false-rejection), which is balanced for most robotics applications.

§L.3.1 — Result: the chi-squared test is exact for linear-Gaussian models, and the gate is the only principled way to reject outliers in the linear KF

Claim. For the linear-Gaussian model, d² = νᵀS⁻¹ν ~ χ²(m) exactly under the null hypothesis, and the gate d² > χ²_{1-α}(m) has a false-rejection rate of exactly α and a detection rate that depends on the outlier’s magnitude (the larger the outlier, the higher the detection rate).

Proof sketch. The innovation ν = y − Hμ⁻ is a linear function of the Gaussian measurement y and the Gaussian state μ⁻, so ν ~ N(0, S) under the null. The whitened innovation S^{-1/2}ν ~ N(0, I_m), and its squared norm νᵀS⁻¹ν is the sum of m independent standard normals squared, which is χ²(m). The gate’s false-rejection rate is P(χ²(m) > χ²_{1-α}(m)) = α by the quantile’s definition. The detection rate is P(χ²(m; λ) > χ²_{1-α}(m)) where λ = δᵀS⁻¹δ is the non-centrality from the outlier’s magnitude δ, which increases monotonically with ‖δ‖.

Corollary. The gate is exact for the linear-Gaussian model, and its guarantees are distribution-free in the sense that the false-rejection rate is α regardless of the model’s parameters (A, H, Q, R), as long as the null holds. The detection rate depends on the model’s parameters (the S and the outlier’s magnitude), and it is higher when S is smaller (the measurement is more precise relative to the prediction).

Corollary. The gate’s threshold is the chi-squared quantile, and its value for α = 0.01 is 6.63 (m = 1), 9.21 (m = 2), 11.34 (m = 3). The larger the measurement’s dimension, the larger the threshold (more degrees of freedom → more variance → a larger quantile). A common mistake is using the m = 1 threshold for a multi-dimensional measurement, which makes the gate too tight (the d² has a larger typical value for larger m, and the 6.63 threshold rejects many good measurements). The correct threshold is the χ²(m) quantile for the actual m.

§L.3.2 — Worked-example ladder

WE-L.3.a — Fully worked: the gate on a specific innovation

Input. Scalar model with S = 0.06, innovation ν = 0.2 (from y = 0.7, Hμ⁻ = 0.5). α = 0.01, m = 1.

Trace.

1. d² = νᵀS⁻¹ν = 0.2²/0.06 = 0.04/0.06 ≈ 0.667.
2. Threshold χ²_{0.99}(1) ≈ 6.63.
3. d² = 0.667 < 6.63 — accept the measurement.

Trace (a larger innovation).

4. ν = 0.5 (the measurement is 0.5 away from the prediction).
5. d² = 0.25/0.06 ≈ 4.17.
6. 4.17 < 6.63 — accept.

Trace (an outlier).

7. ν = 1.0 (the measurement is a meter away from the prediction).
8. d² = 1.0/0.06 ≈ 16.67.
9. 16.67 > 6.63 — reject the outlier.

Output and interpretation. The innovation 0.2 (the typical noise’s magnitude) gives d² = 0.667, well below the threshold; the innovation 0.5 (a 2.5σ deviation) gives d² = 4.17, still below; the innovation 1.0 (a 5σ deviation) gives d² = 16.67, above — the measurement is inconsistent with the model, and the gate rejects it. The threshold 6.63 corresponds to a 2.58σ deviation (the 0.99 quantile’s √6.63 ≈ 2.58), so any innovation beyond 2.58σ is rejected — a 1% false-rejection rate, as designed.

Self-explanation 6. Before reading on: in step 2, the threshold 6.63 corresponds to a 2.58σ deviation. Why is it not 2σ (the 95% quantile’s 1.96σ)?

Answer. The 6.63 is the 99% quantile (α = 0.01), and the 95% quantile is 3.84 (1.96²). The choice of α is the false-rejection rate: α = 0.01 rejects 1% of good measurements, and α = 0.05 rejects 5%. The 2.58σ is the 1% two-sided threshold for a normal (the 0.995 quantile), and its square is 6.63. The 2σ is the 5% threshold, and its square is 3.84. The choice is the trade-off between catching outliers (a tighter gate) and not rejecting good data (a looser gate), and α = 0.01 is the common default because it balances the two.

WE-L.3.b — Completion problem

Input. 2D measurement, S = [[0.06, 0], [0, 0.04]], innovation ν = [0.2, 0.3], α = 0.01.

Trace (final 25% as prompts).

1. d² = νᵀS⁻¹ν. ⟨your turn: compute S⁻¹ and the quadratic form⟩
2. Threshold χ²_{0.99}(2). ⟨your turn: what is the value?⟩
3. Accept or reject? ⟨your turn⟩

Answers.

1. S⁻¹ = [[1/0.06, 0], [0, 1/0.04]] ≈ [[16.67, 0], [0, 25]]. d² = 0.2²·16.67 + 0.3²·25 = 0.667 + 2.25 = 2.917.
2. χ²_{0.99}(2) ≈ 9.21.
3. 2.917 < 9.21 — accept the measurement.

The lesson. The 2D measurement’s d² is the sum of the per-component d² values (0.667 from the first, 2.25 from the second), and the threshold is the χ²(2) quantile, which is larger than the χ²(1) quantile (9.21 vs. 6.63) — the extra degree of freedom increases the threshold, and the gate is looser for 2D than for 1D. The mistake of using the 1D threshold 6.63 for a 2D measurement would reject this measurement (since 2.917 < 6.63 still accepts, but a larger d² between 6.63 and 9.21 would be incorrectly rejected).

WE-L.3.c — Faded: the gate’s false-rejection rate over many steps

Setup. The same 1D model, run for T = 1000 steps with no outliers (all measurements are from N(Hμ⁻, S)).

Question. How many measurements does the gate reject, on average? What is the expected number, and what is the variance?

You must produce the intermediate steps. The setup and the answer are given.

Answer. Each measurement has an independent α = 0.01 probability of being rejected (under the null), so the expected number of rejections is 1000 × 0.01 = 10, and the variance is 1000 × 0.01 × 0.99 = 9.9 (a binomial distribution’s variance). The standard deviation is √9.9 ≈ 3.15, so the observed count is typically between 4 and 16 (a 2σ range). The false-rejection rate is the cost of the gate, and it is predictable from α.

The lesson. The gate’s false-rejection rate is exactly α (under the null), and the cost is the good measurements that are rejected. For α = 0.01, the cost is 1% of the measurements, which is acceptable for most applications; for α = 0.001, the cost is 0.1%, but the detection rate for outliers is lower (the threshold is higher). The choice of α is the trade-off between false-rejection and detection, and α = 0.01 is the balanced default.

WE-L.3.d — Contrastive near-miss: the gate with R instead of S

Correct solution. d² = νᵀS⁻¹ν with S = HΣ⁻Hᵀ + R.

Near-miss. d² = νᵀR⁻¹ν — using the measurement’s covariance instead of the innovation’s.

Why the near-miss produces a plausible but incorrect number. The near-miss’s d² is larger than the correct one (since R ⪯ S), so the gate is too tight — it rejects more good measurements (the false-rejection rate is higher than α), and its threshold χ²(m) is too low for the actual d² distribution (which is χ²(m; λ) with λ from the prior’s contribution, not the R-only χ²(m)). The symptom is a higher-than-α rejection rate for good data, and the consequence is a filter that diverges (too many rejected updates). The correct S includes the prior’s uncertainty, and the gate’s threshold is matched to the innovation’s actual covariance, not the measurement’s.

The discrepancy, stated. R (the measurement’s covariance) and S = HΣ⁻Hᵀ + R (the innovation’s covariance) differ by the prior’s contribution HΣ⁻Hᵀ. The near-miss omits the prior’s contribution, which underestimates the innovation’s spread, and the gate is too aggressive. The correct S includes the prior, and the gate’s threshold is matched to the total uncertainty (prior + measurement), which is the only way to achieve the exact false-rejection rate α.

Self-explanation 7. Before reading on: in the near-miss, if HΣ⁻Hᵀ = 0 (the prediction is exactly the measurement’s expectation, with zero uncertainty in the observed direction), does the near-miss give the correct d²?

Answer. Yes — if HΣ⁻Hᵀ = 0, then S = R, and the near-miss’s d² = νᵀR⁻¹ν equals the correct d². The condition is the prior’s uncertainty in the observed direction being zero, which happens when Σ⁻ = 0 (a perfect prediction, which is unrealistic for a real filter with nonzero process noise). For any Σ⁻ ≻ 0, S ≻ R, and the near-miss is too tight — the generality of the mistake is the prior’s nonzero uncertainty, which is the normal case.

§L.3.3 — Comparative table: rejection strategies and the gate’s parameters

Design principle (P12.8). Calibrate the gate on clean data before trusting it on dirty data. The threshold’s meaning is a false-rejection rate under the null, so the only test that validates it is a run with no outliers: 1,000 clean steps at α = 0.01 should reject about 10 measurements, with a standard deviation of √9.9 ≈ 3.15, and a count of 40 is a gate that is rejecting good data whatever it does to the outliers. The check is one counter and one binomial comparison, and it separates the two failure modes — too loose and too tight — that a dirty-data test cannot distinguish, because a gate that rejects everything also rejects the outliers.

§L.3.4 — Concept check

Concept check (displaces M3). A learner says: “A larger S means the measurement is less informative, so the gate should reject more when S is large.” What has the learner failed to model, and what does the resulting bug look like?

Answer. The learner has failed to model S’s position in the statistic: it is the denominator of d² = νᵀS⁻¹ν, so a larger S makes the same innovation’s d² smaller, and the gate becomes more permissive, not less. The intuition about information is not wrong about the filter — a larger S does mean a less precise innovation — but it is wrong about the test, because the test asks whether the innovation is surprising relative to its own spread, and a spread-out innovation is unsurprising. The bug class is an inverted threshold: a learner who “fixes” the gate by tightening it when S is large rejects exactly the measurements the model says are plausible, the rejection rate climbs above α on clean data, and the filter’s covariance grows without the update’s reduction until the estimate drifts. The correct reading is that the χ²(m) threshold is fixed for a given α and m, and the S’s size is already accounted for by the normalization.

§L.3.5 — Consolidation checkpoint 3

(a) You should now be able to: (i) write d² = νᵀS⁻¹ν and state the distribution it follows under the null; (ii) look up or state the χ²_{1−α}(m) threshold for m = 1, 2, 3 at α = 0.01; (iii) distinguish hard from soft rejection and state when each applies; (iv) state both failure modes of the gate and the calibration that separates them.

(b) Transfer probe. A learner’s gate uses χ²_{0.99}(2) = 9.21 on a 3D measurement and observes a rejection rate of about 2.5% on a clean log. They conclude the sensor is noisier than modeled and increase R. Answer sketch: the threshold is for the wrong degrees of freedom. d² for a 3D measurement is χ²(3), whose 0.99 quantile is 11.34, so the gate is rejecting the upper tail of a χ²(3) beyond 9.21 — about 2.5% of clean data — exactly as observed. Raising R inflates S, which shrinks every d², which reduces the rejection rate and makes the gate look calibrated, while simultaneously telling the filter that the sensor is worse than it is; the estimate then under-weights good measurements for the rest of the run. The correct fix is the threshold for m = 3, and the diagnostic is that the observed rate matched the quantile’s tail for the wrong m — a prediction the “sensor is noisy” hypothesis does not make.

(c) Remediation rule. If you cannot do (b), return to §L.3.2 and redo WE-L.3.b, then re-attempt (b) by computing what fraction of χ²(3) lies above 9.21.

Part D — The Bridge to Nonlinear: EKF, UKF, and PF

§L.4 — Objects: the three approximations and their linearization

The extended Kalman filter (EKF) linearizes the nonlinear model around the current estimate:

The Jacobians are $A_t$ = ∂f/∂x |_{x_t = μ_t}, $H_t$ = ∂h/∂x |_{x_t = $μ_t$⁻}. The predict is μ⁻ = f(μ, u), Σ⁻ = AΣAᵀ + Q; the update is the KF’s with the linearized H.

The unscented Kalman filter (UKF) propagates sigma points through the nonlinearity, and reconstructs the mean and covariance from the propagated points. The sigma points for an n-dimensional state are 2n + 1 points: χ⁽⁰⁾ = μ, χ⁽ⁱ⁾ = μ ± (√((n + λ)Σ))_i for i = 1, …, n, with the weights $w_m$⁽⁰⁾ = λ/(n + λ), $w_c$⁽⁰⁾ = λ/(n + λ) + (1 − α² + β), w⁽ⁱ⁾ = 1/(2(n + λ)). The propagated sigma points’ weighted mean and covariance are the approximated posterior’s mean and covariance.

The particle filter (PF) approximates the posterior with samples ${x_t^{(i)}}$ and weights ${w_t^{(i)}}$, and updates the samples by sampling from the dynamics and the weights by the likelihood. The PF is asymptotically exact as the number of particles grows, and it handles multimodal posteriors (which the EKF/UKF cannot, since they are unimodal Gaussians).

The selection of the estimator:

§L.4.1 — Result: the EKF’s linearization error is O(‖x − μ‖²), and it diverges when the estimate is far from the truth; the UKF is O(‖x − μ‖³), and the PF is exact in the limit

Claim. The EKF’s covariance is the linearized approximation of the true posterior’s covariance, with an error of O(‖x − μ‖²) per step; the UKF’s is O(‖x − μ‖³); the PF’s is exact in the limit of infinite particles. The EKF diverges when the state is far from the estimate (the linearization is invalid), and the divergence is not caught by the EKF’s own covariance (which is small, because the linearization underestimates the uncertainty).

Proof sketch. The EKF’s f(μ) + A(x − μ) is the first-order Taylor expansion of f around μ, and the truncation error is O(‖x − μ‖²). The UKF’s sigma points capture the second-order terms, so its truncation error is O(‖x − μ‖³). The PF’s samples are exact draws from the posterior (up to the sampling error, which is O(1/√$N_particles$)), so the PF converges to the true posterior as N → ∞.

Corollary. The EKF’s divergence is silent — the covariance shrinks (the linearization underestimates the uncertainty), the gain shrinks, and the estimate freezes while the state drifts. The symptom is the same as the “missing Q” bug (a covariance that shrinks, a gain that vanishes, an estimate that freezes), and the diagnosis is the model’s nonlinearity (not the Q’s absence), which is harder to detect from the filter’s internal state — the residuals (the innovations) grow large and systematic, and the test is to compare the filter’s predicted measurements against the actual ones.

Corollary. The choice of estimator is the nonlinearity’s severity, and the boundary between EKF and UKF is the state’s spread relative to the linearization’s validity. For a robot arm’s joint angles (small angles, moderate spread), the EKF is adequate; for a drone’s attitude (large rotations, wide spread), the UKF (or a manifold-aware EKF, which is the 0.2.2 quaternion parameterization) is better; for a global localization with multiple hypotheses (a kidnapped robot, a symmetric environment), the PF is necessary.

§L.4.2 — Worked-example ladder

WE-L.4.a — Fully worked: the EKF’s linearization for the arm’s nonlinear observation

Input. The running case’s 6-DOF arm’s end-effector position p(q) is a nonlinear function of the joint angles, and the observation is the camera’s noisy p. The EKF’s H is ∂p/∂q, the Jacobian of the end-effector position with respect to the joint angles — the 1.1.2 Jacobian (not yet covered, but previewed here).

Trace.

1. The predict is q⁻ = q + q̇·dt (a linear dynamics, or the f if the dynamics are nonlinear).
2. The Jacobian H = ∂p/∂q is the 3 × 6 matrix with H[i, j] = ∂p_i/∂$q_j$.
3. The linearized observation is p ≈ p(q⁻) + H(q − q⁻).
4. The update is the KF’s with H (the 3 × 6 Jacobian) instead of the linear H.
5. The error of the linearization is O(‖q − q⁻‖²), which is small when the joint angles’ uncertainty is small (a tight prior) and large when the uncertainty is wide (a loose prior).

Output and interpretation. The EKF’s H is the Jacobian of the end-effector position, and its value depends on the current q. For a tight prior (‖q − q⁻‖ small), the linearization is accurate, and the EKF is close to the true posterior. For a wide prior, the linearization is inaccurate, and the EKF’s covariance is underestimated — the divergence risk.

Self-explanation 8. Before reading on: in step 3, the linearized observation p ≈ p(q⁻) + H(q − q⁻) is the first-order Taylor expansion. What is the second-order term, and why is it ignored in the EKF?

Answer. The second-order term is ½(q − q⁻)ᵀ(∂²p/∂q²)(q − q⁻), the Hessian’s contribution, and it is O(‖q − q⁻‖²). The EKF ignores it because including it would require the Hessian (a 3 × 6 × 6 tensor for each output component), which is expensive to compute and numerically delicate (the Hessian’s eigenvalues can be negative, and the quadratic form’s contribution is not a covariance). The UKF’s sigma points capture the second-order term implicitly (by propagating the sigma points through the nonlinear p), which is why the UKF is more accurate than the EKF for moderate nonlinearity, at the cost of 2n + 1 forward propagations instead of one Jacobian.

WE-L.4.b — Completion problem

Input. A 1D nonlinear observation y = x² + v with x ~ N(μ = 2, Σ = 0.1), R = 0.01, y = 4.5.

Trace (final 25% as prompts).

1. EKF’s H = ∂h/∂x = 2x evaluated at μ = 2: ⟨your turn⟩
2. EKF’s S = HΣHᵀ + R. ⟨your turn⟩
3. EKF’s K = ΣHᵀ/S. ⟨your turn⟩
4. EKF’s μ = μ + K(y − μ²). ⟨your turn: compute the innovation y − μ² and the update⟩

Answers.

1. H = 2·2 = 4.
2. S = 4²·0.1 + 0.01 = 1.6 + 0.01 = 1.61.
3. K = 0.1·4/1.61 ≈ 0.248.
4. y − μ² = 4.5 − 4 = 0.5. μ = 2 + 0.248·0.5 = 2.124.

The lesson. The EKF’s H = 4 is the local linearization, and the update moves the mean 0.248·0.5 = 0.124 toward the measurement’s implied x (√4.5 ≈ 2.12). The linearization is accurate for this case (the x = 2 is far from the origin, and x² is smooth there), and the EKF is close to the true posterior. For x near 0, the x² is flat and the H = 2x ≈ 0 vanishes, which makes the EKF ignore the measurement — the degenerate case where the linearization is invalid.

WE-L.4.c — Faded: the UKF’s sigma points for the same problem

Setup. Same x ~ N(2, 0.1), y = x² + v, R = 0.01, y = 4.5. The UKF uses α = 0.001, β = 2, κ = 0, λ = α²(n + κ) − n.

Question. What are the sigma points, and what is the propagated mean and covariance?

You must produce the intermediate steps. The setup and the answer are given.

Answer. n = 1, λ = 0.001²·1 − 1 = -0.999999, √(n + λ) = √(0.000001) = 0.001. The sigma points: χ⁽⁰⁾ = 2, χ⁽¹⁾ = 2 + 0.001·√0.1 ≈ 2.000316, χ⁽²⁾ = 2 − 0.001·√0.1 ≈ 1.999684. Weights: $w_m$⁽⁰⁾ = -0.999999/0.000001 = -999999 (a huge negative weight — the λ is very negative for α = 0.001, which is numerically extreme), $w_m$⁽¹⁾ = $w_m$⁽²⁾ = 1/(2·0.000001) = 500000. The propagated points: χ⁽⁰⁾² = 4, χ⁽¹⁾² ≈ 4.001264, χ⁽²⁾² ≈ 3.998736. The weighted mean is -999999·4 + 500000·4.001264 + 500000·3.998736 = -3999996 + 2000632 + 1999368 = 4. The propagated variance is $w_c$-weighted, and it is approximately (2μ)²·0.1 = 1.6 (the EKF’s HΣHᵀ), plus the second-order term ½Σ²·2 = 0.02 (the UKF’s extra term). So the UKF’s S is slightly larger than the EKF’s (1.62 vs. 1.61), and the update is slightly different.

The lesson. The UKF’s sigma points for α = 0.001 are very close to the mean (the √(n + λ) = 0.001 is tiny), and the weights are huge (the -999999 is a numerical warning — the standard UKF uses α ≈ 1e-3 for high-dimensional states, but for 1D the α should be larger to avoid the numerical issues). The propagated mean is exact for the polynomial x² (the UKF’s property for polynomials up to order 2), and the covariance is slightly larger than the EKF’s (the second-order term is captured). The choice of α is a trade-off between the spread of the sigma points (larger α → wider spread → captures the nonlinearity better) and the numerical stability (smaller α → narrower spread → the weights are extreme).

WE-L.4.d — Contrastive near-miss: the EKF for a multimodal posterior

Correct solution. The particle filter (or a mixture filter) for a multimodal posterior (e.g., a robot in a symmetric corridor with two equally likely positions).

Near-miss. The EKF with the single Gaussian, which cannot represent the two modes.

Why the near-miss produces a plausible but incorrect number. The EKF’s single Gaussian has a mean between the two modes, which is a position that is neither of the two likely ones — the average of two equally likely positions is the middle, which is not a likely position. The covariance is large (to cover both modes), and the estimate is the middle, which is wrong for both modes. The filter is confused (it does not know which mode it is in), and its behavior is a compromise that is suboptimal for both modes. The symptom is a large covariance and an estimate that is inconsistent with the measurement (the measurement is close to one mode, but the estimate is far from it).

The discrepancy, stated. The EKF’s single Gaussian and the true multimodal posterior are different distributions, and the EKF’s mean is the unweighted average of the modes (a meaningless position). The PF’s particles can represent the multiple modes, and the filter’s estimate is the particles’ weighted mean, which is the correct Bayes estimate given the multimodal posterior — but the estimate is still the mean of the modes, and the useful output is the full particle set (the multiple hypotheses), not the single mean. The choice of the PF is the multimodality’s representation, and the single mean is the lossy summary.

Self-explanation 9. Before reading on: in the near-miss, if the two modes are very far apart (e.g., two positions 10 m apart), is the EKF’s mean more or less wrong?

Answer. More wrong — the mean is the midpoint (5 m from each mode), which is far from both modes, and the covariance is huge (the (10/2)² = 25 m² variance). The EKF’s estimate is far from the truth (in both modes), and its large covariance is a symptom of the bimodality that the EKF cannot represent. The further the modes are, the worse the EKF’s mean, and the more the PF is necessary — the multimodality is the EKF’s blind spot, and its severity is the mode separation.

§L.4.3 — Comparative table: the three nonlinear estimators

Design principle (P12.9). Choose the estimator from the posterior’s shape and the linearization’s validity, not from the model’s nonlinearity alone. A mild nonlinearity near the operating point is an EKF problem, and the criterion is the state’s spread relative to the region where the Jacobian is a good approximation — a tight prior makes a strongly nonlinear h an EKF problem, and a wide prior makes a mildly nonlinear one a UKF problem. Multimodality is a different axis: no single Gaussian, linearized or sigma-pointed, can carry two hypotheses, so a symmetric corridor is a particle-filter problem regardless of how smooth h is. The two questions are asked separately, and the answer is the more demanding of the two.

§L.4.4 — Concept check

Concept check (displaces M4). A learner says: “The EKF is the KF applied to the nonlinear model — same formulas, Jacobians instead of A and H — so it inherits the KF’s optimality.” What has the learner failed to model, and what does the resulting bug look like?

Answer. The learner has failed to model the locality of the linearization: the EKF’s A and H are evaluated at the current estimate, so the filter’s model is a different linear system at every step, and the approximation is valid only within the region where the first-order expansion holds. The KF’s optimality is a statement about the exact conditional mean and covariance of a linear-Gaussian model; the EKF has neither, because the posterior it computes is the Gaussian propagated through a linearized map. The bug class is silent divergence with a shrinking covariance: far from the truth the linearization underestimates the spread, the covariance shrinks, the gain shrinks, the estimate freezes, and the state drifts — the identical symptom to a missing Q, which is why the diagnosis has to come from the innovations rather than from the filter’s internals. The tell is that the innovations grow large and systematic while Σ reports confidence, and the test is to compare the predicted measurements against the actual ones.

§L.4.5 — Consolidation checkpoint 4

(a) You should now be able to: (i) write the EKF’s predict and update and state where the Jacobians enter; (ii) state the truncation orders of the EKF, the UKF, and the PF; (iii) state why the EKF’s covariance underestimates the spread far from the estimate; (iv) select an estimator from the posterior’s shape and the linearization’s validity, and justify the choice.

(b) Transfer probe. A learner’s EKF tracks a joint angle and works well for hours; then the arm is commanded through a large reconfiguration, and the filter’s estimate sticks at the old angle while its covariance prints 1e-6. The innovations are small and one-signed for a while, then large. Answer sketch: the filter has not been informed that it is far from the truth — the linearization around the old estimate is being evaluated at a point the state has left, the predicted measurement is wrong in a consistent direction, and the covariance is small because the linearized propagation added less spread than the true motion. This is the EKF divergence pattern, and the two tests are: (i) check the covariance’s Q term against the unmodeled motion over the reconfiguration, and (ii) compare h(μ⁻) with y — a systematic innovation during a large motion is the linearization, not the noise. The remedy is a larger Q during fast motion (or a UKF, whose sigma points widen with the prior’s spread and therefore carry the reconfiguration), and the point of the probe is that “works for hours” tested the linearization’s validity in the region the arm had already visited.

(c) Remediation rule. If you cannot do (b), return to §L.4.2 and redo WE-L.4.b, then re-attempt (b) by asking what H = ∂h/∂x evaluates to when the state has moved away from μ.

Part E — Failure Taxonomy, Lab, Cheat Sheet, Exercises, Exit Test

§L.5 — Failure taxonomy

§L.6 — The lab

§L.6.1 — Task

Implement a KalmanFilter nn.Module with the Joseph form, the information form, and the Mahalanobis gate; verify the numerical stability of the Joseph form under a perturbed gain; verify the gate’s false-rejection rate; then break the pipeline three ways.

§L.6.2 — Conditions

Python 3.10+, PyTorch 2.2+, CPU. Seed 42, dtype = float64. Runtime under 3 minutes. The 2D model from 0.3.3: A = [[1, 0.1], [0, 1]], H = [1, 0], Q = diag([0.001, 0.01]), R = 0.04. Trajectory T = 1000 steps with 5% outliers (measurements shifted by ±3).

§L.6.3 — Controls held fixed

The seed, dtype, the model matrices, the trajectory length, and the outlier fraction are fixed.

§L.6.4 — Metrics table

§L.6.5 — Pre-registered hypotheses

• H1. At the optimal gain the two forms are algebraically identical: (I − KH)Σ⁻ is symmetric because KHS = Σ⁻Hᵀ(HΣ⁻Hᵀ + R)⁻¹HΣ⁻ is, so there is nothing to drift. Measured over 1000 steps the naive form’s asymmetry is one ulp (3.5e-18) in float64 and 1.9e-9 in float32 — the last bit, not a trend, and it does not grow with more steps. The prediction is therefore that the naive form’s symmetry drift is at round-off, and that the forms separate only under a suboptimal gain.
• H2. Likewise at the optimal gain: both forms stay PSD ($λ_min$ = 7.51e-3 throughout) and their trajectories coincide. The Joseph form’s guarantee is about an arbitrary gain K, not the optimal one, and that is where the prediction bites: at K = 8·$K_opt$ the naive form’s $λ_min$ goes negative (−8.997e-3) while Joseph’s stays positive (+4.123e-2). The prediction is the pair of signs at a suboptimal gain, with the optimal-gain run as the control.
• H3. The gate’s false_rejection_rate is ≈ 0.01 (within 0.005) for α = 0.01, and its outlier_detection_rate is > 0.95 for the ±3 outliers. The prediction is the pair of values.
• H4. mean_error_gated < mean_error_filtered by > 30% — the gate reduces the outlier’s effect. The prediction is the relative reduction.
• H5. information_vs_covariance_diff < 1e-10 — the two forms agree algebraically. The prediction is the bound.

§L.6.6 — The break-it sequence

1. The naive form’s loss of PSD under a suboptimal gain (silent). Use Σ = (I − KH)Σ⁻ with K = 8·$K_opt$. Predicted symptom: no exception; $λ_min(Σ$) becomes negative, so the “covariance” is not a covariance and the next S may be non-PD. Actual: as predicted — silent, $λ_min$ = −8.997e-3 while Joseph’s stays at +4.123e-2. Note that at the optimal gain neither form drifts: the naive form is algebraically symmetric there, so a test run only at the optimal gain proves nothing.
2. The gate with R instead of S (silent). Compute d² = νᵀR⁻¹ν. Predicted symptom: no exception; the false-rejection rate is higher than α, and the filter diverges (too many rejected updates). Actual: as predicted, and silent — over 1000 good measurements the rate is 0.0040 with the correct S and 0.0120 with R, a three-fold inflation, and nothing in the output distinguishes the two runs. The gate’s threshold is calibrated to χ²(m), which is the distribution of d² only when the covariance is the innovation’s S; using R makes d² systematically larger, so a fixed α no longer buys a 1% rate.
3. The singular S (loud, and it raises rather than returning NaN). Set R = 0 with a rank-deficient H. Predicted symptom: the S⁻¹ is undefined and the computation dies. Actual: torch.linalg.inv raises _LinAlgError: linalg.inv: The diagonal element 1 is zero, the inversion could not be completed — loud, but by exception, not by a NaN in the array. The distinction is not pedantry: the obvious “fix” for a NaN is to switch to torch.linalg.pinv, which succeeds silently on this S and returns K = [0, 0] — a filter that ignores the measurement entirely while reporting no error at all. The pseudoinverse is the right tool only when the rank deficiency is a genuine statement about what the measurement constrains; used as an error-swallower it converts a loud failure into a silent one.

Two of the three are silent; the third is loud, and its loudest property is that its obvious repair silences it.

§L.6.7 — Acceptance criteria

§L.6.8 — What good looks like

A strong submission has five properties:

1. The Joseph form’s symmetry and PSD are demonstrated under a gain the optimal-gain run cannot reach — the symmetry_drift is printed as the control showing the two forms agree there, and the min_eigenvalue sweep is what separates them. A submission that prints only the optimal-gain run has shown that the naive form is fine, which is true and is not the point.
2. The gate’s false-rejection rate is measured, not assumed — the submission runs 1000 good measurements and reports the actual rejection rate, comparing against α.
3. The gate’s benefit is quantified — the mean_error_filtered vs. mean_error_gated are printed, and the reduction is positive.
4. The information and covariance forms’ agreement is verified — the two forms’ Σ values are compared, and the difference is < 1e-10.
5. The break-its are predicted with the symptom (drift, higher rejection, a raised exception) stated before running.

§L.6.9 — Runnable lab code

# lab_0_3_4.py — Robust KF: Joseph form, information form, Mahalanobis gating
# Requires: Python 3.10+, PyTorch 2.2+

import torch, math
torch.manual_seed(42); dtype, device = torch.float64, torch.device("cpu")

A = torch.tensor([[1., 0.1], [0., 1.]], dtype=dtype)
H = torch.tensor([[1., 0.]], dtype=dtype)
Q = torch.diag(torch.tensor([0.001, 0.01], dtype=dtype))
R = torch.tensor([[0.04]], dtype=dtype)

def kf_update_joseph(mu_pred, Sigma_pred, y, H, R):
    S = H @ Sigma_pred @ H.T + R
    K = Sigma_pred @ H.T @ torch.linalg.inv(S)
    mu = mu_pred + K @ (y - H @ mu_pred)
    I = torch.eye(Sigma_pred.shape[0], dtype=dtype)
    IK = I - K @ H
    Sigma = IK @ Sigma_pred @ IK.T + K @ R @ K.T
    return mu, Sigma, K, S

def kf_update_naive(mu_pred, Sigma_pred, y, H, R):
    S = H @ Sigma_pred @ H.T + R
    K = Sigma_pred @ H.T @ torch.linalg.inv(S)
    mu = mu_pred + K @ (y - H @ mu_pred)
    I = torch.eye(Sigma_pred.shape[0], dtype=dtype)
    Sigma = (I - K @ H) @ Sigma_pred
    return mu, Sigma, K, S

def mahalanobis_gate(nu, S, alpha=0.01, m=1):
    """Chi-squared gate. Threshold from scipy would be ideal; here we use the standard table."""
    thresholds = {1: 6.635, 2: 9.210, 3: 11.345}  # for alpha=0.01
    d2 = (nu.T @ torch.linalg.inv(S) @ nu).item()
    return d2, d2 < thresholds.get(m, float('inf'))

# --- Step 1: naive vs. Joseph — where the naive form actually loses PSD ---
# With the OPTIMAL gain the two forms are algebraically identical: (I-KH)S is
# symmetric because KHS = S H^T (H S H^T + R)^-1 H S is. Measured over 1000 steps
# at the optimal gain both forms stay PSD and the drift is one ulp (3.5e-18),
# in float64 and in float32 alike. The naive form is not "numerically unstable"
# at the optimal gain; the defect appears when the gain is SUBOPTIMAL, which is
# the real deployment case (fixed gains, fading memory, approximations).
H1x = torch.tensor([[1., 0.]], dtype=dtype)
Sp = torch.tensor([[0.0114, 0.004], [0.004, 0.05]], dtype=dtype)
S1 = H1x @ Sp @ H1x.T + R
Kopt = Sp @ H1x.T @ torch.linalg.inv(S1)
I2 = torch.eye(2, dtype=dtype)
print("c    max|eig(I-cKH)|   naive lmin      joseph lmin")
for c in [1.0, 2.0, 4.0, 8.0, 16.0]:
    K = c * Kopt
    IK = I2 - K @ H1x
    Pn = IK @ Sp
    Pj = IK @ Sp @ IK.T + K @ R @ K.T
    print(f"{c:4.1f} {float(torch.linalg.eigvals(IK).abs().max()):12.3f} "
          f"{float(torch.linalg.eigvalsh(Pn).min()):+.3e}   {float(torch.linalg.eigvalsh(Pj).min()):+.3e}")
print("[break-it #1] the c-sweep above: at c = 8 the naive lmin goes NEGATIVE while Joseph's stays positive")
# At c = 8 the naive form's min eigenvalue goes NEGATIVE while Joseph's stays positive.

# --- Step 1b: the optimal-gain run, as a control -------------------------
def run_filter(update_fn, T=1000, perturb=0.0):
    mu = torch.tensor([0.5, 0.1], dtype=dtype)
    Sigma = torch.diag(torch.tensor([0.01, 0.04], dtype=dtype))
    symmetry_drifts = []
    min_eigs = []
    for t in range(T):
        mu_pred = A @ mu
        Sigma_pred = A @ Sigma @ A.T + Q
        y = torch.tensor([0.5 + 0.1 * math.sin(0.3 * t)], dtype=dtype)
        mu, Sigma, _, _ = update_fn(mu_pred, Sigma_pred, y, H, R)
        if t % 100 == 0:
            symmetry_drifts.append((Sigma - Sigma.T).abs().max().item())
            min_eigs.append(torch.linalg.eigvalsh(Sigma).min().item())
    return symmetry_drifts, min_eigs

drifts_naive, eigs_naive = run_filter(kf_update_naive, T=1000)
drifts_joseph, eigs_joseph = run_filter(kf_update_joseph, T=1000)
print(f"naive symmetry drift (t=0,100,...,900): {[f'{d:.2e}' for d in drifts_naive]}")
print(f"joseph symmetry drift: {[f'{d:.2e}' for d in drifts_joseph]}")
print(f"naive min eigenvalue (t=0,...,900): {[f'{e:.2e}' for e in eigs_naive]}")
print(f"joseph min eigenvalue: {[f'{e:.2e}' for e in eigs_joseph]}")
# Both stay PSD at the optimal gain — the control showing the run above is the real test.

# --- Step 2: the gate's false-rejection and detection rates ---
# The measurement noise must match R, or the chi-squared gate is calibrated for
# a different distribution than the one it sees and its rate is meaningless.
# With 0.1*randn against R = 0.04 (sd 0.2), E[d^2] = 0.12 against a 6.635
# threshold, so the gate never fires and "false_rejection = 0" is an artifact.
SIG_Y = math.sqrt(R.item())
def run_with_gate(outlier_frac=0.05, T=1000):
    mu = torch.tensor([0.5, 0.1], dtype=dtype)
    Sigma = torch.diag(torch.tensor([0.01, 0.04], dtype=dtype))
    good_rejected = 0; good_total = 0
    out_rejected = 0; out_total = 0
    errors = []
    for t in range(T):
        mu_pred = A @ mu
        Sigma_pred = A @ Sigma @ A.T + Q
        is_outlier = (torch.rand(1).item() < outlier_frac)
        y = 0.5 + 0.1 * math.sin(0.3 * t) + SIG_Y * torch.randn(1).item()
        if is_outlier:
            y += 3.0 * (1 if torch.rand(1).item() > 0.5 else -1)
        nu = torch.tensor([y], dtype=dtype) - H @ mu_pred
        S = H @ Sigma_pred @ H.T + R
        d2, accept = mahalanobis_gate(nu, S, alpha=0.01, m=1)
        if is_outlier:
            out_total += 1
            if not accept: out_rejected += 1
        else:
            good_total += 1
            if not accept: good_rejected += 1
        if accept:
            mu, Sigma, _, _ = kf_update_joseph(mu_pred, Sigma_pred, torch.tensor([y], dtype=dtype), H, R)
        errors.append((mu - torch.tensor([0.5 + 0.1 * math.sin(0.3 * t), 0.1 * 0.3 * math.cos(0.3 * t)], dtype=dtype)).norm().item())
    return good_rejected / good_total, out_rejected / out_total, sum(errors) / T

false_rej, detection, err_gated = run_with_gate()
print$f"\nfalse_rejection_rate = {false_rej:.4f} (target 0.01)"$
print(f"outlier_detection_rate = {detection:.4f} (predicted > 0.95)")

# --- Step 3: filter without gate, for comparison ---
def run_without_gate(outlier_frac=0.05, T=1000):
    mu = torch.tensor([0.5, 0.1], dtype=dtype)
    Sigma = torch.diag(torch.tensor([0.01, 0.04], dtype=dtype))
    errors = []
    for t in range(T):
        mu_pred = A @ mu
        Sigma_pred = A @ Sigma @ A.T + Q
        y = 0.5 + 0.1 * math.sin(0.3 * t) + SIG_Y * torch.randn(1).item()
        if torch.rand(1).item() < outlier_frac:
            y += 3.0 * (1 if torch.rand(1).item() > 0.5 else -1)
        mu, Sigma, _, _ = kf_update_joseph(mu_pred, Sigma_pred, torch.tensor([y], dtype=dtype), H, R)
        errors.append((mu - torch.tensor([0.5 + 0.1 * math.sin(0.3 * t), 0.1 * 0.3 * math.cos(0.3 * t)], dtype=dtype)).norm().item())
    return sum(errors) / T

err_ungated = run_without_gate()
print(f"mean_error_filtered (ungated) = {err_ungated:.4f}")
print(f"mean_error_gated = {err_gated:.4f}")
print(f"relative reduction = {(err_ungated - err_gated) / err_ungated:.2%}")

# --- Step 4: information vs. covariance forms ---
def kf_information_form(mu_pred, Sigma_pred, y, H, R):
    $Y_pred$ = torch.linalg.inv(Sigma_pred)
    yhat_pred = $Y_pred$ @ mu_pred
    Y = $Y_pred$ + H.T @ torch.linalg.inv(R) @ H
    yhat = yhat_pred + H.T @ torch.linalg.inv(R) @ y
    Sigma = torch.linalg.inv(Y)
    mu = Sigma @ yhat
    return mu, Sigma

mu = torch.tensor([0.5, 0.1], dtype=dtype)
Sigma = torch.diag(torch.tensor([0.01, 0.04], dtype=dtype))
mu_pred = A @ mu
Sigma_pred = A @ Sigma @ A.T + Q
y = torch.tensor([0.7], dtype=dtype)
mu_cov, Sigma_cov, _, _ = kf_update_joseph(mu_pred, Sigma_pred, y, H, R)
mu_info, Sigma_info = kf_information_form(mu_pred, Sigma_pred, y, H, R)
print$f"\ninformation vs. covariance mu diff: {(mu_cov - mu_info).norm().item():.2e}"$
print(f"information vs. covariance Sigma diff: {(Sigma_cov - Sigma_info).abs().max().item():.2e}")

# --- Step 5: break-it #3 — singular S (loud: inv raises) ---
print("\n[break-it #3] singular S:")
Sigma_sing = torch.tensor([[0.01, 0.], [0., 0.]], dtype=dtype)
$H_sing$ = torch.tensor([[0., 1.]], dtype=dtype)
$R_sing$ = torch.tensor([[0.]], dtype=dtype)
$S_sing$ = $H_sing$ @ Sigma_sing @ $H_sing$.T + $R_sing$
print(f"S = {S_sing.item()}, det = {torch.linalg.$det(S_sing$).item():.2e}")
try:
    $K_sing$ = Sigma_sing @ $H_sing$.T @ torch.linalg.$inv(S_sing$)
    print(f"K = {K_sing.tolist()}")
except Exception as e:
    print(f"raised: {e}")

# --- Step 6: break-it #2 — gate with R instead of S ---
def gate_with_R(nu, R, m=1):
    d2 = (nu.T @ torch.linalg.inv(R) @ nu).item()
    return d2, d2 < 6.635
# Compare the false-rejection rates: with S (correct) vs. with R (wrong)
torch.manual_seed(123)
rejected_S = 0; rejected_R = 0; $n_good$ = 1000
for _ in $range(n_good$):
    mu_pred = A @ torch.tensor([0.5, 0.1], dtype=dtype)
    Sigma_pred = A @ torch.diag(torch.tensor([0.01, 0.04], dtype=dtype)) @ A.T + Q
    y = 0.5 + SIG_Y * torch.randn(1).item()  # no outlier, just noise matching R
    nu = torch.tensor([y], dtype=dtype) - H @ mu_pred
    S = H @ Sigma_pred @ H.T + R
    _, acc_S = mahalanobis_gate(nu, S, alpha=0.01, m=1)
    _, acc_R = gate_with_R(nu, R, m=1)
    if not acc_S: rejected_S += 1
    if not acc_R: rejected_R += 1
print(f"\n[break-it #2] false-rejection with S: {rejected_S/n_good:.4f}")
print(f"[break-it #2] false-rejection with R: {rejected_R/n_good:.4f}")

# --- Step 7: summary ---
print("\n--- lab 0.3.4 summary ---")
print(f"H1: at the OPTIMAL gain both forms are symmetric (naive {max(drifts_naive):.2e}); "
      f"the defect needs a suboptimal gain (see the c-sweep above)")
print(f"H2: at the optimal gain both stay PSD (naive min eig {min(eigs_naive):+.2e}); "
      f"at c=8 the naive form goes negative and Joseph does not")
print(f"H3: false-rej = {false_rej:.4f} (target 0.01), detection = {detection:.4f} (predicted > 0.95)")
print(f"H4: error reduction = {(err_ungated - err_gated) / err_ungated:.2%} (predicted > 30%)")
print(f"H5: information vs. covariance diff = {(Sigma_cov - Sigma_info).abs().max().item():.2e}")

§L.7 — The cheat sheet

§L.8 — Exercises with solutions

E 0.3.4.1 — The two forms (conceptual). For a state n = 100 and a measurement |y| = 5, which form’s update is cheaper, and by how much?

S 0.3.4.1. Count the dominant terms, with Σ⁻ of size n × n, H of size m × n, R of size m × m.

Covariance form: H Σ⁻ costs m·n·n = mn²; multiplying by Hᵀ costs m·n·m = m²n; inverting the m × m S costs m³; and Σ⁻HᵀS⁻¹ costs mn² + m²n again. Total O(mn² + m³).

Information form: HᵀR⁻¹H costs m²n + mn²; adding it to Σ⁻¹ is O(n²); and inverting the n × n information matrix costs n³. Total O(mn² + m²n + n³).

The two share the mn² term, so the comparison is between the two cubes: m³ for the covariance form against n³ for the information form. At n = 100, m = 5, the covariance form is O(5·10⁴ + 125) ≈ 5 × 10⁴ and the information form is O(10⁶ + …) — twenty times more expensive, and the gap is entirely the n³ inverse. The information form wins when m > n: many measurements of a small state. The lesson: the crossover is m = n, and it is a statement about which matrix gets inverted, not about the HᵀR⁻¹H product, which both forms pay for.

E 0.3.4.2 — The Joseph form (numerical). Σ⁻ = [[0.02, 0.005], [0.005, 0.03]], H = [1, 0], R = 0.01, optimal K = [0.6667, 0.1667]ᵀ. Compute the Joseph form’s Σ.

S 0.3.4.2. KH = [0.6667, 0.1667]ᵀ·[1, 0] = [[0.6667, 0], [0.1667, 0]]. I − KH = [[0.3333, 0], [-0.1667, 1]]. (I − KH)Σ⁻ = [[0.00667, 0.00167], [-0.00333 + 0.005, -0.000833 + 0.03]] = [[0.00667, 0.00167], [0.00167, 0.02917]]. (I − KH)Σ⁻(I − KH)ᵀ = [[0.00667, 0.00167], [0.00167, 0.02917]]·[[0.3333, -0.1667], [0, 1]] = [[0.00222, 0.00056], [0.00056, 0.02917]] (check: [0][0] = 0.00667·0.3333 = 0.00222; [0][1] = 0.00667·(-0.1667) + 0.00167·1 = -0.00111 + 0.00167 = 0.00056; [1][1] = 0.00167·(-0.1667) + 0.02917·1 = -0.00028 + 0.02917 = 0.02889). KRKᵀ = [0.6667, 0.1667]ᵀ·0.01·[0.6667, 0.1667] = [[0.00444, 0.00111], [0.00111, 0.00028]]. $Σ_J$ = [[0.00222 + 0.00444, 0.00056 + 0.00111], [0.00056 + 0.00111, 0.02889 + 0.00028]] = [[0.00667, 0.00167], [0.00167, 0.02917]]. The lesson: the Joseph form’s $Σ_J$ is the same as the naive form’s for the optimal K, and it is symmetric by construction.

E 0.3.4.3 — The gate’s threshold (numerical). For m = 1 and α = 0.01, what is the threshold? For m = 3?

S 0.3.4.3. χ²_{0.99}(1) ≈ 6.635, χ²_{0.99}(3) ≈ 11.345. The larger m, the larger the threshold (more degrees of freedom). The lesson: the threshold depends on m, and using the 1D threshold for a 3D measurement rejects too many good measurements.

E 0.3.4.4 — The gate’s false-rejection (diagnostic). A learner’s gate rejects 10% of good measurements with α = 0.01. What is the likely bug?

S 0.3.4.4. The bug is using R instead of S in the Mahalanobis distance. The R is smaller than S, so the d² is larger, and the gate is too tight — 10× the intended rejection rate. The fix is S = HΣ⁻Hᵀ + R. The lesson: the gate’s S must include the prior’s contribution; using R alone makes the gate too aggressive.

E 0.3.4.5 — The EKF’s linearization error (diagnostic). A robot’s EKF diverges when the joint angles are large, but works for small angles. What is the cause, and what is the fix?

S 0.3.4.5. The cause is the EKF’s linearization error — the first-order Taylor expansion is accurate for small angles (the O(‖x − μ‖²) is small) and inaccurate for large angles (the O(‖x − μ‖²) is large). The fix is the UKF (second-order accurate), the iterated EKF (re-linearize at the updated estimate), or a manifold-aware parameterization (the 0.2.2 quaternion for rotations). The lesson: the EKF’s validity is local, and the “large angle” regime is outside its validity — the fix is the higher-order filter, not a tuning of Q or R.

E 0.3.4.6 — The multimodal case (diagnostic, transfer probe). A robot’s localization in a symmetric corridor gives a single position that is the average of two possible positions, and the covariance is huge. What is the cause, and what is the fix?

S 0.3.4.6. The cause is the multimodal posterior (two equally likely positions), which the single-Gaussian EKF cannot represent. The EKF’s mean is the average of the two modes, which is not a likely position, and the covariance is large (to cover both modes). The fix is the particle filter (which represents the multiple modes), or a multi-hypothesis filter (a mixture of Gaussians). The lesson: the EKF’s unimodal assumption is the limitation, and the multimodality is the symptom that the single Gaussian cannot handle.

E 0.3.4.7 — The information form with uninformative prior (numerical). Y₀ = 0, H = [1, 0], R = 0.01, y = 0.7. What is the first update’s Y and ŷ?

S 0.3.4.7. HᵀR⁻¹H = [[100, 0], [0, 0]], HᵀR⁻¹y = [70, 0]. Y = 0 + [[100, 0], [0, 0]] = [[100, 0], [0, 0]], ŷ = [70, 0]. The Y is singular (rank 1), and the second component is unobserved (the Y’s second row is zero). The μ is [0.7, undefined], and the undefined is the second component’s unobservability — the prior is flat in that direction, and the measurement does not constrain it. The lesson: the information form handles the uninformative prior exactly (the Y = 0 is well-defined), and the singularity of Y is the unobservability, not a bug.

E 0.3.4.8 — The differentiable KF (constructive, transfer probe). A learner wants to learn the process noise’s covariance Q by backpropagating through the KF’s updates. What is the challenge, and what is the workaround?

S 0.3.4.8. The challenge is that the KF’s S⁻¹ and the matrix inverse are differentiable (autograd handles them), but the KF’s state (μ, Σ) is recurrent (each step depends on the previous), so the backpropagation through time (BPTT) has a long graph, and the gradient can vanish or explode — the same issue as RNNs. The workaround is truncated BPTT (backpropagate through a window of steps), gradient clipping, or the implicit differentiation of the KF’s fixed point (for the steady-state KF). The lesson: the KF’s differentiability is structural (all operations are differentiable), but the recurrence’s gradient is unstable for long sequences, and the practical training needs the truncation or clipping.

§L.9 — Exit test

You are ready for Module 1.1 (Forward Kinematics) when you can, without notes:

1. State the covariance form’s predict and update, and the information form’s update (Y = Y⁻ + HᵀR⁻¹H).
2. State the Y = Σ⁻¹, ŷ = Yμ mapping and the conditions under which the information form is preferred.
3. State the Joseph form, and explain why it is symmetric and PSD for any K.
4. State the symmetrization’s limitation (fixes symmetry, not PSD).
5. State the singular S case, the pseudoinverse fix, and the regularization fix.
6. State the Mahalanobis gate and the chi-squared threshold’s m dependence.
7. Compute the gate’s false-rejection rate and detection rate for a given α and outlier magnitude.
8. State the EKF’s linearization (A = ∂f/∂x, H = ∂h/∂x), and the O(‖x − μ‖²) error.
9. State the UKF’s sigma points and the O(‖x − μ‖³) error.
10. State the PF’s sample-based approximation, its multimodality handling, and the O(1/√N) convergence.
11. Diagnose the naive form’s drift, the gate with R instead of S, and the EKF’s large-angle divergence.
12. State the differentiable KF’s challenge (recurrence’s gradient) and the workaround (truncated BPTT, clipping, implicit differentiation).

If any item is not answerable without notes, return to the section that introduced it: 1 to §L.1, 2 to §L.1.1, 3 to §L.2, 4 to §L.2.1, 5 to §L.2, 6 to §L.3, 7 to §L.3.1 and §L.3.2, 8 to §L.4, 9 to §L.4.1, 10 to §L.4.1, 11 to §L.5 and the individual E’s, 12 to E 0.3.4.8.

End of Module 0.3. The learner has now completed the probability and estimation foundations: Bayes’ rule and the Gaussian algebra (0.3.1), the two point estimators and the log-likelihood (0.3.2), the filtering/smoothing distinction and the RTS smoother (0.3.3), and the numerically robust, outlier-gated, nonlinear-bridged KF implementation (0.3.4). The next module, 1.1 — Forward Kinematics, uses the SE(3) algebra of 0.2.1–0.2.3 and the estimation machinery of 0.3.3–0.3.4 to build the arm’s kinematic model, the Jacobian, and the first state estimator that is specific to the running case.

§L.10 — Provenance and evidence tiers

What this lesson reproduces. Every hypothesis in §L.6.5 is executed by the lab in §L.6.9. These claims are tier A, status R.

What this lesson does not reproduce. The float32 asymmetry figure (1.9e-9) quoted in H1 is stated in the text and not printed by the lab, which runs in float64 only. It is tier B, status S: the ordering (1.9e-9 ≫ 3.5e-18) is the expected ε ratio and is arithmetically forced, but the number itself is a claim the reader cannot check against the shipped run.

H1 and H2 are deliberately weak claims, and the lesson says so. At the optimal gain the two forms are algebraically identical, so H1’s residual is one ulp and H2’s $λ_min$ is positive for both forms. Neither test can separate them — the separation appears only at a suboptimal gain (c = 8), which is why the c-sweep exists. A reader who ran only H1 and H2 would conclude Joseph form is unnecessary, and would be wrong in a way the lesson has arranged to be discoverable rather than hidden.

No external sources are cited in this lesson. The Joseph form is derived in §L.2 by expanding (I−KH)Σ⁻(I−KH)ᵀ + KRKᵀ and checking it against the naive update; the lab measures both. The gate is derived from the chi-square statistic in §L.3. No paper is named, and this section does not supply a citation the lesson does not use.