State estimationEngineering methods

State estimation

Recovering hidden state from noisy sensors with the Kalman family — linear KF, EKF and UKF, then GPS+IMU sensor fusion that holds a track straight through a GPS dropout, an EKF battery State-of-Charge estimator, and the consistency and outlier-rejection machinery that says when a filter can be trusted.

DjiniousLabModelling & simulation

A fused GPS+IMU estimate tracking an S-shaped path through a GPS dropout while raw GPS scatters noisily around it

fused error vs raw GPS through a dropout: 0.28×fused error vs raw GPS through a dropout
Kalman RMSE vs raw measurement: 0.22×Kalman RMSE vs raw measurement
RMSE cut from outlier gating: 84%RMSE cut from outlier gating
EKF convergence from a bad guess: 1.10 sEKF convergence from a bad guess
design notebooks: 10design notebooks
requirements PASS: 5 / 5requirements PASS

Every other model simulates a plant. This one reads the sensors.

Most engineering models push a system forward in time. Real systems also have to run the other direction: take noisy, intermittent sensor readings and recover the true state behind them — position from jittering GPS, charge from a wandering battery voltage, an angle from a shaking gyro. That's state estimation, and the Kalman filter family is how it's done. This program builds it from the linear filter up through sensor fusion, and — just as important — shows the consistency, observability and outlier checks that tell you when to believe the answer.

Trust the model and the measurement in the right proportion.

The Kalman filter runs a predict/update recursion: a model predicts where the state should go, a measurement corrects it, and the filter weighs the two by their uncertainties — leaning on the model when the sensor is noisy, on the sensor when the model drifts. On a constant-velocity tracking problem it cuts position error to 0.22× the raw measurement and recovers an unmeasured velocity it never sees directly, while its ±1σ confidence band visibly shrinks as it locks on.

A linear Kalman filter estimate tracking the truth through noisy measurements with a shrinking 1-sigma band — The linear Kalman filter: the estimate (blue) threading the noisy measurement scatter (red) onto the truth (green), with the ±1σ band shrinking from 1.69 to 0.47 m as the filter gains confidence. It also recovers velocity — a state never directly measured.

When the system curves, linearize — by Jacobian or by sigma points.

Real systems aren't linear. The Extended Kalman Filter linearizes the dynamics with an analytic Jacobian each step; the Unscented Kalman Filter skips the calculus entirely and propagates a set of sigma points through the true nonlinear model. On a pendulum the EKF locks on from a deliberately bad initial guess in 1.1 seconds, and the UKF matches or beats it under a hard 2.8-radian swing — no Jacobian required.

An EKF estimate converging onto the true pendulum angle from a bad initial guess — The EKF on a nonlinear pendulum: starting from a deliberately wrong initial angle, the filter linearizes each step through the analytic Jacobian and converges onto the truth in 1.1 s — then tracks it through the swing.

Fused GPS+IMU estimate holding an S-shaped track through a GPS dropout via IMU dead-reckoning — The hero: fusing a slow, noisy GPS with a fast IMU. When GPS drops out (the orange dashed segment) the fused estimate keeps tracking truth on IMU dead-reckoning alone — 0.28× the raw-GPS error — then snaps back when GPS returns. The uncertainty breathes outward across the gap and contracts again: the filter knows when it's coasting.

Estimate what you can't measure — a battery's charge.

You can't put a fuel gauge inside a battery; State-of-Charge has to be estimated from terminal voltage and current. An EKF on an equivalent-circuit battery model corrects a deliberately wrong initial SoC down to a 0.0002 final error, where naive coulomb-counting keeps a 0.4 offset forever. It's the same estimator family as the navigation filter, pointed at a different hidden state — and it pairs directly with the EV-battery flagship.

An EKF battery State-of-Charge estimate correcting a wrong initial value using terminal voltage — EKF battery State-of-Charge: the voltage measurement pulls a wrong initial SoC back to truth, where open-loop coulomb-counting would carry the initial error indefinitely. Estimating a state no sensor reads directly is the whole point.

A filter that's confidently wrong is worse than no filter.

The dangerous failure isn't a noisy estimate — it's a wrong one reported with tight confidence. The program makes that explicit: NEES/NIS consistency checks tell you when the filter's claimed uncertainty matches reality; an observability analysis shows a mode the sensors simply can't resolve; and a divergence case has an overconfident filter walk 16 m off truth while claiming 12 cm of error — then fixes it with honest process noise. A χ²-innovation gate rejects 30 m outliers and cuts tracking error 84%.

A gated Kalman filter staying on truth while an ungated filter jerks toward outlier measurements — Outlier rejection: a χ²-innovation gate (blue) holds the track through 30 m outliers while the ungated filter (orange) lurches at every bad reading — an 84% RMSE cut. Robustness isn't an add-on; it's what makes an estimator deployable.

Every number re-derived at sign-off.

The V&V notebook rebuilds each filter from scratch and re-derives the requirements, printing a PASS/FAIL board.

Result

Linear KF vs raw measurement: 0.22×
EKF convergence from bad init: 1.10 s
UKF vs EKF (hard nonlinearity): 0.98×
Fused GPS+IMU through dropout: 0.28×
Filter consistency (NEES / NIS): 1.87 / 0.98

Requirement

Linear KF vs raw measurement: R-01 < 0.5
EKF convergence from bad init: R-02 < 2 s
UKF vs EKF (hard nonlinearity): R-03 ≤ 1
Fused GPS+IMU through dropout: R-04 < 0.5
Filter consistency (NEES / NIS): R-05

Reproducible filters on idealized sensors.

The deliverable is ten notebooks and the dossier — estimation is a recursive algorithm on sensor data, not an acausal network, so there's no custom block or canvas. The noise is synthetic Gaussian, the IMU and battery sensor models are simplified, and everything is discrete-time; the UKF-vs-EKF margin is modest because these systems are only gently nonlinear, and the coordinated-turn-vs-constant-velocity comparison is close. What the program does deliver is the full estimator family — KF, EKF, UKF, fusion, consistency, observability, divergence, gating — on numbers you can re-run, and it slots straight into the autopilot→Rust-codegen path, because an estimator is exactly the kind of code that has to be certified before it flies.