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.

Equivariant Filter Design for Kinematic Systems on Lie Groups

Equivariant Filter Design for Discrete-time systems

Equivariant Filter (EqF)

Equivariant filter (EqF): A general filter design for systems on homogeneous spaces

Equivariant Filters are Equivariant

Equivariant Filtering Framework for Inertial-Integrated Navigation

Invariant extended Kalman filter on matrix Lie groups

Toward Consistent and Efficient Map-Based Visual-Inertial Localization: Theory Framework and Filter Design

An Equivariant Filter for Visual Inertial Odometry

EqVIO: An Equivariant Filter for Visual Inertial Odometry

Equivariant Filter Design for Inertial Navigation Systems with Input Measurement Biases

Discrete‐time invariant extended Kalman filter on matrix Lie groups

Equivariant Symmetries for Inertial Navigation Systems

Higher-Order Nonlinear Complementary Filtering on Lie Groups

Equivariant Filter for Tightly Coupled LiDAR-Inertial Odometry

MSCEqF: A Multi State Constraint Equivariant Filter for Vision-aided Inertial Navigation

Contact-Aided Invariant Extended Kalman Filtering for Robot State Estimation

Variational Bayesian Iteration-Based Invariant Kalman Filter for Attitude Estimation on Matrix Lie Groups

The Geometry of Navigation Problems

Equivariant Symmetries for Aided Inertial Navigation

Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group