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

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.

Figures (5)

• Figure 1: Hierarchical factor graph architecture of H-RINS. (Top) The persistent graph maintains the full trajectory history, fusing IMU preintegration, radar Doppler velocity, and keyframe-level GICP odometry factors across pose ($\mathbf{P}$), velocity ($\mathbf{V}$), and bias ($\mathbf{B}$) nodes; loop closure factors span distant keyframes to bound long-term drift. (Bottom) The resetting graph operates a sliding window of $N$ nodes and resets when the window is full: state estimates and their marginal covariances collapse into a single prior, discarding graph structure but preserving information. Two reset scenarios are shown: during a stationary period (ZUPT), zero-velocity and zero-motion factors tightly calibrate the bias in-place; during motion, the globally optimized bias and its covariance (blue arrow) are injected into the resetting prior, transferring long-horizon observability to the high-rate estimator.
• Figure 2: R3 Ring Sequence
• Figure 3: Underground garage sequence
• Figure 4: CDE office workspace, with glass walls
• Figure 5: Experimental platforms used in this work. It features an Imaging FMCW radar (Integrant HD-Radar), which outputs 4D point clouds at rate of 20Hz, and a commercial-grade Bosch BMI085 IMU (Built-in of RealSense D455 camera), while inside Intel NUC NUC7i7BNH as compute unit.