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

## TL;DR

A new radar navigation system combines ICP and Doppler data to improve accuracy and reliability in challenging environments.

## Contribution

A novel radar-inertial odometry framework that integrates ICP and Doppler velocity using stochastic cloning for better localization.

## Key findings

- Integrating ICP and Doppler velocity improves localization accuracy and robustness compared to standalone methods.
- Stochastic cloning enhances estimation stability in sparse and noisy radar environments.
- The modular framework supports future ICP algorithms for real-time navigation.

## Abstract

What are the main findings?
An ICP-integrated radar-inertial odometry framework is proposed for 4D imaging radar, integrating relative pose from radar ICP with Doppler-based ego-velocity in a consistent EKF framework.Experiments on various datasets demonstrate that integrating ICP and Doppler velocity achieves higher localization accuracy and robustness than ICP-only or velocity-only approaches.

An ICP-integrated radar-inertial odometry framework is proposed for 4D imaging radar, integrating relative pose from radar ICP with Doppler-based ego-velocity in a consistent EKF framework.

Experiments on various datasets demonstrate that integrating ICP and Doppler velocity achieves higher localization accuracy and robustness than ICP-only or velocity-only approaches.

What are the implications of the main findings?
Integrating point cloud-based spatial constraints with Doppler velocity significantly improves estimation stability under sparse, noisy, and dynamically changing radar environments.The modular ICP-integrated observation model enables scalable and flexible adoption of future ICP algorithms, supporting reliable real-time navigation using 4D imaging radar.

Integrating point cloud-based spatial constraints with Doppler velocity significantly improves estimation stability under sparse, noisy, and dynamically changing radar environments.

The modular ICP-integrated observation model enables scalable and flexible adoption of future ICP algorithms, supporting reliable real-time navigation using 4D imaging radar.

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.