Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation

by   Ross Hartley, et al.

Legged robots require knowledge of pose and velocity in order to maintain stability and execute walking paths. Current solutions either rely on vision data, which is susceptible to environmental and lighting conditions, or fusion of kinematic and contact data with measurements from an inertial measurement unit (IMU). In this work, we develop a contact-aided invariant extended Kalman filter (InEKF) using the theory of Lie groups and invariant observer design. This filter combines contact-inertial dynamics with forward kinematic corrections to estimate pose and velocity along with all current contact points. We show that the error dynamics follows a log-linear autonomous differential equation with several important consequences: (a) the observable state variables can be rendered convergent with a domain of attraction that is independent of the system's trajectory; (b) unlike the standard EKF, neither the linearized error dynamics nor the linearized observation model depend on the current state estimate, which (c) leads to improved convergence properties and (d) a local observability matrix that is consistent with the underlying nonlinear system. Furthermore, we demonstrate how to include IMU biases, add/remove contacts, and formulate both world-centric and robo-centric versions. We compare the convergence of the proposed InEKF with the commonly used quaternion-based EKF though both simulations and experiments on a Cassie-series bipedal robot. Filter accuracy is analyzed using motion capture, while a LiDAR mapping experiment provides a practical use case. Overall, the developed contact-aided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system.


Contact-Aided Invariant Extended Kalman Filtering for Legged Robot State Estimation

This paper derives a contact-aided inertial navigation observer for a 3D...

Legged Robot State Estimation in Slippery Environments Using Invariant Extended Kalman Filter with Velocity Update

This paper proposes a state estimator for legged robots operating in sli...

Fully Proprioceptive Slip-Velocity-Aware State Estimation for Mobile Robots via Invariant Kalman Filtering and Disturbance Observer

This paper develops a novel slip estimator using the invariant observer ...

Deep Multi-Modal Contact Estimation for Invariant Observer Design on Quadruped Robots

This work reports on developing a deep learning-based contact estimator ...

Whole-Body Human Kinematics Estimation using Dynamical Inverse Kinematics and Contact-Aided Lie Group Kalman Filter

Full-body motion estimation of a human through wearable sensing technolo...

On the Use of Torque Measurement in Centroidal State Estimation

State of the art legged robots are either capable of measuring torque at...

A Nonlinear Observer for Free-Floating Target Motion using only Pose Measurements

In this paper, we design a nonlinear observer to estimate the inertial p...

Please sign up or login with your details

Forgot password? Click here to reset