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, 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 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 multi-sensor 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.