Kalman filter

Deep dive

How it works

In: previous x, P, model F, Q, measurement z with H, R · Out: updated x, P and the innovation · State: mean x and covariance P (N + N^2 floats) · Cost: O(N^3) per step dense, plus O(M^3) for M measurements

Model: x_k = F x_{k-1} + w, w ~ N(0, Q); measurement z_k = H x_k + v, v ~ N(0, R).

Predict:

1. x = F x
2. P = F P F^T + Q

Update with measurement z:

1. Innovation y = z - H x
2. Innovation covariance S = H P H^T + R
3. Gain K = P H^T S^-1
4. x = x + K y
5. P = (I - K H) P (I - K H)^T + K R K^T (Joseph form)

          x, P ──predict──► x-, P- ──update(z)──► x, P ──► next step
 trust:   model              grows (+Q)   shrinks via K, K -> 0 if R is huge, K -> H^-1 if R -> 0

Scalar intuition: K = P-/(P- + R), the fraction of the innovation you believe. Only y and S depend on the measurement, so a missing or rejected measurement means skipping the update, nothing else. Per-step consistency check: y^T S^-1 y (NIS) is chi-square with M degrees of freedom, so it gates outliers and diagnoses mistuned Q/R.

Code

pub type Mat<const R: usize, const C: usize> = [[f64; C]; R];

pub fn mul<const A: usize, const B: usize, const C: usize>(x: &Mat<A, B>, y: &Mat<B, C>) -> Mat<A, C> {
    let mut o = [[0.0; C]; A];
    for i in 0..A { for k in 0..B { for j in 0..C { o[i][j] += x[i][k] * y[k][j]; } } }
    o
}
pub fn t<const R: usize, const C: usize>(x: &Mat<R, C>) -> Mat<C, R> {
    let mut o = [[0.0; R]; C];
    for i in 0..R { for j in 0..C { o[j][i] = x[i][j]; } }
    o
}
pub fn add<const R: usize, const C: usize>(x: &Mat<R, C>, y: &Mat<R, C>) -> Mat<R, C> {
    let mut o = *x;
    for i in 0..R { for j in 0..C { o[i][j] += y[i][j]; } }
    o
}

/// Solve S X = B for symmetric positive-definite S via Cholesky (no explicit inverse).
fn chol_solve<const M: usize, const C: usize>(s: &Mat<M, M>, b: &Mat<M, C>) -> Mat<M, C> {
    let mut l = [[0.0; M]; M];
    for i in 0..M {
        for j in 0..=i {
            let mut v = s[i][j];
            for k in 0..j { v -= l[i][k] * l[j][k]; }
            l[i][j] = if i == j { v.sqrt() } else { v / l[j][j] }; // v <= 0 => S not SPD
        }
    }
    let mut x = *b;
    for c in 0..C {
        for i in 0..M { // forward: L y = b
            for k in 0..i { x[i][c] -= l[i][k] * x[k][c]; }
            x[i][c] /= l[i][i];
        }
        for i in (0..M).rev() { // backward: L^T x = y
            for k in i + 1..M { x[i][c] -= l[k][i] * x[k][c]; }
            x[i][c] /= l[i][i];
        }
    }
    x
}

pub struct Kalman<const N: usize, const M: usize> {
    pub x: [f64; N],
    pub p: Mat<N, N>,
    pub f: Mat<N, N>,
    pub q: Mat<N, N>,
    pub h: Mat<M, N>,
    pub r: Mat<M, M>,
}

impl<const N: usize, const M: usize> Kalman<N, M> {
    pub fn predict(&mut self) {
        for i in 0..N { self.x[i] = (0..N).map(|j| self.f[i][j] * self.x[j]).sum(); }
        self.p = add(&mul(&mul(&self.f, &self.p), &t(&self.f)), &self.q);
    }

    pub fn update(&mut self, z: &[f64; M]) {
        let hp = mul(&self.h, &self.p);                       // M x N
        let s = add(&mul(&hp, &t(&self.h)), &self.r);         // innovation covariance
        let k = t(&chol_solve(&s, &hp));                      // K = P H^T S^-1 = (S^-1 H P)^T
        let mut y = [0.0; M];                                 // innovation
        for i in 0..M { y[i] = z[i] - (0..N).map(|j| self.h[i][j] * self.x[j]).sum::<f64>(); }
        for i in 0..N { self.x[i] += (0..M).map(|j| k[i][j] * y[j]).sum::<f64>(); }
        // Joseph form: stays symmetric and positive semi-definite under rounding
        let kh = mul(&k, &self.h);
        let mut a = [[0.0; N]; N];                            // A = I - K H
        for i in 0..N { for j in 0..N { a[i][j] = (i == j) as u8 as f64 - kh[i][j]; } }
        self.p = add(&mul(&mul(&a, &self.p), &t(&a)), &mul(&mul(&k, &self.r), &t(&k)));
    }
}
// Constant-velocity tracker: state [pos, vel], position measured, dt = 0.1 s
// let mut kf = Kalman::<2, 1> { x: [0.0; 2], p: [[100.0, 0.0], [0.0, 100.0]],
//     f: [[1.0, dt], [0.0, 1.0]], q, h: [[1.0, 0.0]], r: [[0.25]] };
// loop { kf.predict(); if let Some(z) = sensor() { kf.update(&[z]); } }

Checked by test: the scalar random-walk case converges to the Riccati solution (q + sqrt(q^2 + 4qr))/2, and a 2-state constant-velocity filter (dt 0.1, measurement sigma 0.5) cut position RMS error from 0.52 (raw measurements) to 0.12 and recovered the true velocity.

Complexity and constants

• Dense: predict F P F^T costs 2 N^3 multiply-adds, update's Joseph form another 2 N^3, plus H P (M N^2) and a Cholesky of S (M^3/6). Order of 4 N^3 per step; P is N^2 x 8 bytes (15 states: 1.8 KB, fits L1).
• No syscalls, no allocation, no I/O: pure arithmetic on a small hot working set.
• Measured on an Apple M4 Pro, one core, the code above (f64, dense, const-generic, no BLAS, one predict plus one update): N=2/M=1 33 ns; N=6/M=3 181 ns; N=15/M=3 1.7 us; N=30/M=6 10.4 us.
• For N of a few hundred, cost is N^3 flops and P stops fitting in cache; use BLAS or move to ensemble or sparse/information forms.

Efficiency

Bound: compute (N^3 matrix products) for a single filter; memory bandwidth when thousands of filters run in a batch.

• Exploit structure of F, H, Q: constant-velocity models are block diagonal per axis; skipping zero blocks turns N^3 into a sum of small cubes.
• Sequential scalar updates: if R is diagonal, apply the M measurements one by one; each is O(N^2) and divides by a scalar, so no M x M factorization at all (same result).
• Steady-state gain: for a time-invariant model with constant Q, R, P converges; precompute the converged K (iterate the Riccati recursion offline) and run x = F x; x += K (z - H F x) at O(N^2) per step. That is the alpha-beta filter family, and fine when dt and noise are constant.
• Gate before updating: a cheap NIS test rejects outliers and saves the update work and the damage.
• Batch many filters: lay out P, x as structure-of-arrays across targets so one SIMD op serves several filters (what simdkalman does across series). f32 halves memory traffic and doubles SIMD width where conditioning allows (see pitfalls).
• Cheaper alternative: if one scalar gain is enough (attitude from gyro plus accelerometer, say), a complementary filter costs a handful of flops. Run predict at the fast sensor rate and update only when measurements actually arrive.

Implementation notes

• Solve with a Cholesky factorization of S (LU/Cholesky), not an explicit inverse; S is symmetric positive definite when R is.
• Store P symmetric; with the Joseph form it stays symmetric and positive semi-definite under rounding. The short form (I - K H) P is a matmul cheaper but drifts asymmetric over long runs; if used, symmetrize (P + P^T)/2.
• Q depends on the time step: for a white-acceleration model, Q = q * [[dt^4/4, dt^3/2], [dt^3/2, dt^2]]. Recompute F and Q per step if dt varies.
• Initialize P large for unknown states (it then follows the first measurements), small only where you are sure.
• Per-step memory layout: fixed-size arrays on the stack (const generics here), row-major, no heap; keep F, H precomputed per mode.
• Embedded targets: f32 needs the Joseph form or a square-root form (UD or Cholesky of P) to stay stable; verify the accuracy loss against an f64 run on your own data.
• Keep timestamps, not step counters: out-of-order or dropped measurements are handled by predicting to the measurement time.

Pitfalls

• Q too small: the filter trusts the model, lags real maneuvers and its covariance lies about the error. Q too large: noisy output. Tune with NIS and logged innovations (they should be white, zero mean, covariance S).
• Treating Q as a free constant when dt changes.
• Wrapping angles: the innovation z - H x must be wrapped to (-pi, pi].
• Non-symmetric or indefinite P from the short update form or f32: leads to negative variances and divergence.
• Correlated measurement noise treated as independent (e.g. repeated/filtered sensor data): overconfident estimate; inflate R.
• Unobservable states (H and F cannot determine them): P for them grows without bound; observability must be checked.
• Applying it to a nonlinear model without linearizing, or with a linearization point far from the truth.

Variants and alternatives

In production

Spacecraft and aircraft navigation (inertial + GPS fusion; the filter's path into aerospace is told in McGee and Schmidt, NASA), flight controllers (PX4's EKF2, ArduPilot's EKF), GPS receivers, radar/target tracking, SLAM back-ends (SLAM, visual-inertial odometry), battery state-of-charge estimation in electric vehicles (Plett, extended filter), and video object trackers such as SORT (constant-velocity filter on bounding boxes, Bewley et al.).

Implementations

Sources

• R. E. Kalman, A New Approach to Linear Filtering and Prediction Problems, J. Basic Engineering 82(1):35-45, 1960. paper
• Greg Welch, Gary Bishop, An Introduction to the Kalman Filter, UNC Chapel Hill TR 95-041, 1995 (rev. 2006).
• Yaakov Bar-Shalom, X. Rong Li, Thiagalingam Kirubarajan, Estimation with Applications to Tracking and Navigation, Wiley 2001.
• Leonard A. McGee, Stanley F. Schmidt, Discovery of the Kalman filter as a practical tool for aerospace and industry, NASA TM-86847, 1985. report
• Roger R. Labbe, Kalman and Bayesian Filters in Python, open book. code
• Gerald J. Bierman, Factorization Methods for Discrete Sequential Estimation, Academic Press 1977 (square-root / UD forms).
• Dan Simon, Optimal State Estimation, Wiley 2006.