New Design Techniques for Globally Convergent Simultaneous Localization and Mapping: Analysis and Implementation

  • Pedro Lourenço
  • Bruno Guerreiro
  • Pedro BatistaEmail author
  • Paulo Oliveira
  • Carlos Silvestre
Part of the Lecture Notes in Control and Information Sciences book series (LNCIS, volume 474)


This chapter presents an overview of algorithms deeply rooted in a sensor-based approach to the SLAM problem that provide global convergence guarantees and allow for the use of partially observable landmarks. The presented algorithms address the more usual range-and-bearing SLAM problem, either in 2-D using a LiDAR or in 3-D using an RGB-D camera , as well as the range-only and bearing-only SLAM problems. For each of these formulations a nonlinear system is designed, for which state and output transformations are considered together with augmented dynamics, in such a way that the underlying system structure can be regarded as linear time-varying for observability analysis and filter design purposes. This naturally allows for the design of Kalman filters with, at least, globally asymptotically stable error dynamics, for which several experimental and simulated trials are presented to highlight the performance and consistency of the obtained filters.


Visible Landmark State Augmentation Loop Closing Stable Error Dynamic VICON Motion Tracking System 
These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.



This work was supported by the Fundação para a Ciência e a Tecnologia (FCT) through ISR under LARSyS UID/EEA/50009/2013, and through IDMEC, under LAETA UID/EMS/50022/2013 contracts, by the University of Macau Project MYRG2015-00126-FST, and by the Macao Science and Technology Development Fund under Grant FDCT/048/2014/A1. The work of P. Lourenço and B. Guerreiro were supported respectively by the Ph.D. Student Grant SFRH/BD/89337/2012 and by the Post-doc Grant SFRH/BPD/110416/2015 from FCT.


  1. 1.
    Ahmad, A., Huang, S., Wang, J.J., Dissanayake, G.: A new state vector for range-only SLAM. In: Proceedings of the 2011 Chinese Control and Decision Conference (CCDC), pp. 3404–3409 (2011)Google Scholar
  2. 2.
    Bacca, B., Salvi, J., Cufí, X.: Long-term mapping and localization using feature stability histograms. Robot. Auton. Syst. 61(12), 1539–1558 (2013)CrossRefGoogle Scholar
  3. 3.
    Bailey, T.: Mobile Robot Localisation and Mapping in Extensive Outdoor Environments. Ph.D. thesis, University of Sydney, Australian Center of Field Robotics (2002)Google Scholar
  4. 4.
    Bailey, T.: Constrained initialisation for bearing-only SLAM. In: Proceedings of the 2003 IEEE International Conference on Robotics and Automation, vol. 2, pp. 1966–1971. IEEE (2003)Google Scholar
  5. 5.
    Batista, P., Silvestre, C., Oliveira, P.: Single range aided navigation and source localization: observability and filter design. Syst. Control Lett. 60(8), 665–673 (2011)CrossRefzbMATHMathSciNetGoogle Scholar
  6. 6.
    Batista, P., Silvestre, C., Oliveira, P.: Globally exponentially stable filters for source localization and navigation aided by direction measurements. Syst. Control Lett. 62(11), 1065–1072 (2013)CrossRefzbMATHMathSciNetGoogle Scholar
  7. 7.
    Bay, H., Ess, A., Tuytelaars, T., Van Gool, L.: Speeded-up robust features (SURF). Comput. Vis. Image Underst. 110(3), 346–359 (2008)CrossRefGoogle Scholar
  8. 8.
    Bishop, A.N., Jensfelt, P.: A stochastically stable solution to the problem of robocentric mapping. In: IEEE International Conference on Robotics and Automation, 2009. ICRA ’09, pp. 1615–1622 (2009)Google Scholar
  9. 9.
    Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., Reid, I., Leonard, J.J.: Past, present, and future of simultaneous localization and mapping: toward the robust-perception age. IEEE Trans. Robot. 32(6), 1309–1332 (2016)CrossRefGoogle Scholar
  10. 10.
    Castellanos, J.A., Martinez-Cantin, R., Tardós, J.D., Neira, J.: Robocentric map joining: improving the consistency of EKF-SLAM. Robot. Auton. Syst. 55(1), 21–29 (2007)CrossRefGoogle Scholar
  11. 11.
    Csorba, M., Durrant-Whyte, H.F., Uhlmann, J.: A new approach to simultaneous localisation and map building. In: SPIE Aerosense (1996)Google Scholar
  12. 12.
    Davison, A.J., Reid, I.D., Molton, N.D., Stasse, O.: MonoSLAM: real-time single camera SLAM. IEEE Trans. Pattern Anal. Mach. Intell. 29(6), 1052–1067 (2007)CrossRefGoogle Scholar
  13. 13.
    Dissanayake, G., Newman, P., Durrant-Whyte, H.F., Clark, S., Csobra, M.: A solution to the simultaneous localisation and mapping (SLAM) problem. IEEE Trans. Robot. Autom. 17(3), 229–241 (2001)CrossRefGoogle Scholar
  14. 14.
    Djugash, J., Singh, S.: Motion-aided network SLAM with range. Int. J. Robot. Res. 31(5), 604–625 (2012)CrossRefGoogle Scholar
  15. 15.
    Dubbelman, G., Browning, B.: COP-SLAM: closed-form online pose-chain optimization for visual SLAM. IEEE Trans. Robot. 31(5), 1194–1213 (2015)CrossRefGoogle Scholar
  16. 16.
    Durrant-Whyte, H.F.: Uncertain geometry in robotics. IEEE J. Robot. Autom. 4(1), 23–31 (1988)CrossRefGoogle Scholar
  17. 17.
    Gelb, A.: Applied Optimal Estimation. MIT Press, Cambridge (1974)Google Scholar
  18. 18.
    Guerreiro, B.J., Batista, P., Silvestre, C., Oliveira, P.: Globally asymptotically stable sensor-based simultaneous localization and mapping. IEEE Trans. Robot. 29(6), 1380–1395 (2013)CrossRefGoogle Scholar
  19. 19.
    Huang, G., Mourikis, A.I., Roumeliotis, S.I.: Observability-based rules for designing consistent EKF SLAM estimators. Int. J. Robot. Res. 29(5), 502–528 (2010)CrossRefGoogle Scholar
  20. 20.
    Huang, S., Dissanayake, G.: Convergence and consistency analysis for extended Kalman filter based SLAM. IEEE Trans. Robot. 23(5), 1036–1049 (2007)CrossRefGoogle Scholar
  21. 21.
    Jensfelt, P., Kragic, D., Folkesson, J., Bjorkman, M.: A framework for vision based bearing only 3D SLAM. In: Proceedings of the 2006 IEEE International Conference on Robotics and Automation, pp. 1944–1950 (2006)Google Scholar
  22. 22.
    Julier, S.J., Uhlmann, J.K.: A counter example to the theory of simultaneous localization and map building. In: Proceedings of the 2001 IEEE International Conference on Robotics and Automation (ICRA), Seul, South Korea, vol. 4, pp. 4238–4243 (2001)Google Scholar
  23. 23.
    Kelly, J., Sukhatme, G.S.: Visual-inertial sensor fusion: localization, mapping and sensor-to-sensor self-calibration. Int. J. Robot. Res. 30(1), 56–79 (2011)CrossRefGoogle Scholar
  24. 24.
    Lemaire,T., Lacroix, S., Solà, J.: A practical 3D bearing-only SLAM algorithm. In: Proceedings of the 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2449–2454 (2005)Google Scholar
  25. 25.
    Lourenço, P., Batista, P., Oliveira, P., Silvestre, C.: A globally exponentially stable filter for bearing-only simultaneous localization and mapping in 3-D. In: Proceedings of the 2015 European Control Conference, Linz, Austria, pp. 2817–2822 (2015)Google Scholar
  26. 26.
    Lourenço, P., Batista, P., Oliveira, P., Silvestre, C.: Simultaneous localization and mapping in sensor networks: a GES sensor-based filter with moving object tracking. In: Proceedings of the 2015 European Control Conference, Linz, Austria, pp. 2359–2364 (2015)Google Scholar
  27. 27.
    Lourenço, P., Batista, P., Oliveira, P., Silvestre, C., Philip Chen, C.L.: Sensor-based globally exponentially stable range-only simultaneous localization and mapping. Robot. Auton. Syst. 68, 72–85 (2015)CrossRefGoogle Scholar
  28. 28.
    Lourenço, P., Guerreiro, B.J., Batista, P., Oliveira, P., Silvestre, C.: 3-D inertial trajectory and map online estimation: building on a GAS sensor-based SLAM filter. In: Proceedings of the 2013 European Control Conference, Zurich, Switzerland, pp. 4214–4219 (2013)Google Scholar
  29. 29.
    Lourenço, P., Guerreiro, B.J., Batista, P., Oliveira, P., Silvestre, C.: Simultaneous localization and mapping for aerial vehicles: a 3-D sensor-based GAS filter. Auton. Robot. 40, 881–902 (2016)CrossRefGoogle Scholar
  30. 30.
    Lourenço, P., Guerreiro, B.J., Batista, P., Oliveira, P., Silvestre, C.: Uncertainty characterization of the orthogonal procrustes problem with arbitrary covariance matrices. Pattern Recognit. 61, 210–220 (2017)CrossRefGoogle Scholar
  31. 31.
    Montemerlo, M., Thrun, S., Koller, D., Wegbreit, B.: FastSLAM: a factored solution to the simultaneous localization and mapping problem. In: Eighteenth National Conference on Artificial Intelligence, pp. 593–598. American Association for Artificial Intelligence (2002)Google Scholar
  32. 32.
    Mur-Artal, R., Montiel, J.M.M., Tardós, J.D.: ORB-SLAM: a versatile and accurate monocular SLAM system. IEEE Trans. Robot. 31(5), 1147–1163 (2015)CrossRefGoogle Scholar
  33. 33.
    Neira, J., Tardós, J.D.: Data association in stochastic mapping using the joint compatibility test. IEEE Trans. Robot. Autom. 17(6), 890–897 (2001)CrossRefGoogle Scholar
  34. 34.
    Smith, R.C., Cheeseman, P.: On the representation and estimation of spatial uncertainty. Int. J. Robot. Res. 5(4), 56–68 (1986)Google Scholar
  35. 35.
    Smith, R.C., Self, M., Cheeseman, P.: Estimating uncertain spatial relationships in robotics. pp. 167–193. Springer, New York (1990)Google Scholar
  36. 36.
    Solà, J., Monin, A., Devy, M., Lemaire, T.: Undelayed initialization in bearing only SLAM. In: Proceedings of the 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2499–2504 (2005)Google Scholar
  37. 37.
    Thrun, S., Liu, Y., Koller, D., Ng, A., Durrant-Whyte, H.: Simultaneous localization and mapping with sparse extended information filters. Int. J. Robot. Res. 23(7–8), 693–716 (2004)CrossRefGoogle Scholar
  38. 38.
    Weiss, L., Sanderson, A.C., Neuman, C.P.: Dynamic sensor-based control of robots with visual feedback. IEEE J. Robot. Autom. 3(5), 404–417 (1987)CrossRefGoogle Scholar

Copyright information

© Springer International Publishing AG 2017

Authors and Affiliations

  • Pedro Lourenço
    • 1
  • Bruno Guerreiro
    • 1
  • Pedro Batista
    • 1
    • 2
    Email author
  • Paulo Oliveira
    • 3
  • Carlos Silvestre
    • 4
  1. 1.Laboratory for Robotics and Engineering SystemsInstitute for Systems and RoboticsLisbonPortugal
  2. 2.Department of Electrical and Computer EngineeringInstituto Superior Técnico, Universidade de LisboaLisbonPortugal
  3. 3.Department of Mechanical EngineeringInstituto Superior Técnico, Universidade de LisboaLisbonPortugal
  4. 4.Department of Electrical and Computer Engineering of the Faculty of Science and TechnologyUniversity of MacauMacaoChina

Personalised recommendations