Fault-Tolerant Navigation Method Aided by Dynamics Model of Quadrotors Based on Federated Unscented Kalman Filter on Lie Group Manifold

In previous navigation positioning methods, a micro inertial measurement unit (MIMU) provides navigation information by measuring the linear acceleration and angular velocity of the quadrotors. However, low-cost MIMU often fails in harsh environments and produces large misalignment angles during initial alignment, reducing the navigation performance of quadrotors. Therefore, this article introduces the dynamics model of quadrotors, integrating the roll and pitch moment model, torque model, drag model, thrust model, XYZ-axis gyroscope, XYZ-axis accelerometer, and global navigation satellite system (GNSS) to form an MIMU fault-tolerant federated Kalman filter based on analytical redundancy. Meanwhile, this article introduces the Lie group manifold theory into the unscented Kalman filter (UKF) to improve the attitude estimation accuracy under large misalignment angles and reduce the influence of attitude errors on velocity and position. The proposed federated UKF on the Lie group manifold can accurately estimate navigation parameters when MIMU fails. Real flight experiment data were used for verification, showing that faults in the XYZ-axis gyroscope, XYZ-axis accelerometer, and dynamics model of quadrotors can be detected and isolated. When there are no faults, the attitude, velocity, and position accuracy of the quadrotor after introducing the dynamics model are increased by more than 5.5%, 3.1%, and 10.7%, respectively. The estimation accuracy of roll, pitch, and yaw increased by more than 88.1%, 68.9%, and 66.4%, respectively, under large misalignment angles.