Design and Performance Validation of 4D Radar ICP-Integrated Navigation with Stochastic Cloning Augmentation

Automotive radar has emerged as a pivotal technology for navigation in GNSS-denied environments, offering superior robustness to adverse weather and fluctuating lighting conditions compared to vision or LiDAR-based sensors. Despite these advantages, the inherent sparsity and noise of radar measurements often lead to degraded estimation accuracy and system reliability. To address these challenges, various radar-based localization frameworks have been explored, ranging from optimization-based and Extended Kalman Filter (EKF) approaches fused with Inertial Measurement Units (IMUs) to point cloud registration techniques like Iterative Closest Point (ICP). While filter-based methods are favored in multi-sensor fusion for their proven stability, ICP is widely utilized for high-precision pose estimation in point-cloud-centric systems. In this study, we propose a novel Radar-Inertial Odometry (RIO) framework that synergistically integrates ICP-based relative pose estimation with model-based sensor fusion. The proposed methodology leverages relative transformations derived from ICP alongside ego-velocity estimations obtained from radar Doppler measurements. To effectively incorporate relative ICP constraints, a stochastic cloning technique is implemented to augment previous states and their associated covariances, ensuring that the uncertainty of historical poses is explicitly accounted for. The performance of the proposed method is validated using public open-source datasets, demonstrating higher localization accuracy and more consistent performance compared to existing algorithms used for comparison.