Uncertainty Modeling for Plane and Line Features to Improve Consistency in RTK/INS/LiDAR Integrated Navigation

The demand for high-precision navigation and positioning is growing rapidly with the advancements in autonomous driving and intelligent robotics. Nowadays, many studies have been conducted on the fusion of the global navigation satellite system (GNSS), inertial navigation systems (INS), and light detection and ranging (LiDAR) because of their complementary characteristics. Notably, in multi-source fusion, it is crucial to precisely model the uncertainty (covariance) of each sensor. While GNSS and IMU covariance modeling are mature, LiDAR covariance modeling remains unsophisticated, resulting in a suboptimal fusion of GNSS/INS/LiDAR. In this work, the LiDAR covariance modeling method is proposed, including the original point cloud covariance, the localmap covariance, and the observation covariance modeling. A point cloud selecting approach based on the eigenvectors and centroids is also introduced to maintain valid surface and edge feature points, thereby reducing repeated observations. These above methods are applied to the tightly coupled RTK/INS/LiDAR_PPL (G+I+PPL) system based on the Multi-State Constraint Kalman Filter (MSCKF) to further improve positioning accuracy and covariance consistency. The experimental results show that the proposed method has better covariance consistency in comparison to LIOSAM and FASTLIO. Meanwhile, G+I+PPL with fused point-to-plane/line (PPL) observations outperforms compared to RTK/INS (G+I) and RTK/INS/LiDAR_CP (G+I+CP) using closest point (CP) observations, in the right, front, and up (RFU) directions, demonstrating a positioning improvement in performance by (50.7%, 58.6%, 54.3%) and (46.2%, 55.0%, 58.8%), respectively.