Abstract
This paper proposes a cascaded state estimation framework based on proprioception for robots. A generalized-momentum-based Kalman filter (GMKF) estimates the ground reaction forces at the feet through joint torques, which are then input into an error-state Kalman filter (ESKF) to obtain the robot's prior state estimate. The system's dynamic equations on the Lie group are parameterized using canonical coordinates of the first kind, and variations in the tangent space are mapped to the Lie algebra via the inverse of the right trivialization. The resulting parameterized system state equations, combined with the prior estimates and a sliding window, are formulated as a moving horizon estimation (MHE) problem, which is ultimately solved using a parallel real-time iteration (Para-RTI) technique. The proposed framework operates on manifolds, providing a tightly coupled estimation with higher accuracy and real-time performance, and is better suited to handle the impact noise during foot-ground contact in legged robots. Experiments were conducted on the BQR3 robot, and comparisons with measurements from a Vicon motion capture system validate the superiority and effectiveness of the proposed method.