Vehicle State Estimation Through Dynamics Modeled Factor Graph

Ego Vehicle state estimation is integral to every autonomous driving software stack. Thereby, the estimation of the state and its components as for example the side slip angle, is a crucial component to track the vehicle maneuvers. In the absence of a direct sensor measuring side slip angle, most of the existing literature either use observers like Kalman Filters or non-modular factor graphs by modeling lateral dynamics. However, the modularity of such graphs, to integrate multiple asynchronous sensors that provide disentangled measurements, like LiDAR, GNSS, and IMU is still overlooked in the literature. In this work, we propose a novel factor graph-based architecture that builds upon the vehicle dynamics at its core to enable the fusion of multiple sensors asynchronously and enables to perform robust and accurate state estimation. We validate the proposed algorithm against two baselines, a model-based Extended Kalman Filter and a factor graph-based state estimator that uses the IMU pre-integration factor as a reference factor. The algorithms are validated in a custom dataset collected using an in-house vehicle.