MARID Error-State Extended Kalman Filter GPS-Denied Sensor Fusion — Design Document Table of Contents Motivation State Vectors Process Model Process Jacobian F Process Noise Q Measurement Models H Update Step Mechanics Full ESKF Cycle Implementation Notes Future Work: Learning-Augmented ESKF 1. Motivation The existing marid_odom_pub.py node fuses ten sensor sources via hand-tuned scalar complementary filters: $$z_\text{fused} = w \cdot z_\text{sensor} + (1-w) \cdot z_\text{prior}$$ The weights $w$ are constants set at launch time. They do not adapt when sensor quality degrades (e.g. FAST-LIO scan-matching fails at altitude), and they carry no covariance — uncertainty is not propagated across sensor boundaries. The Error-State Extended Kalman Filter (ESKF) replaces these fixed weights with a principled estimator that: Computes the Kalman g...
