A low-cost visual inertial odometry for mobile vehicle based on double stage Kalman filter

Abstract

When the low-cost inertial measurement units are used in actual vehicle localization, the measurements suffer from large noise density and unstable bias. Therefore, the integral gyroscope error becomes larger, which results in visual-inertial odometry system degradation or even failure. To this end, this paper proposes a robust visual-inertial odometry method based on a double-stage Kalman filter for a low-cost visual-inertial system, which consists of two Kalman filters. The first filter is a complementary Kalman filter, which uses the accelerometer to correct the gyroscope bias, and then an accurate initial pose estimation is calculated. The second filter is a multi-state observation-constrained Kalman filter, in which the re-projection error of features is calculated based on the multi-state observation constraint strategy to update the system states. Additionally, a Schur complement model is used for the sliding window to marginalize the oldest camera pose of the system states, avoiding the loss of associated information between images and improving the accuracy of the camera pose. Finally, the EuRoC dataset and a homemade low-cost visual-inertial hardware system are used to evaluate the performance of the proposed algorithm. The results show that the accuracy of the low-cost gyroscope bias estimation will decrease when the visual observation is inaccurate in the classic VIO, the proposed algorithm corrects the gyroscope bias with accelerometer measurements, which significantly improves the accuracy and robustness of the vehicle pose estimation when low-cost inertial measurement units are used.