Square Root Inverse Sliding Window Filter (SR-ISWF)

UMN Researchers

Summary

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

 

Aerial map of trajectory.
Basement floor plan with blue (MSC-KF) and red (SR-ISWF) estimated paths through a large hall and corridors, closely tracking each other
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).