Error State Kalman Filter with Implicit Measurement Equations for Position Tracking of a Multi-Sensor System with IMU and LiDAR

With many applications requiring individuals to share spaces with autonomous systems, not only do accurate positioning solutions become crucial, but also understanding of the uncertainty associated with these systems. For applications like public transportation or logistics, position tracking algorithms need to be evaluated on accuracy and consistency. The selection of the appropriate algorithms heavily relies on the specific requirements of the applications, thus demanding novel algorithms for these new use cases.This paper introduces a novel error state Kalman filter with implicit measurement equations, presenting a solution for new applications. The filter facilitates the fusion of inertial measurement unit (IMU) sensor data with other sensors using arbitrary measurement models. To showcase its effectiveness, the filter is demonstrated by fusing simulated IMU and LiDAR observations for position tracking (complete code on Github). Specifically, the LiDAR points are employed in the update step by minimizing the distances to known planes. Furthermore, the performance of the filter is enhanced by motion compensation through pose interpolation, utilizing the available timestamps. The results are thoroughly discussed in terms of accuracy and consistency, revealing significant improvements by employing pose interpolation. However, the consistency analysis indicates slightly pessimistic results, suggesting the need for further optimizations.