Toward Consistent and Efficient Map-Based Visual-Inertial Localization: Theory Framework and Filter Design
Zhuqing Zhang, Yang Song, Shoudong Huang, Rong Xiong, Yue Wang
Abstract
This article focuses on designing a consistent and efficient filter for visual-inertial localization given a prebuilt map. First, we propose a new Lie group with its algebra based on which a novel invariant extended Kalman filter (invariant EKF) is designed. We theoretically prove that, when we do not consider the uncertainty of map information, the proposed invariant EKF is able to naturally preserve the correct observability properties of the system. To consider the uncertainty of map information, we introduce a Schmidt filter. With the Schmidt filter, the uncertainty of map information can be taken into consideration to avoid overconfident estimation while the computation cost only increases linearly with the size of the map keyframes. In addition, we introduce an easily implemented observability-constrained technique because directly combining the invariant EKF with the Schmidt filter cannot maintain the correct observability properties of the system that considers the uncertainty of map information. Finally, we validate our proposed system's high consistency, accuracy, and efficiency via extensive simulations and real-world experiments.