Multi-Sensor Fusion for Quadruped Robot State Estimation using Invariant Filtering and Smoothing

TL;DR

This work addresses proprioceptive drift in quadruped state estimation by embedding LiDAR odometry and GPS into invariant estimators built on Lie-group theory. It develops group-affine observation models and a parallel LiDAR odometry pipeline to fuse kinematics, IMU, LiDAR, and GPS within the InEKF and invariant smoother frameworks, yielding E-InEKF and E-IS. Experiments on KAIST Hound2 in indoor and outdoor settings show significant reductions in z-axis drift and improvements in ATE and RPE compared to proprioceptive and LiDAR-only baselines, demonstrating robustness in challenging terrains. The results highlight the trade-offs between filtering and smoothing, with E-InEKF offering real-time performance and E-IS delivering higher accuracy through temporal batching, enabling accurate, drift-resistant quadruped localization and mapping.

Abstract

This letter introduces two multi-sensor state estimation frameworks for quadruped robots, built on the Invariant Extended Kalman Filter (InEKF) and Invariant Smoother (IS). The proposed methods, named E-InEKF and E-IS, fuse kinematics, IMU, LiDAR, and GPS data to mitigate position drift, particularly along the z-axis, a common issue in proprioceptive-based approaches. We derived observation models that satisfy group-affine properties to integrate LiDAR odometry and GPS into InEKF and IS. LiDAR odometry is incorporated using Iterative Closest Point (ICP) registration on a parallel thread, preserving the computational efficiency of proprioceptive-based state estimation. We evaluate E-InEKF and E-IS with and without exteroceptive sensors, benchmarking them against LiDAR-based odometry methods in indoor and outdoor experiments using the KAIST HOUND2 robot. Our methods achieve lower Relative Position Errors (RPE) and significantly reduce Absolute Trajectory Error (ATE), with improvements of up to 28% indoors and 40% outdoors compared to LIO-SAM and FAST-LIO2. Additionally, we compare E-InEKF and E-IS in terms of computational efficiency and accuracy.

Figures (6)

• Figure 1: The structure of the two proposed frameworks. The E-InEKF and E-IS have the propagation and observation modules, but the E-IS also includes the prior and marginalization modules.
• Figure 2: The indoor experiment involved large impacts and significant changes in the robot’s height, as illustrated in the figures.
• Figure 3: The results for the indoor experiment, where the robot is prone to experience high contact impacts, show that the proposed frameworks, E-IS and E-InEKF, mitigate the position drift, especially in the z-axis, compared to the proprioceptive-only methods, P-IS and P-InEKF. In the purple dotted box, the position in the z-axis of P-IS and P-InEKF is indicated by the green and yellow arrows. Vicon is used as ground truth. In the right plot, the E-IS and E-InEKF trajectories use a color gradient: the lighter sections indicate the start of the estimates, while the darker sections indicate the end.
• Figure 4: The illustration of the outdoor experiment: Google Earth view with screenshots of the robot walking along the outdoor path, on the top right corner.
• Figure 5: The results for the outdoor experiment clearly show the improvement in the z-axis drift when using LiDAR and GPS, even in long-distance operations. The drift is indicated by the green and yellow arrows, for the P-InEKF and P-IS, respectively.