H-RINS: Hierarchical Tightly-coupled Radar-Inertial Navigation via Smoothing and Mapping

TL;DR

This paper introduces H-RINS, a hierarchical radar-inertial navigation system that combines a high-rate resetting graph with a persistent global graph to improve long-term accuracy and reduce drift in radar-inertial SLAM.

Contribution

The paper presents a novel hierarchical factor graph framework that decouples estimation into high-rate and global components, enhancing bias observability and drift correction in radar-inertial navigation.

Findings

Achieves high-accuracy, drift-reduced navigation at 27x real-time speed.

Effectively fuses radar velocities, IMU data, and loop closures.

Maintains long-term bias observability through incremental smoothing.

Abstract

Millimeter-wave radar provides robust perception in visually degraded environments. However, radar-inertial state estimation is inherently susceptible to drift. Because radar yields only sparse, body-frame velocity measurements, it provides weak constraints on absolute orientation. Consequently, IMU biases remain poorly observable over the short time horizons typical of sliding-window filters. To address this fundamental observability challenge, we propose a tightly coupled, hierarchical radar-inertial factor graph framework. Our architecture decouples the estimation problem into a high-rate resetting graph and a persistent global graph. The resetting graph fuses IMU preintegration, radar velocities, and adaptive Zero-Velocity Updates (ZUPT) to generate the smooth, low-latency odometry required for real-time control. Concurrently, the persistent graph is a full-state factor graph maintaining the complete information of poses, velocities, and biases by fusing inertial data with keyframe-based geometric mapping and loop closures. Leveraging Incremental Smoothing and Mapping, the persistent graph can operate without explicit marginalization of variables, preserving their information while ensuring long-term bias observability. The cornerstone of our approach is a probabilistic tight-coupling mechanism: fully observable, optimized biases and their exact covariances are continuously injected from the persistent graph into the resetting graph's prior, effectively anchoring the high-rate estimator against integration drift. Extensive evaluations demonstrate our system achieves high accuracy with drift-reduced estimation at 27x real-time execution speeds. We release the implementation code and datasets upon the acceptance of the paper.