Route-Constrained Robust Fusion Estimation for MEMS/GNSS Integrated Navigation of Unmanned Ground Vehicles in GNSS Degraded Environments

Abstract: To address cumulative localization drift of unmanned ground vehicles in structured road environments under severe Global Navigation Satellite System signal occlusion, this paper proposes a robust route-constrained state estimation method. During periods without satellite signals, the proposed method establishes the correspondence between the historical dead reckoning trajectory and local segments of the mission route extracted from a high-definition map, and estimates a route-referenced position via a two-dimensional rigid transformation. The estimated position is then formulated as a pseudo-position observation and incorporated into an Extended Kalman Filter update. In this way, route constraints at the road level can be continuously injected into a unified state estimation framework, thereby suppressing position deviation relative to the mission route while indirectly improving azimuth estimation. To enhance practical applicability, engineering strategies, such as trigger control, matching quality validation, route offset compensation, and single update correction limiting, are further introduced. Experiments in three representative scenarios, including a long tunnel, a multi-segment tunnel, and a curved tunnel, show that the proposed method effectively suppresses error accumulation during satellite outages, reduces the risk of large maximum deviation, and improves localization continuity and road-level usability.

• The paper demonstrates a route-constrained pseudo-position observation framework integrated into an error-state EKF to reduce dead-reckoning drift.
• It uses 2D rigid registration matching of dead-reckoning trajectories with HD map segments, yielding substantial reductions in position RMSE (e.g., from 186.821 m to 1.672 m in long tunnels).
• The approach offers a cost-effective, sensor-minimal solution that enhances UGV navigation robustness in GNSS-degraded environments through effective drift compensation.

Route-Constrained Robust Fusion Estimation for MEMS/GNSS Integrated Navigation of UGVs in GNSS-Degraded Environments

Problem Statement and Context

Robust and accurate localization remains a primary challenge for unmanned ground vehicles (UGVs) in GNSS-denied environments, such as tunnels, underpasses, or dense urban canyons. Standard GNSS/IMU/ODO-based solutions suffer from accumulated drift during GNSS outages due to the inherent limitations of dead reckoning, particularly in structured road environments. Existing mitigation strategies either depend on supplementary sensors (LiDAR, UWB, or vision) for redundancy or leverage a-priori environmental information, such as environmental geometry or HD maps, to constrain estimation.

This paper introduces a robust route-constrained state estimation strategy, exploiting only the standard sensor suite (GNSS, MEMS IMU, wheel odometry) and SD map data, thereby avoiding the complexity and cost of multisensor fusion.

Methodology: Pseudo-Position Observation via Trajectory-Route Matching

The core innovation consists in constructing a pseudo-position observation by matching a rolling window of historically accumulated dead-reckoning trajectory with the locally corresponding segment of the mission route extracted from an HD map. Unlike naive projection approaches, the paper formulates the matching as a 2D rigid registration problem, using spatial resampling, azimuth consistency checks, and SVD-based solution to identify optimal rotation and translation between the trajectory and map segment. The algorithm is activated exclusively during sustained GNSS outages and only when sufficient trajectory span and geometric information are available, as determined by configurable triggers.

The resulting pseudo-observation encodes the best-fit road-referenced position in the ENU plane, which is incorporated as a measurement update in the error-state Extended Kalman Filter (EKF). This framework only modifies the update step, preserving the original propagation dynamics of the inertial system. Importantly, the pseudo-observation is not treated as a ground-truth correction but as a statistically robust, locally route-referenced constraint. The algorithm further introduces a navigation offset compensation mechanism to account for systematic deviations between the actual trajectory and the route centerline, preserving lateral offsets that are consistent with real vehicle motion.

Quality control mechanisms—trigger frequency management, heading-filtered matching, and single-step update bounds—safeguard against erroneous updates in low-information scenarios, such as long straight segments or ambiguous path topologies.

Experimental Evaluation and Numerical Results

Empirical validation was performed in three representative, challenging environments (long tunnel, multi-segment tunnel, curved tunnel) using real-world datasets from an extra-long spiral tunnel on the Jingli Expressway. The experimental platform comprised a commercial IMU/ODO system and SD map data, and the proposed method was benchmarked against a pure dead-reckoning baseline. Position and heading deviations were quantified relative to the reference route.

Key numerical findings include:

• Long-Tunnel Scenario:
  • Maximum route-relative position deviation dropped from 386.3 m (baseline) to 22.7 m (proposed).
  • Mean position deviation reduced from 142.909 m to 0.745 m.
  • Position RMSE decreased from 186.821 m to 1.672 m.
  • Heading RMSE diminished from 3.524° to 1.257°.
• Multi-Segment Tunnel:
  • Maximum position deviation improved from 23.3 m to 8.5 m.
  • Position RMSE reduced from 6.502 m to 2.351 m.
• Curved Tunnel:
  • Maximum position deviation decreased from 32.6 m to 27.5 m.
  • Position RMSE dropped from 10.376 m to 1.889 m.

The strongest improvements are observed in long, uninterrupted GNSS outages, with the method maintaining tight coupling to the mission route and suppressing both lateral and longitudinal drift. Heading correction is less pronounced, primarily reflecting the indirect constraint propagated via EKF state coupling.

Technical and Practical Implications

This approach offers an engineering-friendly, computationally efficient enhancement to commercial GNSS/INS/ODO fusion systems under severe GNSS degradation. By harnessing already available mission route data, it achieves near-road-level localization without additional sensor cost. Integration into a standard EKF framework enables seamless adoption within existing localization modules.

However, the method is inherently limited to environments where (i) the route geometry is sufficiently informative for trajectory matching and (ii) the mission route accurately reflects the driveable road segment. Scenarios involving geometric ambiguity (parallel roads, ramps, intersections) and large map-to-road biases necessitate further research in adaptive confidence modeling and robust route-candidate rejection.

On a theoretical level, the work reinforces the utility of using environmental priors as soft constraints in state estimation, influencing both position and attitude indirectly through error covariance coupling.

Future Directions

Potential avenues for advancement include:

• Extension to 3D map constraints, allowing integration of altitude information in multi-level or non-planar environments.
• Automated assessment of route-map quality and development of dynamic confidence/adaptive weighting models for pseudo-observations.
• Hybridization with opportunistic sensor fusion (e.g., visual-lane marks) for scenarios with persistent ambiguity.
• Validation with absolute ground truth to assess metric accuracy beyond route-relative deviations.

Conclusion

The proposed route-constrained robust fusion framework significantly enhances UGV localization stability in GNSS-degraded environments by leveraging historical dead-reckoning/mapping-based pseudo-observations within a standard EKF. Its demonstrated suppression of drift—especially in long GNSS outages—highlights its value for safety-critical, road-level autonomous navigation. The results suggest a practical improvement to state-of-the-art localization pipelines in structured road domains, warranting further investigation into generalization and scalability across diverse operational scenarios (2606.19687).