← Back ICRA 2026

InsSo3D: Inertial Navigation System and 3D Sonar SLAM for Turbid Environment Inspection

Simon Archieri, AHMET FATIH CINAR, Shu Pan, Scharff Willners Jonatan, Michele Grimaldi, Ignacio Carlucho, Yvan R. Petillot

PDF

AI summary

Key figure (auto-extracted from paper)

InsSo3D achieves sub-25cm trajectory accuracy and large-scale 3D mapping in murky waters by fusing 3D sonar with inertial navigation and robust loop closure detection.

3D Sonar SLAM Inertial Navigation Underwater Mapping Turbid Environments Pose Graph Optimization

Problem

Traditional underwater SLAM relies on optical sensors that fail in turbid conditions or at long ranges, while 2D sonar approaches suffer from elevation ambiguity and uncorrected odometry drift, hindering accurate 3D mapping.

Approach

The method fuses real-time 3D sonar point clouds with inertial navigation data, using a CFEAR-based registration algorithm and a two-stage frontend-backend framework with loop closure detection to correct drift and build consistent global maps.

Key results

Average trajectory error consistently below 21 cm during 50-minute missions
Reconstructed a 20m × 10m environment with 9 cm average map error
Robust loop closure detection and global alignment using CFEAR registration
Validated in test tanks and flooded quarries against ground truth and visual SFM

Why it matters

Enables reliable, large-scale underwater inspection and navigation for AUVs in visually degraded environments where optical systems fail.

Abstract

This paper presents InsSo3D, an accurate and ef- ficient method for large-scale 3D Simultaneous Localisation and Mapping (SLAM) using a 3D Sonar and an Inertial Navigation System (INS). Unlike traditional sonar, which produces 2D images containing range and azimuth information but lacks elevation information, 3D Sonar produces a 3D point cloud, which therefore does not suffer from elevation ambiguity. We introduce a robust and modern SLAM framework adapted to the 3D Sonar data using INS as prior, detecting loop closure and performing pose graph optimisation. We evaluated InsSo3D performance inside a test tank with access to ground truth data and in an outdoor flooded quarry. Comparisons to reference trajectories and maps obtained from an underwater motion tracking system and visual Structure From Motion (SFM) demonstrate that InsSo3D efficiently corrects odometry drift. The average trajectory error is below 21cm during a 50- minute-long mission, producing a map of 10m by 20m with a 9cm average reconstruction error, enabling safe inspection of natural or artificial underwater structures even in murky water conditions. SLAM, Marine Robotics, Mapping, Sonar

Index terms

Marine Robotics SLAM Range Sensing