An O(N²) Square Root Unscented Kalman Filter for Visual Simultaneous Localization and Mapping

This paper develops a square root unscented Kalman filter (SRUKF) for performing video-rate visual simultaneous localization and mapping (SLAM) using a single camera. The conventional UKF has been proposed previously for SLAM, improving the handling of nonlinearities compared with the more widely used extended Kalman filter (EKF). However, no account was taken of the comparative complexity of the algorithms: In SLAM, the UKF scales as O(N 3) in the state length, compared to the EKF's O(N 2), making it unsuitable for video-rate applications with other than unrealistically few scene points. Here, it is shown that the SRUKF provides the same results as the UKF to within machine accuracy and that it can be reposed with complexity O(N 2) for state estimation in visual SLAM. This paper presents results from video-rate experiments on live imagery. Trials using synthesized data show that the consistency of the SRUKF is routinely better than that of the EKF, but that its overall cost settles at an order of magnitude greater than the EKF for large scenes.

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

[2]  L. B. Hostetler,et al.  Nonlinear Kalman filtering techniques for terrain-aided navigation , 1983 .

[3]  Christopher G. Harris,et al.  3D positional integration from image sequences , 1988, Image Vis. Comput..

[4]  Walterio W. Mayol-Cuevas,et al.  Real-Time and Robust Monocular SLAM Using Predictive Multi-resolution Descriptors , 2006, ISVC.

[5]  John J. Leonard,et al.  Directed Sonar Sensing for Mobile Robot Navigation , 1992 .

[6]  Hanumant Singh,et al.  Exactly Sparse Delayed-State Filters for View-Based SLAM , 2006, IEEE Transactions on Robotics.

[7]  Olivier D. Faugeras,et al.  Maintaining representations of the environment of a mobile robot , 1988, IEEE Trans. Robotics Autom..

[8]  N. Higham.  Analysis of the Cholesky Decomposition of a Semi-definite Matrix , 1990 .

[9]  Olivier Stasse,et al.  MonoSLAM: Real-Time Single Camera SLAM , 2007, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[10]  Olivier Faugeras,et al.  Maintaining representations of the environment of a mobile robot , 1988, IEEE Trans. Robotics Autom..

[11]  José A. Castellanos,et al.  Unscented SLAM for large-scale outdoor environments , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[12]  Eduardo Mario Nebot,et al.  Consistency of the EKF-SLAM Algorithm , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[13]  Reinhard Koch,et al.  Visual Modeling with a Hand-Held Camera , 2004, International Journal of Computer Vision.

[14]  D. E. Brown,et al.  The over-extended Kalman filter - don't use it! , 2003, Sixth International Conference of Information Fusion, 2003. Proceedings of the.

[15]  José A. Castellanos,et al.  Robocentric map joining: Improving the consistency of EKF-SLAM , 2007, Robotics Auton. Syst..

[16]  Andrew J. Davison,et al.  Real-time simultaneous localisation and mapping with a single camera , 2003, Proceedings Ninth IEEE International Conference on Computer Vision.

[17]  Andrew W. Fitzgibbon,et al.  Automatic Camera Recovery for Closed or Open Image Sequences , 1998, ECCV.

[18]  Gamini Dissanayake,et al.  Convergence and Consistency Analysis for Extended Kalman Filter Based SLAM , 2007, IEEE Transactions on Robotics.

[19]  Reinhard Koch,et al.  Self-calibration and metric reconstruction in spite of varying and unknown internal camera parameters , 1998, Sixth International Conference on Computer Vision (IEEE Cat. No.98CH36271).

[20]  Herman Bruyninckx,et al.  Kalman filters for non-linear systems: a comparison of performance , 2004 .

[21]  T. Başar,et al.  A New Approach to Linear Filtering and Prediction Problems , 2001 .

[22]  Jeffrey K. Uhlmann,et al.  Unscented filtering and nonlinear estimation , 2004, Proceedings of the IEEE.

[23]  Rudolph van der Merwe,et al.  The square-root unscented Kalman filter for state and parameter-estimation , 2001, 2001 IEEE International Conference on Acoustics, Speech, and Signal Processing. Proceedings (Cat. No.01CH37221).

[24]  S. Rock,et al.  Passive GPS-Free Navigation for Small UAVs , 2005, 2005 IEEE Aerospace Conference.

[25]  Hugh F. Durrant-Whyte,et al.  Simultaneous Localization and Mapping with Sparse Extended Information Filters , 2004, Int. J. Robotics Res..

[26]  Jeffrey K. Uhlmann,et al.  New extension of the Kalman filter to nonlinear systems , 1997, Defense, Security, and Sensing.

[27]  Frank Dellaert,et al.  iSAM: Fast Incremental Smoothing and Mapping with Efficient Data Association , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[28]  Raja Chatila,et al.  Stochastic multisensory data fusion for mobile robot location and environment modeling , 1989 .

[29]  Ingemar J. Cox,et al.  Dynamic Map Building for an Autonomous Mobile Robot , 1990, EEE International Workshop on Intelligent Robots and Systems, Towards a New Frontier of Applications.

[30]  Peter Cheeseman,et al.  On the Representation and Estimation of Spatial Uncertainty , 1986 .

[31]  Sebastian Thrun,et al.  Probabilistic robotics , 2002, CACM.

[32]  David W. Murray,et al.  Mobile Robot Localisation Using Active Vision , 1998, ECCV.

[33]  David W. Murray,et al.  A Square Root Unscented Kalman Filter for visual monoSLAM , 2008, 2008 IEEE International Conference on Robotics and Automation.

[34]  Gene H. Golub,et al.  Methods for modifying matrix factorizations , 1972, Milestones in Matrix Computation.

[35]  Vidal Calleja,et al.  Visual Navigation in Unknown Environments , 2007 .

[36]  R. E. Kalman,et al.  New Results in Linear Filtering and Prediction Theory , 1961 .

[37]  H.F. Durrant-Whyte,et al.  A new approach for filtering nonlinear systems , 1995, Proceedings of 1995 American Control Conference - ACC'95.

[38]  A.S. Paul,et al.  Dual Kalman filters for autonomous terrain aided navigation in unknown environments , 2005, Proceedings. 2005 IEEE International Joint Conference on Neural Networks, 2005..

[39]  Teresa A. Vidal-Calleja,et al.  Unscented Transformation of Vehicle States in SLAM , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[40]  David Nister,et al.  Automatic Dense Reconstruction from Uncalibrated Video Sequences , 2001 .

[41]  Jeffrey K. Uhlmann,et al.  A counter example to the theory of simultaneous localization and map building , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).

[42]  Reinhard Koch,et al.  Self-Calibration and Metric Reconstruction Inspite of Varying and Unknown Intrinsic Camera Parameters , 1999, International Journal of Computer Vision.