KD-RIEKF: Kinodynamic Right-Invariant EKF for Legged Robot State Estimation

Abstract

We present KD-RIEKF, a novel state estimation framework that incorporates kinodynamic constraints into the Right-Invariant Extended Kalman Filter (RIEKF). Our framework integrates generalized momentum-based contact estimation, centroidal dynamics, and a noise-adaptive module, improving state estimation accuracy by probabilistically adjusting propagation noise to account for contact uncertainty and sensor noise. A key innovation is the expansion of the ground reaction force (GRF) into a state variable. By using GRF-based acceleration as a measurement, our method significantly reduces estimation errors in position, velocity, and orientation. The integration of contact-force-driven adaptive noise effectively boosts the stability of estimation, especially when the system is undergoing turning, acceleration, or deceleration processes. We validated our algorithm in simulation on highly uneven terrain, showing significant enhancements in z-axis position estimation compared to RIEKF. Further experiments on the Unitree Go2 robot across different speeds demonstrated that even in high-speed scenarios over 200 meters, our method reduced position estimation relative error (RE) by 47% and orientation estimation by 42%, confirming its robustness and accuracy under dynamic locomotion.