An Equivariant Filter-Based State Estimator for Tightly-Coupled LiDAR–Radar–Inertial Odometry

Abstract

Over the past decade, the fusion of light detection and ranging (LiDAR) and inertial navigation systems (INS) has become a reliable solution for environmental perception in intelligent mobile platforms. However, LiDAR performance degrades under adverse weather conditions. Millimeter-wave radar offers complementary benefits: it is robust to environmental disturbances and provides direct Doppler velocity measurements. Moreover, most existing fusion frameworks rely on the extended Kalman filter (EKF), which suffers from linearization errors and inconsistency. To address these limitations, we propose and derive a tightly-coupled LiDAR–radar–inertial odometry framework based on the equivariant filter (EqF). The framework leverages both LiDAR and radar to provide complementary constraints on the 9-dimensional navigation state, significantly improving accuracy and robustness in challenging environments. Furthermore, we conduct an in-depth observability analysis using Lie derivative theory, examining both the nonlinear system and its discrete filter system. We show that the EqF preserves the same unobservable directions as the underlying nonlinear system, thereby ensuring consistent state estimation, a property that standard EKF does not possess. In addition, experiments on real-world datasets demonstrate that our method outperforms the state-of-the-art LiDAR–inertial odometry approach and other EKF-based approaches in terms of localization accuracy, robustness and velocity estimation precision. Note to Practitioners—This work aims to provide a high-precision and robust fusion framework for LiDAR, radar, and INS in intelligent mobile platforms such as autonomous vehicles and drones. On such platforms, most existing EKF-based LiDAR-inertial odometry methods suffer from performance degradation under adverse weather conditions and from the inherent inconsistency of the EKF. To address these challenges, we propose a tightly-coupled LiDAR–radar–inertial odometry framework based on the EqF. The approach exploits the symmetry of the semi-direct product group to jointly model the navigation state and IMU biases within a geometrically consistent structure. We further conduct an in-depth observability analysis, demonstrating that the proposed framework preserves the correct unobservable directions of the underlying nonlinear system. As a result, the system exhibits superior robustness against initialization errors and external disturbances, along with improved accuracy compared to traditional approaches.