Autonomous Robots

, Volume 40, Issue 5, pp 881–902 | Cite as

Simultaneous localization and mapping for aerial vehicles: a 3-D sensor-based GAS filter

  • Pedro Lourenço
  • Bruno J. Guerreiro
  • Pedro Batista
  • Paulo Oliveira
  • Carlos Silvestre


This paper presents the design, analysis, and experimental validation of a globally asymptotically stable (GAS) filter for simultaneous localization and mapping (SLAM) with application to unmanned aerial vehicles. The main contributions of this paper are the results of global convergence and stability for SLAM in tridimensional (3-D) environments. The SLAM problem is formulated in a sensor-based framework and modified in such a way that the structure may be regarded as linear time-varying for observability purposes, from which a Kalman filter with GAS error dynamics follows naturally. The proposed solution includes the estimation of both body-fixed linear velocity and rate gyro measurement biases. Experimental results from several runs, using an instrumented quadrotor equipped with a RGB-D camera, are included in the paper to illustrate the performance of the algorithm under realistic conditions.


Simultaneous localization and mapping Aerial robotics  3-D mapping Sensor fusion RGB-D camera 



The authors would like to thank everyone involved in the development of the software and hardware used in the data acquisition for this paper, namely André Oliveira and Bruno Cardeira of the DSOR Lab and David Cabecinhas of the SCORE Lab. 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 was supported by the PhD Student 1136 Grant SFRH/BD/89337/2012 from FCT.


  1. Anderson, B. D. O. (1971). Stability properties of Kalman–Bucy filters. Journal of the Franklin Institute, 291(2), 137–144.MathSciNetCrossRefzbMATHGoogle Scholar
  2. Bailey, T., & Durrant-Whyte, H. (2006). Simultaneous localization and mapping (SLAM): Part II. IEEE Robotics & Automation Magazine, 13(3), 108–117.CrossRefGoogle Scholar
  3. Bailey, T., Nieto, J., Guivant, J., Stevens, M., & Nebot, E. (2006, Oct.). Consistency of the EKFSLAM algorithm. In 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp. 3562–3568).Google Scholar
  4. Batista, P., Silvestre, C., & Oliveira, P. (2010). Optimal position and velocity navigation filters for autonomous vehicles. Automatica, 46(4), 767–774.MathSciNetCrossRefzbMATHGoogle Scholar
  5. Batista, P., Silvestre, C., & Oliveira, P. (2011a). On the observability of linear motion quantities in navigation systems. Systems & Control Letters, 60(2), 101–110.MathSciNetCrossRefzbMATHGoogle Scholar
  6. Batista, P., Silvestre, C., & Oliveira, P. (2011b). Single range aided navigation and source localization: Observability and filter design. Systems & Control Letters, 60(8), 665–673.MathSciNetCrossRefzbMATHGoogle Scholar
  7. Bay, H., Ess, A., Tuytelaars, T., & Gool, L. V. (2008). Speeded-up robust features (SURF). Computer Vision and Image Understanding, 110(3), 346–359.CrossRefGoogle Scholar
  8. Bishop, A., & Jensfelt, P. (2009, May). A stochastically stable solution to the problem of robocentric mapping. In Proceedings of the IEEE International Conference on Robotics and Automation (pp. 1615–1622). Kobe.Google Scholar
  9. Brockett, R. (1970). Finite dimensional linear systems. New York: Wiley.zbMATHGoogle Scholar
  10. Castellanos, J., Martinez-Cantin, R., Tardós, J., & Neira, J. (2007). Robocentric map joining: Improving the consistency of EKF-SLAM. Robotics and Autonomous Systems, 55(1), 21–29.CrossRefGoogle Scholar
  11. Davison, A., Reid, I., Molton, N., & Stasse, O. (2007). Monoslam: Real-time single camera slam. IEEE Transactions on Pattern Analysis and Machine Intelligence, 29(6), 1052–1067.CrossRefGoogle Scholar
  12. Durrant-Whyte, H., & Bailey, T. (2006). Simultaneous localisation and mapping (SLAM): Part I the essential algorithms. IEEE Robotics & Automation Magazine, 13(2), 99–110.CrossRefGoogle Scholar
  13. Endres, F., Hess, J., Engelhard, N., Sturm, J., Cremers, D., & Burgard, W. (2012, May). An evaluation of the RGB-D SLAM system. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA) (pp. 1691–1696). St. Paul, MA.Google Scholar
  14. Gelb, A. (1974). Applied optimal estimation. Cambridge: MIT Press.Google Scholar
  15. Guerreiro, B. J., Batista, P., Silvestre, C., & Oliveira, P. (2012, June). Sensor-based simultaneous localization and mapping—part II: Online inertial map and trajectory estimation. In Proceedings of the 2012 American Control Conference (pp. 6334–6339). Montréal.Google Scholar
  16. Guerreiro, B. J., Batista, P., Silvestre, C., & Oliveira, P. (2013). Globally asymptotically stable sensor-based simultaneous localization and mapping. IEEE Transactions on Robotics, 29(6), 1380–1395.CrossRefGoogle Scholar
  17. Hermann, R., & Krener, A. J. (1977). Nonlinear controllability and observability. IEEE Transactions on Automatic Control, 22(5), 728–740.MathSciNetCrossRefzbMATHGoogle Scholar
  18. Huang, G., Mourikis, A. I., & Roumeliotis, S. I. (2010). Observability-based rules for designing consistent EKF SLAM estimators. The International Journal of Robotics Research, 29(5), 502–528.CrossRefGoogle Scholar
  19. Huang, S., & Dissanayake, G. (2007). Convergence and consistency analysis for extended Kalman filter based SLAM. IEEE Transactions on Robotics, 23(5), 1036–1049.CrossRefGoogle Scholar
  20. Jones, E., & Soatto, S. (2011). Visual-inertial navigation, mapping and localization: A scalable realtime causal approach. The International Journal of Robotics Research, 30(4), 407–430.Google Scholar
  21. Julier, S., & Uhlmann, J. (2001, May). 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) (Vol. 4, pp. 4238–4243). SeoulGoogle Scholar
  22. Kelly, J., & Sukhatme, G. S. (2011). Visual-inertial sensor fusion: Localization, mapping and sensor-tosensor self-calibration. The International Journal of Robotics Research, 30(1), 56–79.CrossRefGoogle Scholar
  23. Kermorgant, O., & Chaumette, F. (2011, May). Multisensor data fusion in sensor-based control: Application to multi-camera visual servoing. In Proceedings of the IEEE International Conference on Robotics and Automation (pp. 4518–4523). doi: 10.1109/ICRA.2011.5979715
  24. Khalil, H. (2002). Nonlinear systems (3rd ed.). Upper Saddle River: Prentice Hall.zbMATHGoogle Scholar
  25. Leutenegger, S., Furgale, P., Rabaud, V., Chli, M., Konolige, K., & Siegwart, R. (2013, June). Keyframe-based visual-inertial slam using nonlinear optimization. In Proceedings of Robotics: Science and Systems. Berlin.Google Scholar
  26. Lourenço, P., Guerreiro, B. J., Batista, P., Oliveira, P., & Silvestre, C. (2013a, July). 3-D inertial trajectory and map online estimation: Building on a GAS sensor-based SLAM filter. In Proceedings of the 2013 European Control Conference (pp. 4214–4219). Zurich.Google Scholar
  27. Lourenço, P., Guerreiro, B. J., Batista, P., Oliveira, P., & Silvestre, C. (2013b, June). Preliminary results on globally asymptotically stable simultaneous localization and mapping in 3-D. In Proceedings of the (2013) American Control Conference (pp. 3093–3098). Washington D.C.Google Scholar
  28. May, S., Droeschel, D., Holz, D., Fuchs, S., Malis, E., Nüchter, A., et al. (2009). Three-dimensional mapping with time-of-flight cameras. Journal of Field Robotics, 26(11–12), 934–965.CrossRefGoogle Scholar
  29. Schönemann, P. H. (1966). A generalized solution of the orthogonal procrustes problem. Psychometrika, 31(1), 1–10.MathSciNetCrossRefzbMATHGoogle Scholar
  30. Se, S., Lowe, D., & Little, J. (2002). Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks. The International Journal of Robotics Research, 21(8), 735–758.CrossRefGoogle Scholar
  31. Umeyama, S. (1991). Least-squares estimation of transformation parameters between two point patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence, 13(4), 376–380.CrossRefGoogle Scholar
  32. Victorino, A., Rives, P., & Borrelly, J.-J. (2000). Localization and map building using a sensorbased control strategy. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (Vol. 2, pp. 937–942). doi: 10.1109/IROS.2000.893139
  33. Weiss, L., Sanderson, A. C., & Neuman, C. P. (1987). Dynamic sensor-based control of robots with visual feedback. IEEE Journal on Robotics and Automation, 3(5), 404–417.CrossRefGoogle Scholar
  34. Weiss, S., Achtelik, M. W., Lynen, S., Achtelik, M. C., Kneip, L., Chli, M., et al. (2013). Monocular vision for long-term micro aerial vehicle state estimation: A compendium. Journal of Field Robotics, 30(5), 803–831.CrossRefGoogle Scholar

Copyright information

© Springer Science+Business Media New York 2015

Authors and Affiliations

  • Pedro Lourenço
    • 1
  • Bruno J. Guerreiro
    • 2
    • 4
  • Pedro Batista
    • 1
    • 2
  • Paulo Oliveira
    • 1
    • 3
    • 5
  • Carlos Silvestre
    • 1
    • 4
  1. 1.Institute for Systems and RoboticsLaboratory of Robotics and Engineering SystemsLisbonPortugal
  2. 2.Instituto Superior TécnicoUniversidade de LisboaLisbonPortugal
  3. 3.LAETA - Associated Laboratory for Energy, Transports and AeronauticsInstitute of Mechanical EngineeringLisbonPortugal
  4. 4.Department of Electrical and Computer Engineering, Faculty of Science and TechnologyUniversity of MacauMacauChina
  5. 5.Institute of Mechanical Engineering, Associated Laboratory for EnergyTransports and AeronauticsLisbonPortugal

Personalised recommendations