An Equivariant Approach to Robust State Estimation for the ArduPilot Autopilot System

The majority of commercial and open-source autopilot software for uncrewed aerial vehicles rely on the tried and tested extended Kalman filter (EKF) to provide the state estimation solution for the inertial navigation system (INS). While modern implementations achieve remarkable robustness, it is often due to the careful implementation of exception code for a multitude of corner cases along with significant skilled tuning effort. In this paper, we use the data wealth of the ArduPilot community to identify and highlight the most common real-world challenges in INS state estimation, including sensor self-calibration, robustness in static conditions, global navigation satellite system (GNSS) outliers and shifts, and robustness to faulty inertial measurement units (IMUs). We propose a novel equivariant filter (EqF) formulation for the INS solution that exploits a Semi-Direct-Bias symmetry group for multi-sensor fusion with self-calibration capabilities and incorporates equivariant velocity-type measurements. We augment the filter with a simple innovation-covariance inflation strategy that seamlessly handles GNSS outliers and shifts without requiring coding of a whole set of exception cases. We use real-world data from the Ardupilot community to demonstrate the performance of the proposed filter on known cases where existing filters fail without careful exception handling or case-specific tuning and benchmark against the ArduPilot's EKF3, the most sophisticated EKF implementation currently available.