Contact-aided invariant extended Kalman filtering for robot state estimation

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 contactaided InEKF provides better performance in comparison with the quaternion-based EKF as a result of exploiting symmetries present in system. changes in lighting as well as the operating environment. It is therefore beneficial to develop a low-level state estimator that fuses data only from proprioceptive sensors to form accurate high-frequency state estimates. This approach was taken by Bloesch et al. [15] when developing a quaternion-based extended Kalman filter (QEKF) that combines inertial, contact, and kinematic data to estimate the robot's base pose, velocity, and a number of contact states. In this article, we expand upon these ideas to develop an invariant extended Kalman filter (InEKF) that has improved convergence and consistency properties allowing for a more robust observer that is suitable for long-term autonomy.The theory of invariant observer design is based on the estimation error being invariant under the action of a matrix Lie group [1,20], which has recently led to the development of the InEKF 1 [18,8,10,11] with successful applications and promising results in simultaneous localization and mapping [8,86] and aided inertial navigation systems [4,5,8,83]. The invariance of the estimation error with respect to a Lie group action is referred to as a symmetry of the system [5]. Summarized briefly, Barrau and Bonnabel [10] showed that if the state is defined on a Lie group, and the dynamics satisfy a particular "group affin...

Bayesian Spatial Kernel Smoothing for Scalable Dense Semantic Mapping

“…6. To further test the mapping performance of our methods on sparse data, we down sample the point clouds per scan to a resolution of 0.2 m, and build a semantic occupancy map with a resolution of 0.1 m. The mapping drift after one full round of the Wave Field is because of the odometry system used in the experiment [47], [48]. The details of both maps are given in Fig.…”

A Mathematical Framework for IMU Error Propagation with Applications to Preintegration

“…This is the key property opening the door to invariant filtering, autonomous error variables, log-linearity and EKF stability leveraged in e.g. [18,6]. The next section summarizes the links between the latter formulation of inertial navigation and the theory of preintegration of [27,17].…”

“…We are now in a position to derive a preliminary yet remarkable result regarding propagation through noise free IMU equations of a concentrated Gaussian (18).…”

“…Remark 3 [2] considers propagation by compounding and proves noise free propagation T k+1 = Γ k T k Υ k , in place of the more sophisticated dynamics (19), preserves statistical distributions (18), see Eq. (26) in Reference [2].…”

“…In [4,5] the introduction of the group SE 2 (3), along with the discovery of the associated group affine property and hence log-linearity of IMU equations using SE 2 (3), proves a major step to overcome these obstacles. It has already led to more robust EKFs for data fusion with IMU, has prompted an industrial product [6], has improved EKF-based visual inertial consistency [34,20,9,10,21] and robot state estimation [19,18]. In this paper, we show the group SE 2 (3) allows transposing the recent results about estimation of poses using wheel speeds of [2,32] to the context of IMUs.…”