Reliability-Aware Sensor Fusion via Bidirectional Diffusion for Robust Robot Odometry

Technical Summary: Reliability-Aware Sensor Fusion via Bidirectional Diffusion for Robust Robot Odometry

Problem Statement
Autonomous robots operating in dynamic or degraded environments (e.g., forests, disaster zones, urban settings) rely on multi-modal sensor fusion (LiDAR, RGB-D cameras, IMUs) for state estimation. However, existing fusion frameworks suffer from three primary limitations:

1. Temporal Asynchrony: Mismatched sensor frequencies (e.g., IMU at 400 Hz vs. LiDAR at 10 Hz) introduce integration errors that grow over time, often addressed only reactively after residuals appear.
2. Static Noise Assumptions: Standard filters (EKF, IEKF) assume fixed Gaussian noise, failing to account for non-Gaussian, time-varying noise caused by motion blur, occlusions, or dynamic illumination. They do not explicitly predict short-horizon sensor reliability to adapt covariances before state updates.
3. Spatial Information Loss: Many systems project 3-D data to 2-D grids, discarding critical height and surface normal information needed for complex terrain traversal.

The core challenge addressed is converting predicted short-horizon sensor reliability into adaptive filter covariances before corrupted measurements induce overconfident state corrections.

Methodology: PUD–DIKF
The paper proposes PUD–DIKF (Prediction-Uncertainty-Driven Diffusion-Informed Kalman Filter), a unified estimation framework where a central Kalman filter is driven by a learned reliability model. The architecture is hierarchical, with supporting front-end modules feeding a core estimator.

1. Unified Multi-Modal Tensor (PUT): The core interface is the Prediction Uncertainty Tensor ($\Psi_t$), a 3-D voxel grid integrating heterogeneous sensor streams (RGB, depth, normals, LiDAR points/intensity, IMU data). It encodes three uncertainty components:

   • Aleatoric: Irreducible sensor noise.
   • Epistemic: Model ignorance due to limited training data.
   • Predictive ($U_{pred}$): Short-horizon reliability forecasts.
     These are bounded and combined into an effective uncertainty ($U_{eff}$) that modulates the filter's covariances.

2. Bidirectional Diffusion Synchronization (BDS):

   • Retrospective Diffusion Estimator (RDE): Denoises current sensor observations to reduce measurement noise before the Kalman update.
   • Proactive Diffusion Predictor (PDP): Forecasts future sensor reliability ($U_{pred}$) by learning patterns in historical uncertainty trajectories (e.g., increasing variance prior to occlusion).
   • The PDP output adaptively modulates the diffusion noise schedule, allowing the system to model residual temporal asynchrony as structured noise.

3. Diffusion-Augmented Calibration (DAMC): Performs online extrinsic calibration between LiDAR and cameras using a Cayley parameterization for rotations. It jointly optimizes geometric alignment and diffusion regularization, using the predictive uncertainty to adapt the optimization step size.

4. SE(3)-Equivariant Feature Extraction: Extracts robust features using SE(3)-equivariant convolutions extended with a diffusion-based uncertainty channel. This preserves geometric consistency under rigid-body transformations and communicates per-region reliability to the filter.

5. Core PUD–DIKF Estimator:

   • Builds on the FAST-LIO2 error-state structure.
   • Adaptive Covariances: The process noise ($Q_k$) and measurement noise ($R_k$) are augmented by positive-semidefinite, state-dependent increments derived from the PUT.
   • Monotonicity Guarantee: The paper proves (Proposition 1) that increasing predicted uncertainty cannot increase measurement information or reduce the propagated covariance below the fixed-noise baseline. This ensures that degraded reliability leads to conservative (less confident) updates, preventing overcorrection.

6. Downstream Mapping (POF): A Predictive Occupancy Field consumes the estimated state and uncertainty for dense mapping but does not feed pose corrections back to the estimator, maintaining a strict unidirectional flow.

Key Contributions

1. Core Estimator: A Diffusion-Informed Kalman Filter that maps retrospective and predictive uncertainty into state-dependent, positive-semidefinite covariance increments on an SO(3) error-state filter.
2. Theoretical Linkage: A monotonicity condition proving that higher predicted uncertainty strictly limits measurement information and increases propagated covariance, preventing the filter from becoming overconfident during degradation.
3. Controlled Validation: Extensive experiments on a custom multi-modal campus dataset and public benchmarks (HILTI–Oxford, KITTI), including ablation studies that isolate the contribution of each component.

Experimental Results

• Campus Dataset (438.58 m evaluation window): PUD–DIKF achieved a trajectory RMSE of 0.034 m and an RPE of 0.008 m with a median point-cloud alignment error of 0.008 m. The system operates at 42 ms per LiDAR frame (10 Hz).
  • Compared to FAST-LIVO2, RMSE decreased by 19% (0.042 m $\to$ 0.034 m).
  • Compared to R3LIVE, RMSE decreased by 35% (0.052 m $\to$ 0.034 m).
• Public Benchmarks:
  • HILTI–Oxford: Achieved the lowest ATE RMSE (mean 0.0155 m) across three construction sequences, outperforming baselines in geometrically complex and partially occluded environments.
  • KITTI: Reported the lowest translational drift ($t_{rel}$) on four sequences (mean 0.0183%), though the authors note these are preliminary internal evaluations awaiting official server verification.
• Ablation Studies:
  • Removing the Retrospective Denoising (RDE) increased RMSE by 32%, identifying it as the most critical component for this dataset.
  • Removing the Proactive Prediction (PDP) increased RMSE by 21%, validating the proactive covariance adaptation hypothesis.
  • Replacing SE(3)-equivariant features with a standard MLP increased RMSE by 15%.
  • Removing the camera modality increased RMSE by only 3%, indicating the system is primarily geometry-inertial with the camera providing modest, dataset-dependent correction.
• Dense Mapping: Integrating PUT uncertainty into the mapping module reduced occupancy RMSE by 53% compared to a TSDF baseline and improved the F1 score from 0.73 to 0.89.

Significance and Claims
The paper claims that PUD–DIKF successfully addresses the limitation of reactive fusion by modeling reliability changes before the state update. The central hypothesis is validated: converting predicted uncertainty into adaptive covariances prevents degraded measurements from dominating the state update. The authors emphasize that the system's advantages are most evident in structurally complex and partially occluded environments where sensor degradation is frequent.

The paper maintains modesty regarding its claims:

• It explicitly states that the KITTI results are "preliminary benchmark context" rather than statistically verified superiority, as they were not produced via paired reruns on identical hardware or verified by the official server.
• It acknowledges that the visual input (camera) is supportive rather than dominant in the current dataset.
• It notes limitations, including non-monotonic parameter variation in DAMC under high noise, the assumption that at least one spatial sensor remains informative, and the need for evaluation on other robotic platforms (e.g., legged or aerial).

The work positions itself not as a collection of independent modules, but as a unified hypothesis where supporting elements (BDS, DAMC, SE(3) features) serve the core mechanism of covariance adaptation driven by the Prediction Uncertainty Tensor.