Geometrically-Constrained Radar-Inertial Odometry via Continuous Point-Pose Uncertainty Modeling

Abstract: Radar odometry is crucial for robust localization in challenging environments; however, the sparsity of reliable returns and distinctive noise characteristics impede its performance. This paper introduces geometrically-constrained radar-inertial odometry and mapping that jointly consolidates point and pose uncertainty. We employ the continuous trajectory model to estimate the pose uncertainty at any arbitrary timestamp by propagating uncertainties of the control points. These pose uncertainties are continuously integrated with heteroscedastic measurement uncertainty during point projection, thereby enabling dynamic evaluation of observation confidence and adaptive down-weighting of uninformative radar points. By leveraging quantified uncertainties in radar mapping, we construct a high-fidelity map that improves odometry accuracy under imprecise radar measurements. Moreover, we reveal the effectiveness of explicit geometrical constraints in radar-inertial odometry when incorporated with the proposed uncertainty-aware mapping framework. Extensive experiments on diverse real-world datasets demonstrate the superiority of our method, yielding substantial performance improvements in both accuracy and efficiency compared to existing baselines.

• The paper introduces a continuous-time framework that integrates radar and inertial data by jointly modeling point and pose uncertainty for robust registration.
• It employs an iterated Extended Kalman Filter with cubic B-spline trajectory representation to propagate uncertainty and adaptively enforce geometric constraints.
• Experimental results demonstrate significant reductions in ATE and RPE, achieving LiDAR-level accuracy even in challenging, dynamic environments.

Geometrically-Constrained Radar-Inertial Odometry via Continuous Point-Pose Uncertainty Modeling

Introduction

Radar odometry has emerged as a highly robust solution for localization in environments where traditional modalities such as LiDAR or vision are unreliable, particularly under adverse weather or low-visibility scenarios. However, automotive radar-based odometry presents unique challenges: specifically, the sparsity of radar returns and their pronounced, non-Gaussian noise characteristics hinder the reliability of conventional scan matching and pose estimation approaches. The paper "Geometrically-Constrained Radar-Inertial Odometry via Continuous Point-Pose Uncertainty Modeling" (2604.02745) introduces a framework that robustly consolidates heteroscedastic measurement and temporally-continuous pose uncertainty, thereby enabling the integration of explicit geometric constraints for high-fidelity radar-inertial odometry and mapping.

Figure 1: We jointly model and continuously consolidate point-pose uncertainty in radar odometry and mapping, enabling the effective usage of explicit local geometry.

Methodological Framework

System Pipeline Overview

The proposed method integrates preprocessing, continuous-time state estimation, and adaptive map updating. In preprocessing, dynamic objects are filtered via ego-motion estimates, exploiting Doppler and inertial cues to suppress spurious returns. Measurements with excessive dynamics or ill-conditioned associations are omitted to stabilize scan registration. The subsequent state estimation phase employs an iterated Extended Kalman Filter (IEKF) operating on a continuous cubic B-spline trajectory parameterization. Here, residuals from scan registration, Doppler, and IMU are iteratively incorporated, and uncertainty propagation is executed at each update step. After convergence, candidate points with low propagated uncertainty are selectively inserted or used to refine the local map.

Figure 2: System consists of preprocessing, state estimation, and mapping, with iterative uncertainty propagation and localizability-constrained updates guiding the robust selection of mapping candidates.

Continuous-Time Trajectory and Uncertainty Propagation

The radar-inertial trajectory is modeled using cubic B-splines, allowing for precise state representation and asynchronous measurement fusion. Both translation and orientation control points are explicitly maintained, and their covariance is propagated across the trajectory, enabling uncertainty-aware transformations at arbitrary timestamps.

Unlike classical discrete methods, this continuous-time formulation couples the measurement uncertainty of each radar return with the propagated pose covariance, producing per-point uncertainties in world coordinates. These serve as statistical confidence scores for downstream selection and observation weighting.

Figure 3: Control point uncertainties are propagated to radar trajectory poses and then to individual radar returns, yielding per-point confidence for local geometry-informed registration.

The framework leverages the full joint uncertainty—derived by propagating both state covariance and measurement noise—during scan-to-submap registration and map maintenance. This enables dynamic down-weighting of unreliable points and adaptive replacement in the local map structure, ultimately mitigating artifacts induced by radar’s sensing noise or state drift.

Localizability-Constrained State Update

To prevent degeneracy in ill-conditioned geometries (e.g., bridge or tunnel scenarios), a localizability Hessian is computed on the local map. The state update step is projected onto the well-constrained subspace, i.e., the nullspace of the constraint matrix formed by the axes suffering geometric ambiguity. This regularization ensures stable estimation even under repetitive or degenerate features, as evidenced by marked ATE/RPE improvements under such conditions.

Adaptive Registration Using Explicit Local Geometry

The mapping pipeline exploits planar features by adaptively fitting local planes to reliable radar returns in the map. If planarity confidence (derived from propagated point uncertainties) is insufficient, the system gracefully switches to a point-to-distribution residual model (weighted by radar cross-section information). This dual-residual mechanism, guided by environment-dependent weighting, retains robustness in both structured and unstructured environments without excessive performance degradation due to unreliable planarity assumptions.

Robust Mapping via Uncertainty-Aware Replacement

Candidate points are inserted into an incremental kd-tree map only if their propagated uncertainties are lower than those of existing points within a fixed radius. This online pruning-replacement scheme ensures high map fidelity and suppresses accumulation of outliers, as demonstrated across diverse environmental and sensor conditions.

Figure 4: (a) Raw mapping in Library, color-coded by uncertainty, exposes noise artifacts. (b) Without pose uncertainty, spurious measurements persist. (c) The full uncertainty module effectively retains only reliable points.

Experimental Evaluation

Quantitative evaluations were conducted on the HeRCULES, SNAIL-Radar, and HKUST datasets, spanning large-scale outdoor, challenging urban, and indoor environments with state-of-the-art radar-inertial odometry baselines (Go-RIO, PUA-RIO, EKFRIO-TC) and LiDAR-inertial methods.

Strong numerical results include substantial performance gains in all mean error metrics (ATE, translational and rotational RPE), often achieving or surpassing the localization accuracy of LiDAR-inertial odometry even in highly dynamic and sparse measurement regimes. For example, in the Parking Lot and Street scenarios, the proposed approach achieves ATE below 1.1 m and 2.6 m respectively, improving over Fast-LIO2 and radar-inertial baselines by significant margins.

Figure 5: Estimated trajectories for Street. Proposed method (blue) exhibits superior trajectory alignment and outperforms LiDAR-inertial odometry (red) against ground truth (black).

Figure 6: Comparison on Parking Lot between proposed and Fast-LIO2 (LiDAR), illustrating LiDAR-level odometry and map consistency through robust radar-based estimation.

Ablation studies directly contradict the prior assumption that measurement uncertainty alone suffices for robust weighting: neglecting pose uncertainty in propagation reduces odometry and mapping accuracy to the level of uncertainty-ignorant variants, affirming the necessity of the proposed holistic approach. Furthermore, the restoration of explicit geometric (planarity) constraints via uncertainty-driven selection critically enhances performance—a feature previously considered unviable for radar due to low return density and high noise.

The influence of hyperparameter selection (uncertainty threshold in mapping) was minimal, indicating the robustness of the approach to heuristic sensitivity.

Computational Analysis

The method operates well within real-time domains (>20 Hz), even in long trajectories with high point densities. This efficiency preserves headroom for multimodal extensions or denser radar deployment.

Figure 7: Runtime analysis for SNAIL-Radar 20240115_2 demonstrates real-time performance with ample computational margin.

Implications and Future Prospects

This research empirically establishes that propagating full point-pose uncertainty in continuous-time radar odometry directly enables explicit geometric constraints—traditionally reserved for higher-precision sensors such as LiDAR—to be leveraged even in sparse and noisy radar data regimes. Practically, this approach closes the performance gap between radar and LiDAR-inertial odometry, positioning radar as a viable primary sensor for robust, all-weather autonomous localization and mapping.

Theoretically, the results suggest that further refinement of continuous uncertainty modeling, submap refinement mechanisms, and multi-sensor fusion strategies (e.g., multi-radar or radar-LiDAR) could further generalize this paradigm to a wider range of operational conditions and map representations. Integration of higher-order local geometric features or learning-based inlier selection mechanisms under this uncertainty framework is a promising avenue for future research.

Conclusion

The paper delivers a rigorous, uncertainty-aware radar-inertial odometry and mapping framework that jointly consolidates and propagates pose and measurement uncertainty in a continuous-time setting. Through this joint formulation, the method enables adaptive confidence-weighted registration, explicit geometric constraints, and high-fidelity radar mapping, demonstrably outperforming state-of-the-art baselines across multiple benchmarks. Its empirical findings fundamentally challenge the notion that radar’s geometric sparseness prohibits the use of geometric constraints, advocating for uncertainty-centric methods in the ongoing advancement of robust sensor fusion and SLAM architectures.

(2604.02745)