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

2026-03-14

AI Summary

This work addresses the challenge of significant drift in radar-inertial state estimation, which arises from the radar’s provision of only sparse ego-velocity measurements, leading to weak absolute orientation constraints and short-term unobservability of IMU biases. To mitigate this, the authors propose a hierarchical tightly coupled factor graph framework that decouples the system into a high-frequency reset graph and a persistent global graph. The reset graph fuses IMU preintegration, radar velocity, and adaptive zero-velocity updates (ZUPT) to deliver low-latency odometry, while the global graph maintains long-term consistency through keyframe-based geometric mapping and loop closure, leveraging incremental smoothing and mapping (iSAM) to avoid explicit marginalization. A key innovation lies in the bidirectional graph collaboration: precisely optimized IMU biases and their covariances from the global graph are continuously injected into the reset graph as probabilistic priors, substantially enhancing bias observability and suppressing integration drift. The system achieves high-precision, low-drift radar-inertial navigation at 27× real-time speed.

Technology Category

• Intelligent Robots: State Estimation
• Planning, Routing, and Scheduling: Optimization of Spatio-temporal Systems
• Reasoning under Uncertainty: Graphical Models

Application Category

• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs
• Search and Retrieval-Augmented AI: Web learning to rank, online learning, and counterfactual learning for ranking
• Semantics and Knowledge: Methods to enhance, augment, integrate or synergize semantic models such as knowledge graphs and LLMs

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.

Problem

• radar-inertial navigation
• drift
• observability
• IMU bias
• state estimation

Innovation

• tightly-coupled radar-inertial navigation
• hierarchical factor graph
• bias observability
• incremental smoothing and mapping
• zero-velocity update