In this work, we present an efficient vision-aided inertial navigation system (VINS) for high-precision localization in GPS-denied areas. To the best of our knowledge, this is the first VINS in the information domain that operates in real time on resource-constrained mobile devices, such as cell phones and quadrotors. Due to its square-root formulation, the SR-ISWF's superior numerical properties, as compared to an inverse filter (INVF), allows using single-precision format for performing numerical operations very fast. In contrast to the multi-state constraint Kalman filter (MSC-KF), the SR-ISWF is highly responsive and supports smoothing, i.e., all visual-inertial measurements can be processed as soon as they become available and relinearized multiple times to achieve higher accuracy. The SR-ISWF takes advantage of the particular structure of the VINS problem, to deliver significant computational gains. In experiments carried out using a Samsung S4 mobile phone, the SR-ISWF achieves comparable positioning accuracy with competing algorithms, such as the MSC-KF, while significantly outperforming them in terms of speed.
Algorithm Highlights
The SR-ISWF algorithm:
Optimizes over a sliding window of recent pose and feature states, hence has adjustable computational requirements
Classfies and processes features based their information content, to provide a trade-off between accuracy and efficiency
Estimates parameters (e.g., IMU-camera time-sychronization and extrinsics, camera rolling shutter time) for precisely modeling low-cost sensors
Employs efficient QR factorizations that take advantage of the problem structure
SR-ISWF vs. MSC-KF estimated trajectories overlayed on the areas' blueprints.
Videos
Live demonstration on a Samsung S4 cell phone in the Walter Library, University of Minnesota
Relevant Publications
C1. K.J. Wu, A.M. Ahmed, G.A. Georgiou, and S.I. Roumeliotis, "A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices," In Proc. of Robotics: Science and Systems, Rome, Italy, Jul. 13-17, 2015 (pdf).
