A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation

In this paper, we present an extended Kalman filter (EKF)-based algorithm for real-time vision-aided inertial navigation. The primary contribution of this work is the derivation of a measurement model that is able to express the geometric constraints that arise when a static feature is observed from multiple camera poses. This measurement model does not require including the 3D feature position in the state vector of the EKF and is optimal, up to linearization errors. The vision-aided inertial navigation algorithm we propose has computational complexity only linear in the number of features, and is capable of high-precision pose estimation in large-scale real-world environments. The performance of the algorithm is demonstrated in extensive experimental results, involving a camera/IMU system localizing within an urban area.

[1]  Stergios I. Roumeliotis,et al.  On the treatment of relative-pose measurements for mobile robot localization , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[2]  Stefano Soatto,et al.  Structure from Motion Causally Integrated Over Time , 2002, IEEE Trans. Pattern Anal. Mach. Intell..

[3]  Stergios I. Roumeliotis,et al.  Augmenting inertial navigation with image-based motion estimation , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[4]  John Oliensis.  A new structure-from-motion ambiguity , 1999, Proceedings. 1999 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (Cat. No PR00149).

[5]  Hanumant Singh,et al.  Visually Navigating the RMS Titanic with SLAM Information Filters , 2005, Robotics: Science and Systems.

[6]  P. Perona,et al.  Recursive 3-D Visual Motion Estimation Using Subspace Constraints , 1997, International Journal of Computer Vision.

[7]  Gene H. Golub,et al.  Matrix computations , 1983 .

[8]  T. Kanade,et al.  Vision-Based Kalman Filtering for Aircraft State Estimation and Structure from Motion , 2005 .

[9]  D.S. Bayard,et al.  An estimation algorithm for vision-based exploration of small bodies in space , 2005, Proceedings of the 2005, American Control Conference, 2005..

[10]  Jonghyuk Kim,et al.  Six DoF Decentralised SLAM , 2003 .

[11]  Matthew Deans,et al.  Maximally informative statistics for localization and mapping , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[12]  Jacob Willem Langelaan.  State estimation for autonomous flight in cluttered environments , 2006 .

[13]  James R. Bergen,et al.  Visual odometry for ground vehicle applications , 2006, J. Field Robotics.

[14]  Richard Szeliski,et al.  Vision Algorithms: Theory and Practice , 2002, Lecture Notes in Computer Science.

[15]  Javier Civera,et al.  Unified Inverse Depth Parametrization for Monocular SLAM , 2006, Robotics: Science and Systems.

[16]  A. B. Chatfield.  Fundamentals of high accuracy inertial navigation , 1997 .

[17]  Stephen M. Rock,et al.  Relative position sensing by fusing monocular vision and inertial rate sensors , 2003 .

[18]  Philip F. Mclauchlan,et al.  The Variable State Dimension Filter applied to Surface-Based Structure from Motion , 1999 .

[19]  Xavier Cufí,et al.  Augmented state Kalman filtering for AUV navigation , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[20]  G LoweDavid,et al.  Distinctive Image Features from Scale-Invariant Keypoints , 2004 .

[21]  David D. Diel.  Stochastic constraints for vision-aided inertial navigation , 2005 .

[22]  Andrew W. Fitzgibbon,et al.  Bundle Adjustment - A Modern Synthesis , 1999, Workshop on Vision Algorithms.

[23]  David W. Murray,et al.  Simultaneous Localization and Map-Building Using Active Vision , 2002, IEEE Trans. Pattern Anal. Mach. Intell..

[24]  P. Perona,et al.  Motion estimation via dynamic vision , 1994, Proceedings of 1994 33rd IEEE Conference on Decision and Control.

[25]  Sanjiv Singh,et al.  Motion Estimation from Image and Inertial Measurements , 2004, Int. J. Robotics Res..

[26]  W MurrayDavid,et al.  Simultaneous Localization and Map-Building Using Active Vision , 2002 .

[27]  Tom Drummond,et al.  Scalable Monocular SLAM , 2006, 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR'06).