Proprioceptive State Estimation for Quadruped Robots using Invariant Kalman Filtering and Scale-Variant Robust Cost Functions

TL;DR

A new Invariant Extended Kalman filter for legged robot state estimation using only proprioceptive sensors is presented, formulated by combining recent advances in state estimation theory with the use of robust cost functions in the measurement update.

Abstract

Accurate state estimation is crucial for legged robot locomotion, as it provides the necessary information to allow control and navigation. However, it is also challenging, especially in scenarios with uneven and slippery terrain. This paper presents a new Invariant Extended Kalman filter for legged robot state estimation using only proprioceptive sensors. We formulate the methodology by combining recent advances in state estimation theory with the use of robust cost functions in the measurement update. We tested our methodology on quadruped robots through experiments and public datasets, showing that we can obtain a pose drift up to 40% lower in trajectories covering a distance of over 450m, in comparison with a state-of-the-art Invariant Extended Kalman filter.

Figures (8)

• Figure 1: AlienGo robot walking on slippery terrain.
• Figure 2: Comparison of the $l_2$, Huber, Tukey and ideal cost functions for $c=1.5$.
• Figure 3: Main state variables and reference frames of the robot: $\mathbf{p}$ (IMU centre), $R$ (IMU orientation), $\mathbf{p}_f^{i},i=\{ 1,2,3,4\}$ (feet contact position), fixed frame ($W$), IMU frame ($I$) and base frame ($B$).
• Figure 4: Summary of right-invariant error IEKF. The true state is in black, the nominal state (green), $\bar{\mathcal{X}}_i$, is propagated with (\ref{['eq:dynamics_approximated']}), the right-invariant error, $\boldsymbol{\xi}_{r,i}$, with (\ref{['eq:error_propagation']}) and the state update is given by (\ref{['eq:aux_s']})-(\ref{['eq:state_update']}).
• Figure 5: The Kalman's measurement update for the one-dimensional case can be seen as a least square problem. The estimate can be seen as the intersection of $\mathbf{x}^{T} C^{-1} \mathbf{x}$ with $S_i$ (\ref{['eq:robust_kalman']}) (top). The same problem but changing $\rho$ to the Tukey cost function (c = 2.0). Note that a change of basis by $L$, $C_i = LL^{T}$, should be performed zoubir2018 (bottom).