Vehicle State Estimation through Modular Factor Graph-based Fusion of Multiple Sensors

TL;DR

This paper presents a modular, plug-and-play factor graph-based state estimator for autonomous vehicles that integrates multiple sensors without needing prior sensor parameter knowledge, ensuring robust and accurate state estimation in diverse scenarios.

Contribution

Introduces a novel sensor-agnostic factor graph-based state estimator that operates without predefined sensor parameters, suitable for multiple sensor configurations in autonomous vehicles.

Findings

Accurately estimates vehicle state across various sensor setups.

Maintains robustness during GNSS degradation or outages.

Validated on different vehicles with diverse sensor configurations.

Abstract

This study focuses on the critical aspect of robust state estimation for the safe navigation of an Autonomous Vehicle (AV). Existing literature primarily employs two prevalent techniques for state estimation, namely filtering-based and graph-based approaches. Factor Graph (FG) is a graph-based approach, constructed using Values and Factors for Maximum Aposteriori (MAP) estimation, that offers a modular architecture that facilitates the integration of inputs from diverse sensors. However, most FG-based architectures in current use require explicit knowledge of sensor parameters and are designed for single setups. To address these limitations, this research introduces a novel plug-and-play FG-based state estimator capable of operating without predefined sensor parameters. This estimator is suitable for deployment in multiple sensor setups, offering convenience and providing comprehensive state estimation at a high frequency, including mean and covariances. The proposed algorithm undergoes rigorous validation using various sensor setups on two different vehicles: a quadricycle and a shuttle bus. The algorithm provides accurate and robust state estimation across diverse scenarios, even when faced with degraded Global Navigation Satellite System (GNSS) measurements or complete outages. These findings highlight the efficacy and reliability of the algorithm in real-world AV applications.