Consistent Covariance Pre-Integration for Invariant Filters with Delayed Measurements

Abstract

Sensor fusion systems merging (multiple) delayed sensor signals through a statistical approach are challenging setups, particularly for resource constrained platforms. For statistical consistency, one would be required to keep an appropriate history, apply the correcting signal at the given time stamp in the past, and re-apply all information received until the present time. This re-calculation becomes impractical (the bottleneck being the re-propagation of the covariance matrices for estimator consistency) for platforms with multiple sensors/states and low compute power.This work presents a novel approach for consistent covariance pre-integration allowing delayed sensor signals to be incorporated in a statistically consistent fashion with very low complexity. We leverage recent insights in Invariant Extended Kalman Filters (IEKF) and their log-linear, state independent error propagation together with insights from the scattering theory to mimic the re-calculation process as a medium through which we can propagate waves (covariance information in this case) in single operation steps. We support our findings in simulation and with real data.