Correlation-aware invariant extended Kalman filter for quadruped robot state estimation

This paper introduces a new Correlation-Aware Invariant Extended Kalman Filter (CA-IEKF) for quadruped robot state estimation. Accurate state estimation is essential for motion planning and control of legged robots. While existing Invariant EKF (IEKF) methods provide strong convergence guarantees on Lie groups, they do not explicitly account for cross-correlation between estimation sources (e.g. IMU-propagated state and kinematic measurements). This cross-correlation arises naturally in sensor fusion systems due to shared clock, common environmental factors, and coupling through robot dynamics, leading to inconsistent estimation when ignored. The main contributions of this work are: (1) A CA-IEKF framework that explicitly handles correlation between estimation sources using Split Covariance Intersection (SCI), enabling consistent fusion without the over-conservatism of standard Covariance Intersection. (2) Integration of geomagnetic measurements within the CA-IEKF framework to achieve observable yaw angle estimation, addressing the well-known unobservability issue in contact-aided inertial estimation. (3) Real-world experiments on the SCIT Dog quadruped robot demonstrating a 67.4% reduction in Absolute Trajectory Error (ATE) and a 9.4× improvement in Sum of Squared Errors (SSE) compared to conventional IEKF, with the proposed method maintaining consistent heading while the baseline exhibits significant yaw drift.