This Article 
 Bibliographic References 
 Add to: 
An O(N²) Square Root Unscented Kalman Filter for Visual Simultaneous Localization and Mapping
July 2009 (vol. 31 no. 7)
pp. 1251-1263
Steven A. Holmes, University of Oxford, Oxford
Georg Klein, University of Oxford, Oxford
David W. Murray, University of Oxford, Oxford
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] A.W. Fitzgibbon and A. Zisserman, “Automatic Camera Recovery for Closed or Open Image Sequences,” Proc. Fifth European Conf. Computer Vision, vol. 1, pp. 311-326, 1998.
[2] M. Pollefeys, R. Koch, and L. Van Gool, “Self-Calibration and Metric Reconstruction in Spite of Varying and Unknown Internal Camera Parameters,” Int'l J. Computer Vision, vol. 32, no. 1, pp. 7-25, 1999.
[3] M. Pollefeys, L. Van Gool, M. Vergauwen, F. Verbiest, K. Cornelis, J. Tops, and R. Koch, “Visual Modeling with a Hand-Held Camera,” Int'l J. Computer Vision, vol. 59, no. 3, pp. 207-232, 2004.
[4] D. Nistér, “Automatic Dense Reconstruction from Uncalibrated Video Sequences,” PhD thesis, Royal Inst. of Technology KTH, Stockholm, Sweden, Mar. 2001.
[5] 2d3 Ltd. Boujou,,, Feb. 2008.
[6] REALVIZ SA. Matchmover, http:/, Feb. 2008.
[7] R.C. Smith and P. Cheeseman, “On the Representation and Estimation of Spatial Uncertainty,” Int'l J. Robotics Research, vol. 5, no. 4, pp. 56-68, 1986.
[8] J.J. Leonard, H.F. Durrant-Whyte, and I.J. Cox, “Dynamic Map Building for an Autonomous Mobile Robot,” Int'l J. Robotics Research, vol. 11, no. 8, pp. 286-298, 1992.
[9] J.J. Leonard and H.F. Durrant-Whyte, Directed Sonar Sensing for Mobile Robot Navigation. Kluwer Academic Publishers, 1992.
[10] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. MIT Press, 2005.
[11] A.J. Davison and D.W. Murray, “Mobile Robot Localisation Using Active Vision,” Proc. Fifth European Conf. Computer Vision, pp. 809-825, 1998.
[12] A.J. Davison, “Real-Time Simultaneous Localisation and Mapping with a Single Camera,” Proc. Ninth IEEE Int'l Conf. Computer Vision, pp. 1403-1410, 2003.
[13] A.J. Davison, I.D. Reid, N. Molton, and O. Stasse, “MonoSLAM: Real-Time Single Camera SLAM,” IEEE Trans. Pattern Analysis and Machine Intelligence, vol. 29, no. 6, pp. 1052-1067, June 2007.
[14] N. Ayache and O.D. Faugeras, “Maintaining Representations of the Environment of a Mobile Robot,” IEEE Trans. Robotics and Automation, vol. 5, no. 6, pp. 804-819, 1989.
[15] C.G. Harris and J.M. Pike, “3D Positional Integration from Image Sequences,” Image and Vision Computing, vol. 6, no. 2, pp. 87-90, 1987.
[16] S.J. Julier and J.K. Uhlmann, “Unscented Filtering and Nonlinear Estimation,” Proc. IEEE, vol. 92, no. 3, 2004.
[17] R. van der Merwe and E. Wan, “The Square-Root Unscented Kalman Filter for State and Parameter Estimation,” Proc. IEEE Int'l Conf. Acoustics, Speech, and Signal Processing, vol. 6, pp. 3461-3464, 2001.
[18] T. Lefebvre, H. Bruyninckx, and J. de Schutter, “Kalman Filters for Non-Linear Systems: A Comparison of Performance,” Int'l J. Control, vol. 77, no. 7, pp. 639-653, 2004.
[19] S.J. Julier, J.K. Uhlmann, and H.F. Durrant-Whyte, “A New Approach for Filtering Nonlinear Systems,” Proc. 14th Am. Control Conf., pp. 1628-1632, 1995.
[20] S.J. Julier and J.K. Uhlmann, “A New Extension of the Kalman Filter to Nonlinear Systems,” Proc. SPIE 11th Ann. Int'l Symp. Aerospace/Defense Sensing, Simulation, and Controls, pp. 182-193, 1997.
[21] R. Martinez-Cantin and J.A. Castellanos, “Unscented SLAM for Large-Scale Outdoor Environments,” Proc. IEEE/RSJ Int'l Conf. Intelligent Robots and Systems, pp. 3427-3432, 2005.
[22] J. Andrade-Cetto, T. Vidal-Calleja, and A. Sanfeliu, “Unscented Transformation of Vehicle States in SLAM,” Proc. IEEE Int'l Conf. Robotics and Automation, pp. 323-328, 2005.
[23] J. Langelaan and S. Rock, “Passive GPS-Free Navigation for Small UAVs,” Proc. IEEE Aerospace Conf., pp. 1-9, 2005.
[24] D. Chekhlov, M. Pupilli, W. Mayol-Cuevas, and A. Calway, “Real-Time and Robust Monocular SLAM Using Predictive Multi-Resolution Descriptors,” Proc. Second Int'l Symp. Visual Computing, pp. 276-285, 2006.
[25] P. Moutarlier and R. Chatila, Stochastic Multisensor Data Fusion for Mobile Robot Location and Environment Modelling, 1989.
[26] S. Thrun, Y. Liu, D. Koller, A.Y. Ng, Z. Ghahramani, and H.F. Durrant-Whyte, “Simultaneous Localization and Mapping with Sparse Extended Information Filters,” Int'l J. Robotics Research, vol. 23, nos. 7/8, pp. 693-716, 2004.
[27] R.M. Eustice, H. Singh, and J.J. Leonard, “Exactly Sparse Delayed-State Filters for View-Based SLAM,” IEEE Trans. Robotics, vol. 22, no. 6, pp. 1100-1114, Dec. 2006.
[28] M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Fast Incremental Smoothing and Mapping with Efficient Data Association,” Proc. IEEE Int'l Conf. Robotics and Automation, pp. 1670-1677, Apr. 2007.
[29] A.S. Paul and E.A. Wan, “Dual Kalman Filters for Autonomous Terrain Aided Navigation in Unknown Environments,” Proc. IEEE Int'l Joint Conf. Neural Networks, pp. 2784-2789, July 2005.
[30] S. Holmes, G. Klein, and D.W. Murray, “A Square Root Unscented Kalman Filter for Visual MonoSLAM,” Proc. IEEE Int'l Conf. Robotics and Automation, pp. 3710-3716, May 2008.
[31] R.E. Kalman, “A New Approach to Linear Filtering and Prediction Problems,” Trans. ASME J. Basic Eng., vol. 82, pp. 35-45, 1960.
[32] R.E. Kalman and R.S. Bucy, “New Results in Linear Filtering and Prediction Theory,” Trans. ASME J. Basic Eng., vol. 83, pp. 95-108, 1961.
[33] T.A. Vidal Calleja, “Visual Navigation in Unknown Environments,” PhD thesis, Institut de Robòtica i Informàtica Industrial, Universitat Politècnica de Catalunya, 2007.
[34] J.M.M. Montiel, J. Civera, and A.J. Davison, “Unified Inverse Depth Parametrization for Monocular SLAM,” Proc. Robotics: Science and Systems, 2006.
[35] D.F. Bizup and D.E. Brown, “The Over-Extended Kalman Filter— Don't Use It,” Proc. Sixth Int'l Conf. Information Fusion, vol. 1, pp.40-46, 2003.
[36] L.D. Hostetler and R.D. Andreas, “Nonlinear Kalman Filtering Techniques for Terrain-Aided Navigation,” IEEE Trans. Automatic Control, vol. 28, no. 3, pp. 315-323, 1983.
[37] S. Julier and J.K. Uhlmann, “A Counter Example to the Theory of Simultaneous Localization and Map Building,” Proc. IEEE Int'l Conf. Robotics and Automation, pp. 4238-4243, 2001.
[38] S. Huang and G. Dissanayake, “Convergence and Consistency Analysis for Extended Kalman Filter Based Slam,” IEEE Trans. Robotics, vol. 23, no. 5, pp. 1036-1049, Oct. 2007.
[39] J.A. Castellanos, R. Martinez-Cantin, J.D. Tardós, and J. Neira, “Robocentric Map Joining: Improving the Consistency of EKF-SLAM,” Robotics and Autonomous Systems, vol. 55, no. 1, pp. 21-29, Jan. 2007.
[40] T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot, “Consistency of the EKF-SLAM Algorithm,” Proc. IEEE/RSJ Int'l Conf. Intelligent Robots and Systems, pp. 3562-3568, 2006.
[41] P.E. Gill, G.H. Golub, W. Murray, and M.A. Saunders, “Methods for Modifying Matrix Factorizations,” Math. Computation. pp. 505-535, 1974.
[42] G.H. Golub, private communication, 2007.
[43] N.J. Higham, “Analysis of the Cholesky Decomposition of a Semi-Definite Matrix,” Reliable Numerical Computation, M.G. Cox and S.J.Hammarling, eds., pp. 161-185, Oxford Univ. Press, 1990.

Index Terms:
Structure from motion, simultaneous localization and mapping, unscented Kalman filter.
Steven A. Holmes, Georg Klein, David W. Murray, "An O(N²) Square Root Unscented Kalman Filter for Visual Simultaneous Localization and Mapping," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 31, no. 7, pp. 1251-1263, July 2009, doi:10.1109/TPAMI.2008.189
Usage of this product signifies your acceptance of the Terms of Use.