Implementing Loosely Coupled UKF Fusion of ICP and Odometry for 2D SLAM

Accurate and robust SLAM is vital for autonomous mobile robots in unknown environments. However, low-cost LiDAR-based systems often fail in rapid motion or feature-poor areas due to sparse data or low update rates. We propose a loosely coupled 2D SLAM framework that fuses ICP-based point-cloud registration with odometry using a UKF to improve pose estimation and mapping. The UKF merges LiDAR and odometry estimates, handling motion nonlinearity. UKF estimation pose provides initial guesses for ICP, which is refined via Levenberg–Marquardt minimization. A distance-based outlier rejection filters inconsistent point pairs, preventing erroneous updates in low-feature scenes. A set-difference merge incorporates new scans into the global map, reducing obstacle inflation. Experiments show significant improvements in pose accuracy and map quality with efficient computation. The loosely coupled design ensures modularity and flexibility for future sensor integration.