A Probabilistic Moving Horizon Estimation Framework Applied to the Visual-Inertial Sensor Fusion Problem

We propose a novel method to compute the arrival cost for the moving horizon estimator. The choice of the arrival cost is an important challenge and is known to have significant influence on the performance of the estimator. Most common approaches are based on implementing a complementary extended Kalman filter to propagate an approximate measure of the uncertainty. Our approach is based on the probabilistic interpretation of the moving horizon estimator and its analogy to the maximum a posteriori estimator. We derive a method to directly obtain the required uncertainties from the Hessian of the moving horizon estimation objective function. We showcase our novel approach with the challenging visual-inertial sensor fusion problem that commonly arises in visual navigation systems. The estimation performance is significantly better compared to our previous results based on the extended Kalman filter. Additionally, the proposed algorithm calibrates the inertial sensor online and is immediately ready for operation.