Accurate and Rapidly-Convergent GNSS/INS/LiDAR Tightly-Coupled Integration via Invariant EKF Based on Two-Frame Group

Abstract

Nowadays, increasing attention has been directed toward the integration of global navigation satellite system (GNSS), inertial navigation satellite system (INS), and light detection and ranging (LiDAR) for intelligent system navigation. However, the existing systems, which generally adopt estimators of the extended Kalman filter (EKF) or factor graph optimization (FGO), still face challenges regarding consistency and convergence. Such methods could provide optimal navigation solutions only if the initial guess of the state is sufficiently close to the true trajectory; otherwise, the systems might undergo accuracy loss or even worse, divergence. To address this issue, we derive an invariant extended Kalman filter (IEKF) based on the two-frame group (TFG) in the left-invariant form, and integrate raw GNSS double-differenced observations, inertial measurements, and LiDAR plane features within this framework. By designing a unified group structure that simultaneously maintains both the navigation states and inertial measurement unit (IMU) biases, TFG contributes to the approximate log-linearity and invariance of the system dynamics model, expected to effectively resolve the convergence issue. A set of real-world experiments was conducted to evaluate the system, with results indicating its potential to achieve submeter to centimeter-level positioning accuracy, surpassing state-of-the-art methods in terms of accuracy, availability, and convergence. Note to Practitioners—This work aims to extend the usability and applicability of GNSS/INS/LiDAR integrated systems on highly maneuverable platforms, such as automobiles and drones. In such cases, the existing approaches based on EKF or FGO face the challenges of degradation or divergence, since their optimality is not guaranteed with erroneous initial state guesses due to severe linearization errors. To tackle these issues induced by model nonlinearities, we propose a tightly-coupled GNSS/INS/LiDAR integration based on the derived TFG-LIEKF, which effectively mitigates the linearization errors by making the dynamics model approximately log-linear via a unique group structure that considers both navigation states and IMU biases. On this basis, the proposed system ensures continuity through multi-sensor fusion, improves accuracy through the tightly-coupled scheme and continuous tracking of LiDAR features, and promotes convergence through the estimator based on TFG.