Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Equivariant Filter for Tightly Coupled LiDAR-Inertial Odometry

Anbo Tao, Yarong Luo, Chunxi Xia, Chi Guo, Xingxing Li

TL;DR

This work addresses the inconsistency of traditional EKF-based LiDAR–Inertial Odometry by introducing Eq-LIO, a tightly coupled estimator built on the Equivariant Filter (EqF) that leverages a semidirect-product symmetry to jointly estimate IMU biases, navigation state, and LiDAR extrinsics. By lifting the system to a Lie group and defining a right-invariant error, Eq-LIO achieves fixed-point linearization and improved robustness, further enhanced by incorporating gravity constraints on $\mathbb{S}^2$. The authors provide a rigorous mathematical framework (manifolds, Lie groups, and equivariant lifting) and concrete implementation details for the error dynamics and measurement model, including scan-to-map geometry. Empirical results on public and private datasets demonstrate higher accuracy and robustness than IEKF-based LIO and EKF-based FAST-LIO2, with a publicly available implementation to support further development and benchmarking.

Abstract

Pose estimation is a crucial problem in simultaneous localization and mapping (SLAM). However, developing a robust and consistent state estimator remains a significant challenge, as the traditional extended Kalman filter (EKF) struggles to handle the model nonlinearity, especially for inertial measurement unit (IMU) and light detection and ranging (LiDAR). To provide a consistent and efficient solution of pose estimation, we propose Eq-LIO, a robust state estimator for tightly coupled LIO systems based on an equivariant filter (EqF). Compared with the invariant Kalman filter based on the $\SE_2(3)$ group structure, the EqF uses the symmetry of the semi-direct product group to couple the system state including IMU bias, navigation state and LiDAR extrinsic calibration state, thereby suppressing linearization error and improving the behavior of the estimator in the event of unexpected state changes. The proposed Eq-LIO owns natural consistency and higher robustness, which is theoretically proven with mathematical derivation and experimentally verified through a series of tests on both public and private datasets.

Equivariant Filter for Tightly Coupled LiDAR-Inertial Odometry

TL;DR

This work addresses the inconsistency of traditional EKF-based LiDAR–Inertial Odometry by introducing Eq-LIO, a tightly coupled estimator built on the Equivariant Filter (EqF) that leverages a semidirect-product symmetry to jointly estimate IMU biases, navigation state, and LiDAR extrinsics. By lifting the system to a Lie group and defining a right-invariant error, Eq-LIO achieves fixed-point linearization and improved robustness, further enhanced by incorporating gravity constraints on . The authors provide a rigorous mathematical framework (manifolds, Lie groups, and equivariant lifting) and concrete implementation details for the error dynamics and measurement model, including scan-to-map geometry. Empirical results on public and private datasets demonstrate higher accuracy and robustness than IEKF-based LIO and EKF-based FAST-LIO2, with a publicly available implementation to support further development and benchmarking.

Abstract

Pose estimation is a crucial problem in simultaneous localization and mapping (SLAM). However, developing a robust and consistent state estimator remains a significant challenge, as the traditional extended Kalman filter (EKF) struggles to handle the model nonlinearity, especially for inertial measurement unit (IMU) and light detection and ranging (LiDAR). To provide a consistent and efficient solution of pose estimation, we propose Eq-LIO, a robust state estimator for tightly coupled LIO systems based on an equivariant filter (EqF). Compared with the invariant Kalman filter based on the group structure, the EqF uses the symmetry of the semi-direct product group to couple the system state including IMU bias, navigation state and LiDAR extrinsic calibration state, thereby suppressing linearization error and improving the behavior of the estimator in the event of unexpected state changes. The proposed Eq-LIO owns natural consistency and higher robustness, which is theoretically proven with mathematical derivation and experimentally verified through a series of tests on both public and private datasets.
Paper Structure (19 sections, 4 theorems, 30 equations, 8 figures, 3 tables)

This paper contains 19 sections, 4 theorems, 30 equations, 8 figures, 3 tables.

Table of Contents

  1. Introduction
  2. Related Works
  3. LiDAR-Inertial Odometry
  4. Kalman Filtering for Navigation Applications
  5. Mathematical Preliminaries and Notation
  6. Smooth Manifolds and Lie Theory
  7. Group Action, Useful Maps and Notation Explanation
  8. System Overview
  9. System Definition
  10. Equivariant Symmetry of the System
  11. Lifted System
  12. Equivariant Filter
  13. Error Dynamics
  14. Measurement Model
  15. Gravity Constraints on Manifolds S2
  16. ...and 4 more sections

Key Result

proposition 4.1

Define $\phi:G\times\mathcal{M}\to\mathcal{M}$ as where $\mathop{\mathrm{Ad}}\nolimits_{A^{-1}}:\mathfrak{se}_2(3)\to\mathfrak{se}_2(3)$ represents the isomorphism induced by conjugation $Y\mapsto A^{-1}YA$ on $\mathop{\mathrm{SE}}\nolimits_2(3)$. Then $\phi$ is a transitive and free right group action of $G$ on $\mathcal{M}$fornasier2022overcoming

Figures (8)

  • Figure 1: Overview of Eq-LIO. The equivariant filtering state estimator is employed in the odometry module, with the measurement model constructed through scan-to-map matching. The red box in the figure highlights the main content of this paper. For detailed information, please refer to Section \ref{['sec:system']} and \ref{['sec:eqf']}.
  • Figure 2: The measurement model.
  • Figure 3: Illustration of the error state on $\mathbb{S}^2$. The neighborhood at $\boldsymbol{g}_k\in\mathbb{S}^2$ is homeomorphic to $\mathbb{R}^2$. $\boldsymbol{x}$ is a minimal parameterization of the error between $\boldsymbol{g}_{k+1}$ with $\boldsymbol{g}_{k}$.
  • Figure 4: The estimated three-axis gyroscope bias sequence in the Schloss-1 dataset.
  • Figure 5: Time series plot of the estimated velocities of the three axes in the hkust_canpus_01 dataset.
  • ...and 3 more figures

Theorems & Definitions (4)

  • proposition 4.1
  • proposition 4.2
  • theorem 4.3
  • theorem 4.4