msckf-vio
A from-scratch Multi-State Constraint Kalman Filter for visual-inertial odometry, in NumPy: verified in simulation, then made to survive real flight data.
- 0.23 m
- ATE RMSE on EuRoC MH_01
- 0.29 %
- drift over 80.4 m of flight
- ~70 fps
- in pure Python
Why build an MSCKF by hand?
The Multi-State Constraint Kalman Filter (Mourikis & Roumeliotis, 2007) is the backbone of a whole family of production VIO systems, and it is also one of those algorithms that everyone cites and almost nobody implements from scratch. I wanted the version of understanding you only get by writing every Jacobian yourself, so this is the full filter in NumPy/SciPy, with no estimation libraries doing the thinking.
Proving it works before trusting it
The filter was first verified against an analytic ground-truth simulation: a 60-second, 82-meter trajectory with consumer-grade IMU noise and 1 px image noise.
- MSCKF: 0.23 m position RMSE (0.28 % of trajectory length), 0.13 m final drift
- IMU dead reckoning: 82.5 m error, fully diverged
More important than accuracy: the filter is consistent. Estimation error stays inside its own reported 3σ covariance envelope, which is the property that separates a working EKF from one that merely looks plausible.
Then real data disagreed
The same filter runs on the EuRoC MH_01 sequence through a KLT optical-flow frontend (forward-backward checked, RANSAC pruned), processing 183 seconds of real MAV flight at about 70 frames per second. Final score: 0.233 m ATE RMSE, 0.29 % of the 80-meter trajectory.
Getting there surfaced two failure modes that are invisible in simulation:
Standing still is a failure mode. MH_01 sits stationary for ~20 seconds before takeoff. Zero baseline makes every feature’s depth unobservable, updates get rejected, and the filter quietly dead-reckons 2 m of drift that persists for the whole flight. A zero-velocity update (stationarity detected from IMU variance) cut ATE from 0.84 m to 0.23 m.
Measurement double-counting. Spending a still-live track’s observations at clone marginalization and then reusing those observations in a later update feeds the same information into the filter twice, making it overconfident. Consumed observations are now cleared from the track.
Both fixes are one paragraph here and were several days each in practice, which is roughly the exchange rate for making estimators work on real robots.