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.

该研究开发了利用 Lie 群和不变观测器设计技术的接触辅助不变扩展卡尔曼滤波器（InEKF），该滤波器结合了接触惯性动力学和正向运动学校正，可估计姿态和速度以及所有当前接触点。研究表明，误差动力学遵循对数线性自治微分方程，从而提高了收敛性能，并产生了符合非线性系统的本地可观测矩阵。该过程在 Cassie 系列双足机器人上进行了模拟和实验，并表明其比基于四元数的 EKF 具有更好的性能。

基于接触辅助的不变扩展卡尔曼滤波法用于机器人状态估计

Contact-Aided Invariant Extended Kalman Filtering for Robot State  Estimation

The main contribution of this paper is an invariant extended Kalman filter
(EKF) for visual inertial navigation systems (VINS). It is demonstrated that
the conventional EKF based VINS is not invariant under the stochastic
unobservable transformation, associated with translations and a rotation about
the gravitational direction. This can lead to inconsistent state estimates as
the estimator does not obey a fundamental property of the physical system. To
address this issue, we use a novel uncertainty representation to derive a Right
Invariant error extended Kalman filter (RIEKF-VINS) that preserves this
invariance property. RIEKF-VINS is then adapted to the multistate constraint
Kalman filter framework to obtain a consistent state estimator. Both Monte
Carlo simulations and real-world experiments are used to validate the proposed
method.

本文提出了一种基于不变扩展卡尔曼滤波器 (RIEKF-VINS) 的方法，并通过 Monte Carlo 模拟和实际实验验证了该方法在视觉惯性导航系统中的有效性。该方法使用一种新的不确定性表示方法，可以保留 VINS 中的不变性质，并将 RIEKF-VINS 适用于多状态约束卡尔曼滤波器框架，从而获得一致性的状态估计。