Iterative Adaptive Multi-State Constrained Localization Algorithm Based on Vision/inertial Fusion

【Purposes】 An iterative adaptive multi-state constrained Kalman filter binocular vision/inertial mileage calculation method (NN-MSCKF) is proposed to address the problem that the existing binocular vision/inertial mileage calculation method cannot accurately capture data in real time when the rescuers are performing localization calculations in obscured space. 【Methods】First, the tracking efficiency and real-time requirements of the rescue personnel’s violent and complex movements in occluded space analyzed, an iterative adaptive algorithm was designed, and window data iteration was used to judge the excitation and trigger the initialisation condition to construct the measurement update; Second, the way of evaluating and screening the number of map points and pixel differentiation was studied, and a map point optimisation mechanism was introduced to improve the real-time performance of evaluating and screening map points; Finally, a simulation and test platform is built to validate the algorithm. 【Findings】 The experimental results show that the algorithm improves the real-time performance by 1s, the global accuracy by 55% and the local accuracy by 88.9% compared with the MSCKF algorithm, which verifies the effectiveness of the method.