Backward Smoothing Based Visual-Inertial Odometry Algorithm for Discontinuous Visual Tracking

Visual-Inertial integrated odometry is one of the main navigation methods for unmanned platforms. However, existing algorithms reinitialize a new world frame when visual feature or optical flow tracking is lost, which leads to inconsistent representation of the estimated motion parameters and multiple uncalibrated submaps. To generate consecutive and consistent motion estimations without detecting common visual features between submaps, we propose a novel backward smoothing based visual-inertial odometry. The new odometry is built upon a state transformation extended Kalman filter (ST-EKF), which uses inertial measurements for prediction and velocity as the filter observation. When visual tracking is lost, the algorithm performs forward ST-EKF prediction with inertial measurement only; meanwhile the visual-inertial odometry detects new visual tracking for reinitialization. As visual-inertial reinitialization can-not estimate the position, we project the reinitialized velocity to the inertial frame as the observation for the ST-EKF component, and perform Rauch-Tung-Striebel (RTS) backward smoothing to compensate for the inertial accumulated errors. We conducted multiple experiments with different durations of visual loss and varying levels of IMU accuracy. The results show that compare to consecutive visual tracking condition, the proposed algorithm only has a degradation of 0.8 % D in relocalization accuracy after 60 seconds lost of visual tracking. The results illustrate the proposed algorithm can provide consecutive and consistent motion estimation under unstable visual tracking.