Uncertainty-Aware UWB/LiDAR/INS Tightly Coupled Fusion Pose Estimation via Filtering Approach

Precise and reliable pose estimation is a critical requirement for autonomous system. In recent years, light detection and range (LiDAR)-inertial odometry (LIO) has made significant advancements, especially in challenging environments with varying illumination and other complexities. However, LIO systems is known to local navigation, there can be a problem of error accumulation over time, particularly in global navigation satellite system (GNSS)-denied environments or in the absence of prior maps. Therefore, integrating the ultra-wideband (UWB) technique can effectively correct long-term state drift and enhance system performance, making it a promising solution for the demanding estimation task. In this contribution, we propose the tightly coupled method to fuse point cloud, inertial measurement, and UWB ranging information via iterative error state Kalman filtering (IESKF). With the UWB-aid initialization, the global-type and drift-free initial state can be obtained, which also facilitates the convergence of solution. We establish uncertainty-aware models and derive observation covariance for sensors, which play a crucial role in the heterogeneous multisensor fusion for pose estimation. Furthermore, extensive experiments in various scenarios are conducted using a customized platform. The results demonstrate that our approach can provide robust and consistent trajectory and mapping results while keeping computational costs low, even in the case when laser degeneration, non-line-of-sight (NLOS) ranging or limited UWB anchor stations.