MEKF
A multiplicative extended Kalman filter written for Windhover Labs. Running in their software-in-the-loop simulation with Gazebo.
Designed to be modular and MISRA-friendly: thin filter core, sensor-owned updates, and compile-time types with Jinja/Python codegen.
About
This is a stability test under strenuous maneuvers. The filter converges quickly, then holds steady through high-G rolls and loops.
Before the first GPS read, everything runs in a local NED frame. Once GPS arrives, that frame is reanchored onto WGS84. The first magnetometer read clamps yaw. There is a visible altitude offset in the video from how Gazebo and the soft GPS sensor register altitude.
Design
Compile-Time Sensor Types
Sensors share one interface used by the filter loop, but every type stays compile-time. Required for static analysis and safety-critical use.
Sensor-Owned Updates
Instead of returning a measurement matrix and innovation to the filter, each sensor receives the nominal state and covariance and completes the update itself.
Jinja Autogeneration
New sensors are added to a master YAML file. Templates generate the sensor scaffolding, including the innovation update and nominal state update.
Delayed Measurement Replay
A history queue stores past filter state so delayed sensors can rewind and replay as if they arrived on time. Built with VIO and image processing delay in mind.
Van Loan + Joseph Update
Propagation uses Van Loan discretization. Covariance updates use a Joseph form for better numerical stability.
RF-Denied Ready
If GPS never locks, the filter stays in a local NED frame. Yaw and barometer altitude do not latch onto a global fix until one is available.
Architecture
A lot of the state machine was inspired by PX4's EKF, but PX4's estimator keeps sensors tightly coupled. Here encapsulation is weakened a bit for modularity. Adding or removing a sensor is only a couple of lines in the thin MEKF core, and the rest is generated.
Everything is wrapped in a StateEstimator that owns the proprietary state machine. It manages landed state, GPS and yaw locking, and attitude confidence stages. Separating the state machine from the MEKF keeps testing easy and the MEKF generic.
Pseudo-sensors live behind the same sensor-agnostic loop as real ones. That keeps special cases out of the state machine and makes them easy to enable or disable for testing.
Sensors
GPS Position / Velocity
Treated as separate sensors since their noises do not require covariance between them. First GPS fix reanchors the local NED frame onto WGS84.
Magnetometer + Declination
Yaw clamps hard onto the first magnetometer read. Declination corrects the offset between true north and magnetic north from WMM lookup tables.
Barometer + IMU
Barometer tracks altitude. The IMU drives propagation; gravity is derived from the accelerometer once bias is learned.
Airspeed + Sideslip
Airspeed observes wind. Sideslip is a pseudo-sensor used for wind velocity on fixed-wing vehicles.
Zero Velocity / Zero Gyro
While landed, these feed zero velocity and zero gyro readings so the filter can learn gyro bias and stop position drift.
Fake Position / Height / Yaw
Pseudo-sensors used when horizontal tracking is lost, before magnetometer latch, or while sitting still. Kept as sensors so they can be swapped for unit tests.
State Vector
The nominal state is configured from YAML and generated into code: position, velocity, attitude, magnetic field, accelerometer bias, gyroscope bias, magnetometer bias, and wind velocity.