Decoupled Right Invariant Error States for Consistent Visual-Inertial Navigation
Date
2022-01-04
Journal Title
Journal ISSN
Volume Title
Publisher
IEEE Robotics and Automation Letters
Abstract
The invariant extended Kalman filter (IEKF) is proven to preserve the observability property of visual-inertial navigation systems (VINS) and suitable for consistent estimator design. However, if features are maintained in the state vector, the propagation of IEKF will become more computationally expensive because these features are involved in the covariance propagation. To address this issue, we propose two novel algorithms which preserve the system consistency by leveraging the invariant state representation and ensure efficiency by decoupling features from covariance propagation. The first algorithm combines right invariant error states with first-estimates Jacobian (FEJ) technique, by decoupling the features from the Lie group representation and utilizing FEJ for consistent estimation. The second algorithm is designed specifically for sliding-window filter-based VINS as it associates the features to an active cloned pose, instead of the current IMU state, for Lie group representation. A new pseudo-anchor change algorithm is also proposed to maintain the features in the state vector longer than the window span. Both decoupled right- and left-invariant error based VINS methods are implemented for a complete comparison. Extensive Monte-Carlo simulations on three simulated trajectories and real world evaluations on the TUM-VI datasets are provided to verify our analysis and demonstrate that the proposed algorithms can achieve improved accuracy than a state-of-art filter-based VINS algorithm using FEJ.
Description
© 2022 IEEE. Personal use of this material is permitted. Permission from IEEE must be obtained for all other uses, in any current or future media, including reprinting/republishing this material for advertising or promotional purposes, creating new collective works, for resale or redistribution to servers or lists, or reuse of any copyrighted component of this work in other works. This article was originally published in IEEE Robotics and Automation Letters. The version of record is available at: https://doi.org/10.1109/LRA.2021.3140054.
Keywords
Invariant extended Kalman filter, localization, mapping, visual-inertial SLAM
Citation
Y. Yang, C. Chen, W. Lee and G. Huang, "Decoupled Right Invariant Error States for Consistent Visual-Inertial Navigation," in IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1627-1634, April 2022, doi: 10.1109/LRA.2021.3140054.