A Contracting Hierarchical Observer for Pose-Inertial Fusion

Abstract: This work presents a contracting hierarchical observer that fuses position and orientation measurements with an IMU to generate smooth position, linear velocity, orientation, and IMU bias estimates that are guaranteed to converge to their true values. The proposed approach is composed of two contracting observers. The first is a quaternion-based orientation observer that also estimates gyroscope bias. The output of the orientation observer serves as an input for another contracting observer that estimates position, linear velocity, and accelerometer bias thus forming a hierarchy. We show that the proposed observer guarantees all state estimates converge to their true values. Simulation results confirm the theoretical performance guarantees.