Covariance Pre-Integration for Delayed Measurements in Multi-Sensor Fusion

Abstract

Delay compensation in filter based sensor fusion frameworks for multiple sensors with varying delays and different rates quickly results in large computational overhead should the delayed measurements be incorporated in a statistically meaningful way. Even more so if high rate propagation sensors (e.g. IMU) are used. This work presents an approach to implement such frameworks with significant complexity reduction compared to standard implementations. We set particular focus on the state covariance propagation as this chain of re-computations (i.e. FPFT + Q per propagation step) upon a delayed update is the dominant bottleneck. We draw our inspiration from the scattering theory and propose a method which projects the idea of wave propagation to an efficient concatenation of covariance propagation steps between filter updates. Through this approach, we reach a speed-up of more than a factor of 10 for the covariance propagation and render the computational complexity independent of the number of propagation steps between filter updates. We evaluated our method in simulation and with real data.