Simultaneous Localization and Mapping in Marine Environments

  • Maurice F. FallonEmail author
  • Hordur Johannsson
  • Michael Kaess
  • John Folkesson
  • Hunter McClelland
  • Brendan J. Englot
  • Franz S. Hover
  • John J. Leonard


Accurate navigation is a fundamental requirement for robotic systems—marine and terrestrial. For an intelligent autonomous system to interact effectively and safely with its environment, it needs to accurately perceive its surroundings. While traditional dead-reckoning filtering can achieve extremely low drift rates, the localization accuracy decays monotonically with distance traveled. Other approaches (such as external beacons) can help; nonetheless, the typical prerogative is to remain at a safe distance and to avoid engaging with the environment. In this chapter we discuss alternative approaches which utilize onboard sensors so that the robot can estimate the location of sensed objects and use these observations to improve its own navigation as well as its perception of the environment. This approach allows for meaningful interaction and autonomy. Three motivating autonomous underwater vehicle (AUV) applications are outlined herein. The first fuses external range sensing with relative sonar measurements. The second application localizes relative to a prior map so as to revisit a specific feature, while the third builds an accurate model of an underwater structure which is consistent and complete. In particular we demonstrate that each approach can be abstracted to a core problem of incremental estimation within a sparse graph of the AUV’s trajectory and the locations of features of interest which can be updated and optimized in real time on board the AUV.


Extended Kalman Filter Autonomous Underwater Vehicle Mooring Line Acoustic Modem Doppler Velocity Logger 
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.



The work described in this chapter was partially supported by the Office of Naval Research under grants N00014-05-10244, N00014-11-10119, N00014-07-11102, N00014-06-10043, N00014-10-10936, and N00014-12-10020, and by the MIT Sea Grant College Program under research grant 2007-R/RCM-20. We wish to acknowledge the contributions of Michael Benjamin, Joseph Curcio, Georgios Papadopoulos, and Andrew Patrikalakis at MIT. Each project had a number of commercial and academic partners which we also wish to acknowledge including Ryan Eustice and Ayoung Kim (University of Michigan); Doug Horner and Tad Mazek (Naval Postgraduate School); Ed Matson and Bryan Schulz (iRobot); Jerome Vaganay, Kim Shurn, and Mike Elkins (Bluefin Robotics); Scott Reed, Alastair Cormack, and Jose Vasquez (SeeByte); and Dan Kucik and Paul Carroll (Naval Surface Warfare Center-Panama City).


  1. 1.
    Bahr A (2009) Cooperative Localization for Autonomous Underwater Vehicles. PhD thesis, Massachusetts Institute of Technology, Cambridge, MA, USA.Google Scholar
  2. 2.
    Bahr A, Leonard J, Fallon M (2009) Cooperative localization for autonomous underwater vehicles. Intl. J. Robotics. Res. 28(6):714–728Google Scholar
  3. 3.
    Ballard RD, Stager LE, Master D, Yoerger D, Mindell D, Whitcomb, LL, Singh H, Piechota D (2002) Iron Age shipwrecks in deep water off Ashkelon, Israel. Am J Archaeol 106(2):151–168Google Scholar
  4. 4.
    Belcher E, Hanot W, Burch J (2002) Dual-frequency identification sonar (DIDSON). In: Underwater Technology, Proceedings of the International Symposium on, p 187–192Google Scholar
  5. 5.
    Biber P, Strasser W (2003) The normal distributions transform: a new approach to laser scan matching. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), vol 3, pp 2743–2748Google Scholar
  6. 6.
    Bosse M, Newman P, Leonard J, Teller S (2004) Simultaneous localization and map building in large-scale cyclic environments using the Atlas framework. Intl J Robotic Res 23(12): 1113–1139Google Scholar
  7. 7.
    Clark DE, Bell J (2005) Bayesian multiple target tracking in forward scan sonar images using the PHD filter. IEE Radar, Sonar and Navigation 152:327–334Google Scholar
  8. 8.
    Cummins M, Newman P (2007) Probabilistic appearance based navigation and loop closing. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), pp 2042–2048Google Scholar
  9. 9.
    Dellaert F, Kaess M (2006) Square Root SAM: Simultaneous localization and mapping via square root information smoothing. Intl J Robotics Res 25(12):1181–1203Google Scholar
  10. 10.
    Duckett T, Marsland S, Shapiro J (2002) Fast, on-line learning of globally consistent maps. Auton Robotics 12(3):287–300Google Scholar
  11. 11.
    Durrant-Whyte H, Bailey T (2006) Simultaneous localisation and mapping (SLAM): Part I. Robotics Automat Mag 13(2):99 –110Google Scholar
  12. 12.
    Edgerton HE (1986) Sonar images. Prentice-Hall, Englewood CliffsGoogle Scholar
  13. 13.
    Englot B, Johannsson H, Hover F (2009) Perception, stability analysis, and motion planning for autonomous ship hull inspection. In: Proceedings of the Int. Symp. on Unmanned Untethered Submersible Technology (UUST)Google Scholar
  14. 14.
    Eustice R (2005) Large-Area Visually Augmented Navigation for Autonomous Underwater Vehicles. PhD thesis, Massachusetts Institute of Technology, Cambridge, MA, USA.Google Scholar
  15. 15.
    Eustice R, Singh H, Leonard J (2006). Exactly sparse delayed-state filters for view-based SLAM. IEEE Trans. Robotics., 22(6):1100–1114Google Scholar
  16. 16.
    Eustice R, Singh H, Leonard J, Walter M, Ballard R (2005) Visually navigating the RMS Titanic with SLAM information filters. In: Robotics: Science and Systems (RSS), pp 3281–3288Google Scholar
  17. 17.
    Eustice RM, Whitcomb LL, Singh H, Grund M (2007) Experimental results in synchronous-clock one-way-travel-time acoustic navigation for autonomous underwater vehicles. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), pp 4257–4264, Rome, ItalyGoogle Scholar
  18. 18.
    Fallon M, Kaess M, Johannsson H, Leonard J (2011) Efficient AUV navigation fusing acoustic ranging and side-scan sonar. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), Shanghai, ChinaGoogle Scholar
  19. 19.
    Fallon M, Papadopoulos G, Leonard J (2009) Cooperative AUV navigation using a single surface craft. In: Field and Service RoboticsGoogle Scholar
  20. 20.
    Fallon MF, Folkesson J, McClelland H, Leonard JJ (2012). Relocating underwater features autonomously using sonar-based SLAM. IEEE J. Ocean Engineering. To AppearGoogle Scholar
  21. 21.
    Fallon MF, Papadopoulos G, Leonard, JJ (2010) A measurement distribution framework for cooperative navigation using multiple AUVs. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), pp 4803–4808Google Scholar
  22. 22.
    Fallon MF, Papadopoulos G, Leonard JJ, Patrikalakis NM (2010) Cooperative AUV navigation using a single maneuvering surface craft. Intl J Robotics Res 29(12):1461–1474Google Scholar
  23. 23.
    Folkesson J, Leonard J (2009) Autonomy through SLAM for an underwater robot. In: Proc. of the Intl. Symp. of Robotics Research (ISRR) 70:55–70Google Scholar
  24. 24.
    Freitag L, Grund M, Singh S, Partan J, Koski P, Ball K (2005) The WHOI micro-modem: An acoustic communications and navigation system for multiple platforms. In: Proceedings of the IEEE/MTS OCEANS Conference, vol  1, pp 1086–1092Google Scholar
  25. 25.
    Geyer E, Creamer P, D’Appolito J, Gains R (1987). Characteristics and capabilities of navigation systems for unmanned untethered submersibles. In: Proc. Int. Symp. on Unmanned Untethered Submersible Technology, pp 320–347Google Scholar
  26. 26.
    Golub G, Loan CV (1996) Matrix computations, 3rd edn. Johns Hopkins University Press, BaltimoreGoogle Scholar
  27. 27.
    Grisetti G, Kümmerle R, Stachniss C, Frese U, Hertzberg C (2010) Hierarchical optimization on manifolds for online 2D and 3D mapping. In :IEEE Intl. Conf. on Robotics and Automation (ICRA), Anchorage, AlaskaGoogle Scholar
  28. 28.
    Grisetti G, Stachniss C, Grzonka S, Burgard W (2007) A tree parameterization for efficiently computing maximum likelihood maps using gradient descent. In: Robotics: Science and Systems (RSS).Google Scholar
  29. 29.
    Harris S, Slate E (1999) Lamp Ray: Ship hull assessment for value, safety and readiness. In: Proceedings of the IEEE/MTS OCEANS Conference, vol 1, pp 493 –500Google Scholar
  30. 30.
    Heckman DB, Abbott RC (1973) An acoustic navigation technique. In: Proceedings of the IEEE/MTS OCEANS Conference, pp 591–595Google Scholar
  31. 31.
    Horner DP, McChesney N, Masek T, Kragelund SP (2009) 3D reconstruction with an AUV mounted forward looking sonar. In: Proc. Int. Symp. on Unmanned Untethered Submersible Technology, pp 1464–1470Google Scholar
  32. 32.
    Hover FS, Eustice RM, Kim A, Englot B, Johannsson H, Kaess M, Leonard JJ (2012) Advanced perception, navigation and planning for autonomous in-water ship hull inspection. Intl J Robotics Res. To AppearGoogle Scholar
  33. 33.
    Hunt M, Marquet W, Moller D, Peal K, Smith W, Spindel R (1974) An acoustic navigation system. Technical Report WHOI-74-6, Woods Hole Oceanographic Institution.Google Scholar
  34. 34.
    Johannsson H, Kaess M, Englot B, Hover F, Leonard J (2010) Imaging sonar-aided navigation for autonomous underwater harbor surveillance. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Taipei, TaiwanGoogle Scholar
  35. 35.
    Julier S, Uhlmann J (2001) A counter example to the theory of simultaneous localization and map building. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), vol  4, pp 4238–4243Google Scholar
  36. 36.
    Kaess, M Dellaert F (2009) Covariance recovery from a square root information matrix for data association. J RoboticS Auton Syst 57(12):1198–1210Google Scholar
  37. 37.
    Kaess M, Ila V, Roberts R, Dellaert F (2010) The Bayes tree: An algorithmic foundation for probabilistic robot mapping. In: Intl. Workshop on the Algorithmic Foundations of Robotics, WAFR, SingaporeGoogle Scholar
  38. 38.
    Kaess M, Johannsson H, Roberts R, Ila V, Leonard JJ, Dellaert, F (2012) iSAM2: Incremental smoothing and mapping using the Bayes tree. IntL J RoboticS Res 31:217–236Google Scholar
  39. 39.
    Kaess M, Ranganathan A, Dellaert F (2008) iSAM: Incremental smoothing and mapping. IEEE Trans Robotics 24(6):1365–1378Google Scholar
  40. 40.
    Kim A, Eustice R (2009) Pose-graph visual SLAM with geometric model selection for autonomous underwater ship hull inspection. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)Google Scholar
  41. 41.
    Kim A, Eustice RM (2011) Combined visually and geometrically informative link hypothesis for pose-graph visual SLAM using bag-of-words. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), San Francisco, CA, pp 1647–1654Google Scholar
  42. 42.
    Kinsey JC, Eustice RM, Whitcomb LL (2006) A survey of underwater vehicle navigation: Recent advances and new challenges. In: IFAC Conference of Manoeuvering and Control of Marine Craft, Lisbon, Portugal. Invited paperGoogle Scholar
  43. 43.
    Konolige K (2004) Large-scale map-making. In: AAAI’04: Proceedings of the 19th national conference on Artificial intelligence, pp 457–463. AAAI Press/The MIT PressGoogle Scholar
  44. 44.
    Konolige K, Grisetti G, Kummerle R, Burgard W, Limketkai B, Vincent R (2010) Efficient sparse pose adjustment for 2D mapping. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Taipei, Taiwan, pp 22–29Google Scholar
  45. 45.
    Kümmerle R, Grisetti G, Strasdat H, Konolige K, Burgard W (2011) g2o: A general framework for graph optimization. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), Shanghai, ChinaGoogle Scholar
  46. 46.
    Leonard JJ, Bennett AA, Smith CM, Feder HJS (1998) Autonomous underwater vehicle navigation. Technical Report Marine Robotics Laboratory Technical Memorandum 98–1, MITGoogle Scholar
  47. 47.
    Lu F, Milios E (1997) Globally consistent range scan alignment for environmental mapping. Auton Robotics 4:333–349Google Scholar
  48. 48.
    Mandt M, Gade K, Jalving, B (2001) Integrating DGPS-USBL positioning measurements with inertial navigation in the HUGIN 3000 AUV. In: International Conference on Integrated Navigation Systems, Saint Petersburg, RussiaGoogle Scholar
  49. 49.
    Mahon I, Williams S, Pizarro O, Johnson-Roberson M (2008) Efficient view-based SLAM using visual loop closures. IEEE Trans Robotic 24(5):1002–1014Google Scholar
  50. 50.
    Mallios A, Ridao P, Hernandez E, Ribas D, Maurelli F, Petillot Y (2009) Pose-based SLAM with probabilistic scan matching algorithm using a mechanical scanned imaging sonar. In: Proceedings of the IEEE/MTS OCEANS ConferenceGoogle Scholar
  51. 51.
    Milne PH (1983) Underwater acoustic positioning systems. E. F. N. Spon, LondonGoogle Scholar
  52. 52.
    Mindell D (2007) Precision navigation and remote sensing for underwater archaeology. Remote sensing in archaeology, p 499Google Scholar
  53. 53.
    Montemerlo M, Thrun S, Roller D, Wegbreit B (2003) FastSLAM 2.0: An improved particle filtering algorithm for simultaneous localization and mapping that provably converges. In: Intl. Joint Conf. on Artificial Intelligence, Morgan Kaufmann, pp 1151–1156Google Scholar
  54. 54.
    Moutarlier P, Chatila R (1990) An experimental system for incremental environment modeling by an autonomous mobile robot. In: Intl. Sym. on Experimental Robotics (ISER).Google Scholar
  55. 55.
    Negahdaripour S, Firoozfam P, Sabzmeydani P (2005) On processing and registration of forward-scan acoustic video imagery. In: Computer and Robot Vision, 2005. Proceedings. The 2nd Canadian Conference on, pp 452–459Google Scholar
  56. 56.
    Negahdaripour S, Sekkati H, Pirsiavash H (2009) Opti-acoustic stereo imaging: On system calibration and 3-D target reconstruction. IEEE Transactions on Image Processing:Google Scholar
  57. 57.
    Neira J, Tardos J, Castellanos J (2003) Linear time vehicle relocation in SLAM. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), pp 427–433Google Scholar
  58. 58.
    Neira J, Tardos JD (2001) Data association in stochastic mapping using the joint compatibility test. IEEE Trans Robotic Autom 17(6):890–897Google Scholar
  59. 59.
    Nuske S, Roberts J, Prasser D, Wyeth G (2010) Experiments in visual localisation around underwater structures. In: Field and Service Robotics. Springer Tracts in Advanced Robotics, vol 62. Springer, Berlin, pp 295–304Google Scholar
  60. 60.
    Olson E, Leonard J, Teller S (2006) Fast iterative alignment of pose graphs with poor initial estimates. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), pp 2262–2269Google Scholar
  61. 61.
    Paz L, Guivant J, Tardós J, Neira J (2007) Data association in O(n) for divide and conquer SLAM. In: Robotics: Science and Systems, RSS, Atlanta, GA, USAGoogle Scholar
  62. 62.
    Petillot Y, Ruiz IT, Lane DM (2001) Underwater vehicle obstacle avoidance and path planning using a multi-beam forward looking sonar. J Ocean Eng 26:240–251Google Scholar
  63. 63.
    Ranganathan A, Kaess M, Dellaert F (2007) Loopy SAM. In: Intl. Joint Conf. on Artificial Intelligence (IJCAI), Hyderabad, India, pp 2191–2196Google Scholar
  64. 64.
    Ribas D, Ridao P, Neira J, Tardós, J (2006) SLAM using an imaging sonar for partially structured underwater environments. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS)Google Scholar
  65. 65.
    Rosen D, Kaess M, Leonard J (2012) An incremental trust-region method for robust online sparse least-squares estimation. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), St. Paul, MN, pp 1262–1269Google Scholar
  66. 66.
    Schulz B, Hughes R, Matson R, Moody R, Hobson B (2005) The development of a free-swimming UUV for mine neutralization. In: Proceedings of the IEEE/MTS OCEANS Conference, IEEE, pp 1443–1447Google Scholar
  67. 67.
    Sekkati H, Negahdaripour S (2007) 3-D motion estimation for positioning from 2-D acoustic video imagery. Lecture Notes in Computer Science, 4478:80Google Scholar
  68. 68.
    Smith R, Self M, Cheeseman P (1990) Estimating uncertain spatial relationships in robotics. In: Autonomous Robot Vehicles. Springer, pp 167–193Google Scholar
  69. 69.
    Tena Ruiz I, de Raucourt S, Petillot Y, Lane D (2004) Concurrent mapping and localization using sidescan sonar. J Ocean Eng 29(2):442–456Google Scholar
  70. 70.
    Thompson R, Zehner W (1999) Frequency-steered acoustic beam forming system and process. US Patent 5,923,617Google Scholar
  71. 71.
    Thrun S, Beetz M, Bennewitz M, Burgard W, Cremers A, Dellaert F, Fox D, Hahnel D, Rosenberg C, Roy N, Schulte J, Schulz D (2000) Probabilistic algorithms and the interactive museum tour-guide robot minerva. Intl J Robotics Res 19(11):972–999Google Scholar
  72. 72.
    Thrun S, Burgard W, Fox D (2005) Probabilistic robotics. The MIT press, CambridgeGoogle Scholar
  73. 73.
    Thrun S, Liu Y, Koller D, Ng A.Y., Ghahramani Z., Durrant-Whyte H (2004) Simultaneous localization and mapping with sparse extended information filters. Intl J Robotics Res, 23(7):693–716Google Scholar
  74. 74.
    Vaganay J, Elkins M, Esposito D, O’Halloran W, Hover F, Kokko M (2007) Ship hull inspection with the HAUV: US Navy and NATO demonstrations results. In: Proceedings of the IEEE/MTS OCEANS Conference and Exhibition, vol  1, pp 761–766Google Scholar
  75. 75.
    Vaganay J, Leonard J, Curcio J, Willcox J (2004) Experimental validation of the moving long base line navigation concept. In: Autonomous Underwater Vehicles, 2004 IEEE/OES, pp 59–65Google Scholar
  76. 76.
    von Alt C, Allen B, Austin T, Stokey R (1994) Remote environmental monitoring units. In: AUV 94Google Scholar
  77. 77.
    Walls JM, Eustice RM (2011) Experimental comparison of synchronous-clock cooperative acoustic navigation algorithms. In: Proceedings of the IEEE/MTS OCEANS Conference, Kona, HI, USA.Google Scholar
  78. 78.
    Walter M (2008) Sparse Bayesian information filters for localization and mapping. PhD thesis, Massachusetts Institute of Technology.Google Scholar
  79. 79.
    Walter M, Hover F, Leonard J (2008) SLAM for ship hull inspection using exactly sparse extended information filters. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), pp 1463–1470Google Scholar
  80. 80.
    Webster SE (2010) Decentralized single-beacon acoustic navigation: combined communication and navigation for underwater vehicles. PhD thesis, Johns Hopkins University, Baltimore, MD, USA.Google Scholar
  81. 81.
    Webster SE, Whitcomb LL, Eustice RM (2010) Preliminary results in decentralized estimation for single-beacon acoustic underwater navigation. In: Robotics: Science and Systems (RSS), Zaragoza, Spain.Google Scholar
  82. 82.
    Whitcomb L, Yoerger D, Singh H (1999a) Advances in doppler-based navigation of underwater robotic vehicles. In: IEEE Intl. Conf. on Robotics and Automation (ICRA), vol 1, pp 399–406Google Scholar
  83. 83.
    Whitcomb L, Yoerger D, Singh H (1999b) Doppler/LBL based navigation of underwater vehicles. In: Proceedings of the Intl Symp on Unmanned Untethered Submersible Technology (UUST)Google Scholar
  84. 84.
    Whitcomb L, Yoerger D, Singh H, Mindell D(1998) Towards precision robotic maneuvering, survey, and manipulation in unstructured undersea environments. In: Proc. of the Intl. Symp. of Robotics Research (ISRR), vol 8, Springer, London, pp 45–54Google Scholar
  85. 85.
    Yoerger D, Jakuba M, Bradley A, Bingham B (2007) Techniques for deep sea near bottom survey using an autonomous underwater vehicle. Intl J Robotics Res 416–429Google Scholar
  86. 86.
    Yoerger D, Mindell D (1992) Precise navigation and control of an ROV at 2200 meters depth. In: Proceedings of Intervention/ROV, vol  92Google Scholar

Copyright information

© Springer Science+Business Media New York 2013

Authors and Affiliations

  • Maurice F. Fallon
    • 1
    Email author
  • Hordur Johannsson
    • 1
  • Michael Kaess
    • 1
  • John Folkesson
    • 2
  • Hunter McClelland
    • 3
  • Brendan J. Englot
    • 4
  • Franz S. Hover
    • 4
  • John J. Leonard
    • 1
  1. 1.Computer Science and Artificial Intelligence LaboratoryMassachusetts Institute of TechnologyCambridgeUSA
  2. 2.Center for Autonomous Systems, Kungliga Tekniska Högskolan (KTH)StockholmSweden
  3. 3.Robotics and Mechanisms LaboratoryVirginia Polytechnic Institute of Technology and State University (Virginia Tech)BlacksburgUSA
  4. 4.Department of Mechanical EngineeringMassachusetts Institute of TechnologyCambridgeUSA

Personalised recommendations