Adaptive compensation of measurement delays in multi-sensor fusion for inertial motion tracking using moving horizon estimation

Robust and accurate pose estimation of moving systems is a challenging task that is often tackled by combining information from different sensor subsystems in a multi-sensor fusion setup. To obtain robust and accurate estimates, it is crucial to respect the exact time of each measurement. Data fusion is additionally challenged when the sensors are running at different rates and the information is subject to processing- and transmission delays. In this paper, we present an optimization-based moving horizon estimator which allows to estimate and compensate for time-varying measurement delays without the need for any synchronization signals between the sensors. By adopting a direct collocation approach, we find a continuous-time solution for the navigation states which allows us to incorporate the discrete-time sensor measurements in an optimal way despite the presence of unknown time delays. The presented sensor fusion algorithm is applied to the problem of pose estimation by fusing data of a high-rate inertial measurement unit and a low-rate centimeter-accurate global navigation satellite system receiver using simulated and real-data experiments.