Region Iterative Correction Algorithm in Visual-Inertial Navigation System

Abstract

In this paper, a region iterative correction algorithm is proposed to improve the performance of Multi-State Constraint Kalman Filter (MSCKF) based visual-inertial navigation system in large-scale long-distance scenarios. To improve the real-time performance of the algorithm, traditional MSCKF uses a sliding window to constrain the state. However, for the long-distance landmarks, the relatively small sliding window capacity is inable to provide sufficient baseline and accurate disparity calculation for high accuracy landmark estimation. Meanwhile, in order to keep the consistency of the systematic observations, MSCKF maintains an initial nullspace containing the landmark estimation to constrain the system transfer matrix with the observability constrained (OC) method. As a result, poor landmark estimation leads to the constructed initial nullspace deviating from the true value, and keeping the wrong correction direction for a long time makes the system performance degraded. To address the above problem, this paper proposes a landmark estimation algorithm based on region iterative correction. The algorithm forms a region judgment logic based on sliding window by setting a time threshold constraint and monitoring the initial frame change in window. When the logic determines that the carrier moves into a new region, iterative estimation of landmarks is realized with the combination of the current region observation. The joint correction of multi-level regions can effectively avoid the degradation of system performance caused by the error of nullspace constraints. The results based on Airsim virtual scenario indicate that the region iterative correction algorithm can effectively overcome the drawback of incorrect correction directions caused by poor initial landmark estimation and improve the positioning accuracy of visual-inertial navigation system in large scale, long-distance and long-time tracking environments.