Developing Moving Horizon Estimation Based Ranging Measurement for Supporting Vision-Aided Inertial Navigation System

The objective of this paper is to develop an advanced Vision-and-Ranging-aided Inertial Navigation System (VRINS), which combines a Vision-aided Inertial Navigation System (VINS) with Moving Horizon Estimation (MHE) based ranging measurement update. The traditional VINS estimate suffers the error accumulation from the camera observation, which makes the system diverge and fails to track the vehicle trajectory in long-term operation. Hence, a ranging sensor is integrated with VINS in the sequential-sensor-update structure, which allows the filter to operate for longer duration. The ranging measurement update is developed with the MHE, which directly incorporates the system constraints into the optimization process. The VINS is developed with Cubature Multi-State Constraint Kalman Filter (MSCKF), which has 30-dimension filter state, tight constraints of state transition and observability. Those elements need to be considered in the design of MHE optimization. The implementation of MHE is conducted with CASADI library. The proposed VRINS will be validated using KITTI dataset and compared against the VINS.