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

TL;DR

This paper introduces a fast, decentralized state estimation framework for legged robots using EKF and MHE, achieving high-frequency, accurate estimates of orientation and velocity to improve locomotion control.

Contribution

It proposes a novel combination of EKF and MHE with a marginalization method to efficiently estimate states in legged robots, balancing accuracy and computational load.

Findings

Capable of 200 Hz estimation on various robots

Accurate orientation and velocity estimation demonstrated

Effective for highly dynamic legged locomotion

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.