Fast Decentralized State Estimation for Legged Robot Locomotion via EKF and MHE

TL;DR

This work tackles the challenge of fast, accurate state estimation for legged robots under dynamic locomotion by decoupling the problem into nonlinear orientation estimation via an EKF and linear velocity estimation via Moving Horizon Estimation (MHE). It introduces a marginalization-based arrival cost that converts the Full Information Filter into an equivalent constrained MHE, enabling multirate sensor fusion and state constraints within a fixed window. The approach is validated on multiple platforms (PogoX, Cassie, Unitree Go1) at 200 Hz with a 0.1 s window, demonstrating improved velocity/orientation estimates using IMU, joint encoders, and VO data, and achieving real-time performance with open-source software. The work advances practical, high-frequency, constraint-aware state estimation for legged locomotion with potential impact on control robustness and autonomy in outdoor environments.

Abstract

In this paper, we present a fast and decentralized state estimation framework for the control of legged locomotion. The nonlinear estimation of the floating base states is decentralized to an orientation estimation via Extended Kalman Filter (EKF) and a linear velocity estimation via Moving Horizon Estimation (MHE). The EKF fuses the inertia sensor with vision to estimate the floating base orientation. The MHE uses the estimated orientation with all the sensors within a time window in the past to estimate the linear velocities based on a time-varying linear dynamics formulation of the interested states with state constraints. More importantly, a marginalization method based on the optimization structure of the full information filter (FIF) is proposed to convert the equality-constrained FIF to an equivalent MHE. This decoupling of state estimation promotes the desired balance of computation efficiency, accuracy of estimation, and the inclusion of state constraints. The proposed method is shown to be capable of providing accurate state estimation to several legged robots, including the highly dynamic hopping robot PogoX, the bipedal robot Cassie, and the quadrupedal robot Unitree Go1, with a frequency at 200 Hz and a window interval of 0.1s.

Figures (9)

• Figure 1: The proposed estimation framework (bottom) and its application for realizing dynamic hopping on PogoX (top).
• Figure 2: Illustration of MHE, arrival cost $\Gamma$ and their relationship with FIF, where $\hat{x}_{i|k}$, $\{i,k \in \mathbb{N}^+|\ 0 \leq i \leq k \leq T \}$ is the estimate of $x$ at time index $i$, using measurements from time index 0 to time index $k$.
• Figure 3: The VO measurements are synchronized with the nearest IMU frames and interpolated at all IMU frames as the motion constraints.
• Figure 4: (a) PogoX hardware and reference frames. (b) Indoor experiment of square hopping. (c) Bipedal robot Cassie hardware and reference frames. (d) Quadrupedal robot Unitree Go1 hardware and reference frames.
• Figure 5: Block Diagram of the Decentralized Estimation on PogoX.