Invariant-DLIO: Direct LiDAR–Inertial Odometry Based on Invariant Kalman Filtering

摘要

The topic of LiDAR-Inertial Odometry (LIO) is raising the interest of researchers as one of the key areas for robotics navigation, among which Extended Kalman Filtering (EKF) based LIO has become the mainstream of LIO because of its excellent computational speed and good accuracy. However, the EKF-based methods cannot avoid the inconsistency from estimation error linearization. As the complement, Invariant Extended Kalman Filtering (InEKF) designed for state trajectories lying on the matrix Lie groups has been proposed and proved to be excellent in convergence and consistency. In this paper, we propose the method of Direct LiDAR-Inertial Odometry Based on Invariant Kalman Filtering (Invariant-DLIO), which contains the InEKF-based state estimator with the fusion of IMU measurements and LiDAR point clouds, where the error dynamics meets the properties of log-linear and trajectory-independent. A lightweight Scan-to-Mapping module is also designed for the refinement of pose estimation, where the mapping is updated and operated with <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">O</i>(1) time complexity. Extensive experiments, including different public datasets and Magni robot data acquisition, are conducted in comparison with a series of state-of-the-art LIO/LO methods. Experimental results show that Invariant-DLIO achieves superior accuracy and efficiency.