EKF-Based Visual Inertial Navigation Using Sliding Window Nonlinear Optimization

In this paper, we present a hybrid visual inertial navigation algorithm for an autonomous and intelligent vehicle that combines the multi-state constraint Kalman filter (MSCKF) with the nonlinear visual-inertial graph optimization. The MSCKF is a well-known visual inertial odometry (VIO) method that performs the fusion between an inertial measurement unit (IMU) and the image measurements within a sliding window. The MSCKF computes the re-projection errors from the camera measurements and the states in the sliding window. During this process, the structure-only estimation is performed without exploiting the full information over the window, like the relative interstate motion constraints and their uncertainties. The key contribution of this paper is combination of the filtering and non-linear optimization method for VIO, and the design of a novel measurement model that exploits all of the measurements and information available within the sliding window. The local visual-inertial optimization is performed using pre-integrated IMU measurements and camera measurements. It infers the probabilistically optimal relative pose constraints. These local optimal constraints are used to estimate the global states under the MSCKF framework. The proposed local-optimal-multi-state constraint Kalman filter is validated using a simulation data set, as well as publicly available real-world data sets generated from real-world urban driving experiments.