Abstract: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.

Distributed Invariant Extended Kalman Filter Using Lie Groups: Algorithm and Experiments

Distributed Invariant Kalman Filter for Cooperative Localization using Matrix Lie Groups

Distributed Invariant Unscented Kalman Filter based on Inverse Covariance Intersection with Intermittent Measurements

A Cubature Kalman Filtering Algorithm for Robot Pose Estimation

Invariant Cubature Kalman Filtering-Based Visual-Inertial Odometry for Robot Pose Estimation

Adaptive Robust Invariant Extended Kalman filtering for Biped Robot.

Robust State Estimation Via Maximum Correntropy EKF on Matrix Lie Groups with Application to Low-Cost INS/GPS-Integrated Navigation System

Distributed Invariant Kalman Filter for Object-level Multi-robot Pose SLAM

Distributed Kalman Filter for Cooperative Localization with Integrated Measurements.

Covariance Intersection-based Invariant Kalman Filtering(DInCIKF) for Distributed Pose Estimation

Distributed Kalman Consensus Filter for Estimation with Moving Targets

Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation

A Novel Distributed Adaptive Cubature Kalman Filter with Variational Bayesian Approach

Discrete‐time invariant extended Kalman filter on matrix Lie groups

Invariant extended Kalman filter on matrix Lie groups

Distributed Kalman Filters for Relative Formation Control of Mobile Agents

Inertial-Kinect Fusion for Robot Navigation based on the Extended Kalman Filter

Stereo MSCKF with Online Extrinsic Calibration using Invariant Extended Kalman Filter

Distributed Kalman Filter for Multitarget Tracking Systems with Coupled Measurements.

Distributed Extended Kalman-consensus Filtering Algorithm Based on Sensor Network for Nonlinear System

Invariant Extended Kalman Filtering for Tightly Coupled LiDAR-Inertial Odometry and Mapping