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.
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.
Live demonstration on a Samsung S4 cell phone in the Walter Library, University of Minnesota
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).