Two-Stage Proprioceptive State Estimation with Stability Guarantee for Legged Robots

To achieve dynamic legged locomotion, real-time acquisition of the robot floating base state is critical. However, current full-coupled state estimation solutions may suffer from convergence issues. To address this, a two-stage state estimator that uses proprioceptive sensors is proposed to estimate the floating base's velocity and pose. The approach decomposes the floating-base state estimation problem into base orientation estimation and base velocity and position estimation, resulting in a two-stage multi-sensor fusion algorithm with stability guarantees. Additionally, a covariance inflation method is introduced to consider the influences of contact switching by adjusting noise covariances. The proposed state estimator has been successfully implemented and verified on a real quadrupedal robot, supporting dynamic motions such as multi-gaited locomotion and push recovery.