2025
GNSS/INS Sensor Fusion Filter
An error-state Kalman filter in Rust that fuses inertial and GNSS measurements, written to learn state estimation properly.
- Rust
- nalgebra
- Python
- criterion
Why an error-state filter
A full-state Kalman filter on pose is awkward because attitude lives on SO(3), not in a vector space, and the dynamics are nonlinear enough that the usual linearisation is fragile. An error-state EKF estimates a small correction around a nominal trajectory instead: the nominal state carries the nonlinear propagation, and the filter only ever sees a locally Euclidean error vector that stays near zero when the model is honest.
That split is the whole point of the project. I wanted the formulation that shows up in real INS literature, not a textbook 2D position filter with a friendly Jacobian.
Propagation and correction
The IMU runs at a high rate and drives the nominal state: integrate angular rate for attitude, specific force for velocity and position, with gravity and Earth-rate terms kept explicit rather than absorbed into a black-box motion model. GNSS arrives sparsely and corrects position and velocity. Between fixes the filter trusts the inertial model; when a fix lands, the measurement update pulls the error state back and injects the correction into the nominal pose.
Attitude is stored as a unit quaternion and manipulated with nalgebra, so the composition and normalisation steps stay in one place instead of being re-derived at every call site. Process and measurement noise are tuned against synthetic trajectories first, then checked against logged sensor traces where those exist.
Proving it
The Rust core is the filter. A Python harness wraps it for simulation: generate ground-truth trajectories, corrupt them with sensor noise models, run the filter, and plot position, velocity, and attitude error over time. That loop is how the formulation earns trust. Criterion benches cover the predict and update hot paths so a change that makes the maths prettier but the filter slower shows up immediately.
This is a learning project written to the standard I would want in production code: typed state, explicit noise models, and a way to see when the estimate is lying. The repository is not public yet; once it is, the Source link will point at it.