PaperlayerView exact revision on arXiv

Source-linked AI summary

SVIn+: Multi-Modal Framework for Robust Underwater State Estimation using 3D Sonar, Visual-Inertial, Water-Pressure, and DVL

Chinmay Burgul, Xiaoming Zhao, Yewei Huang, Michalis Chatzispyrou, Marios Xanthidis, Ioannis Rekleitis, Alberto Quattrini Li

arXiv:2610.36454v1cs.RO

TL;DR

Underwater state estimation is difficult in low-visibility settings, where visual tracking can fail and onboard sensors leave drift and environmental-context gaps. SVIn+ fuses visual-inertial, pressure, DVL, and 3D sonar information; its reported evaluation shows reduced drift, including lower mean error on BajanQueen-01.

  • Problem

    Underwater state estimation remains challenging because visual-inertial odometry can lose features in turbid water or motion blur, while pressure and DVL lack environmental context for loop closures.

    Source: §I · p. 1

  • Method

    SVIn+ jointly optimizes visual-inertial, water-pressure, and DVL measurements while incorporating relative pose factors from KISS-ICP processing of 3D sonar scans.

    Source: §I · p. 1, §IV · p. 3, C. 3D Sonar Fusion · p. 3

  • Results

    On BajanQueen-01, SVIn+ yields a mean error of 0.21 m, a 60.4% overall error reduction from the SVIn2 baseline mean error of 0.53 m.

    Source: E. Ablation study · p. 6

  • Takeaways & Limitations

    3D sonar produces rich environmental maps that can support subsequent path planning and autonomous tasks, in addition to enhancing localization.

    Source: E. Ablation study · p. 7

Abstract

from arXiv · show

As expansion and scalability in underwater oper- ations becomes more important and challenging, autonomous underwater vehicles (AUVs) become more relevant and the necessity for reliable autonomy more critical. A fundamental challenge in any autonomous underwater operation, from control to safe navigation and mapping, is state estimation; a notorious hard problem to address, especially in low visibility conditions. In this paper, we tackle this problem, by presenting SVIn+, a multi-sensor framework for robust state estimation of AUVs equipped with an IMU, cameras, a water-pressure sensor, a Doppler Velocity Log (DVL), and a compact 3D sonar. This framework extends an established open-source state-of-the-art visual-inertial SLAM, SVIn, by integrating water-pressure and DVL measurements into a tightly-coupled nonlinear optimiza- tion formulation resulting in improved estimates. To leverage the structural information of recently available 3D acoustic sonars while maintaining computational efficiency, we introduce a hierarchical architecture that incorporates estimates from a decoupled front-end via KISS-ICP, treated as relative pose factors within the factor graph. The proposed framework is validated through field deployments in a shipwreck and coral reef, demonstrating drift mitigation and global consistency. To support further research in underwater multi-modal fusion, the code and the datasets can be found on project website https://cmburgul.github.io/projects/svin+/ https://dartmouthrobotics.github.io/svin-plus

I. INTRODUCTION

Underwater SLAM remains difficult in low-visibility environments, where visual tracking can fail and existing sensors leave gaps in drift control and environmental context. SVIn+ combines visual-inertial, pressure, DVL, and 3D sonar measurements, using a decoupled sonar front end to support efficient estimation.

I. INTRODUCTION

  • Underwater SLAM faces light attenuation and limited visibility, while visual-inertial odometry can fail after feature-tracking loss in turbid water or motion blur during dynamic maneuvers.Pressure and DVL can mitigate depth and velocity drift, but do not provide environmental context for loop closures; 2D multibeam sonar also has geometric ambiguity in complex 3D structures.Source: §I · p. 1
  • Compact 3D acoustic sonars offer volumetric point clouds, but their integration into standard multi-sensor SLAM remains underexplored, and their sparse, low-rate, multipath-corrupted scans complicate fusion.Prior work addressed sonar-inertial or visual-inertial-acoustic configurations without the full suite of sensors commonly used in AUVs.Source: §I · p. 1
  • SVIn+ integrates vision, IMU, DVL, and water-pressure measurements in tightly coupled nonlinear optimization, with KISS-ICP sonar pose factors to preserve graph sparsity and support embedded real-time operation.The framework also reports field validation in shipwreck and coral reef environments, with code and datasets released to the community.Source: §I · pp. 1–2
  • The contributions include real-world validation in shipwrecks and coral reefs, alongside release of the associated code and datasets.The paper presents the framework as a response to challenging underwater state-estimation conditions.Source: §I · p. 2

II. RELATED WORK

Prior underwater SLAM combines inertial, visual, and acoustic sensing in different ways, but tightly coupled fusion of the full sensor suite with 3D sonar remains an open problem. The paper situates its approach within that gap and defines the sensor frames and states used in its pipeline.

II. RELATED WORK

  • Earlier EKF fusion of IMU, DVL, depth, and acoustic positioning managed short-term uncertainty but lacked dense environmental context for constraining long-term drift.Visual-inertial methods progressed from offline structure-from-motion and bundle adjustment to online, loosely coupled SLAM and DVL integration.Source: §II · p. 2
  • Tightly coupled visual-DVL and related camera-based approaches pursued localization accuracy, but vision remains limited by restricted field of view, attenuation, backscatter, and turbidity.Sonar-based methods include planar 2D mapping and 3D mapping with mechanical or dual sonars; compact 3D sonar has also been combined with visual-inertial pipelines.Source: §II · p. 2
  • Tightly coupled multi-sensor state estimation for robots equipped with 3D sonar remains an open problem, and the paper proposes joint optimization across sonar, camera, water-pressure, and DVL measurements.The related-work discussion identifies reliable long-term estimation and dense, accurate reconstruction in murky water as ongoing challenges.Source: §II · p. 2
  • The paper describes its framework as partially tightly coupled, jointly optimizing measurements from sonar, camera, water-pressure, and DVL.The motivating gap is a robust long-term solution that supports dense, accurate reconstruction, particularly in murky water.Source: §II · p. 2

III. PRELIMINARIES

  • The global frame is G; IMU, camera, sonar, DVL, and water-pressure sensor frames are rigidly attached to the robot, with extrinsics assumed known from calibration.The state vector includes the current body/IMU state and historical non-keyframe states in the local sliding window; Fig. 2 distinguishes camera modules, sonar processing, and marine-specific components by color.Source: §III · p. 2, Figure 2 · p. 3

IV. MULTI SENSOR ODOMETRY

The local optimizer estimates AUV state through nonlinear, tightly coupled fusion of visual, inertial, water-pressure, DVL, and 3D-sonar odometry measurements. It minimizes modality-specific residuals over a sliding window.

IV. MULTI SENSOR ODOMETRY

  • The local optimization module tightly couples visual, inertial, water-pressure, DVL, and 3D-sonar odometry measurements in a nonlinear estimator.The module is designed to provide high-frequency, reliable state estimates.Source: §IV · p. 3
  • The sliding-window cost sums camera reprojection, IMU, DVL-velocity, water-depth, and 3D-sonar residuals, weighted by measurement information matrices.The inertial residual uses IMU preintegration.Source: §IV · p. 3
  • Within the window, k indexes frames, i cameras, and j visual landmarks observed in frame k.The camera index allows extension to an n-camera system.Source: §IV · p. 3
  • The state includes the IMU rotation, position and velocity in the global frame, and gyroscope and accelerometer biases.The rotation is represented by a Hamilton unit quaternion.Source: §IV · p. 3

A. Water-Pressure Sensor Fusion

Water-pressure measurements are converted to water-column depth and incorporated as a depth residual. The formulation uses the initial depth measurement to establish a global depth reference and assumes a constant water surface during the experiments.

A. Water-Pressure Sensor Fusion

  • The pressure sensor’s water-pressure measurement is converted to water-column depth relative to the surface and used to form a depth residual.The residual is defined for a depth measurement at each time step.Source: A. Water-Pressure Sensor Fusion · p. 3
  • The depth formulation extracts global-frame Z position and accounts for the translation between the IMU and water-pressure sensor.Its global depth reference is computed using the initial depth measurement.Source: A. Water-Pressure Sensor Fusion · p. 3
  • The experiments assume a constant water-surface level, neglecting tidal cycles and ambient-pressure variations because their duration is short relative to the tidal timescale.Source: A. Water-Pressure Sensor Fusion · p. 3

B. Doppler Velocity Log (DVL) Fusion

The DVL fusion forms a velocity residual by relating sensor-frame velocity measurements to the system state. The residual accounts for sensor-to-IMU extrinsics and is weighted using the DVL’s velocity covariance.

B. Doppler Velocity Log (DVL) Fusion

C. 3D Sonar Fusion

SVIn+ registers 3D sonar scans with KISS-ICP, using the previous SVIn+ pose as the motion prior. It converts asynchronous sonar odometry into relative-pose measurements that can be compared with the estimated motion.

C. 3D Sonar Fusion

  • KISS-ICP registers each sonar scan against a local map, using the previous SVIn+ pose as its motion prior instead of a constant-velocity model.The resulting sonar odometry is expressed in a frame initialized to align with the first sonar frame, distinct from the SVIn+ world frame.Source: C. 3D Sonar Fusion · pp. 3–4
  • SE(3) interpolation aligns asynchronous sonar odometry with SVIn+ time steps k−1 and k before forming a relative-pose measurement.The measurement contains a rotation vector and translation, with additive noise.Source: C. 3D Sonar Fusion · p. 4
  • The sonar relative-pose residual compares the interpolated measurement with the transformation predicted from SVIn+ pose estimates and the IMU–sonar extrinsic.The corresponding Jacobians with respect to state variables are derived analytically.Source: C. 3D Sonar Fusion · p. 4

V. EXPERIMENTAL VALIDATION

The experiments evaluate SVIn+ on diverse underwater datasets, including shipwreck and reef environments. They assess sonar constraints in a tightly coupled visual-inertial-DVL estimator and compare performance with state-of-the-art baselines.

V. EXPERIMENTAL VALIDATION

  • Experiments use diverse underwater datasets collected off the coast of Barbados to assess the proposed system.The stated evaluation aims to demonstrate drift mitigation from integrating 3D sonar constraints into a tightly coupled visual-inertial-DVL estimator.Source: §V · p. 4
  • The evaluation includes shipwreck and coral reef environments and compares the system quantitatively against state-of-the-art baselines.The experiments also describe hardware configuration, extrinsic calibration, and dataset characteristics.Source: §V · p. 4
  • Fig. 3 presents a detailed view of the onboard sensor configuration.The caption identifies the configuration view but does not specify sensor-by-sensor results.Source: Figure 3 · p. 4

A. Experimental Setup

The Aqua2 AUV carries synchronized stereo cameras alongside inertial, DVL, pressure, and 3D sonar sensors. The setup calibrates sensor-frame relationships using a combination of Kalibr, manufacturer-provided extrinsics, and underwater targets.

A. Experimental Setup

  • Aqua2 is equipped with a MicroStrain 3DM-CV7 AHRS, hardware-synchronized stereo cameras, a DVL, a pressure sensor, and a 3D sonar.The sensors include two AR0234 global-shutter cameras, a Water Linked DVL-A50, a Bar30 pressure sensor, and a Water Linked 3D-15 sonar.Source: A. Experimental Setup · p. 4
  • Camera–IMU extrinsics are calibrated with Kalibr, while IMU-to-DVL and IMU-to-pressure extrinsics come from the manufacturer.Sonar-to-camera rotation and translation are estimated using PnP from four underwater cinder-block targets visible in sonar and visual data.Source: A. Experimental Setup · p. 4
  • The pipeline was also evaluated in real time on a low-power laptop with an Intel Core i7-10510U CPU, 16 GB RAM, and NVIDIA GTX 1050 GPU.The workstation setup used Ubuntu 24.04, an Intel Core Ultra 7 155H CPU, 64 GB RAM, and an NVIDIA RTX 5000 Ada Generation GPU.Source: A. Experimental Setup · p. 4

B. Dataset Description

Three Barbados datasets span confined shipwreck interiors, a longer shipwreck route, and a shallow reef with dynamic lighting. On BajanQueen-01, return-to-origin evaluation reports lower drift for SVIn+ than SVIn2, while sonar odometry and offline SfM show distinct weaknesses.

B. Dataset Description

  • Three Aqua2 datasets from two Barbados locations provide synchronized stereo vision, DVL, and 3D sonar in complex underwater settings.The sequences range from confined structures to open-water reefs, a suite the authors say was not publicly available in comparable form.Source: B. Dataset Description · p. 4
  • BajanQueen-01 covers about 75 meters over eight minutes inside a shipwreck, with a fixed ArUco marker visited three times for start-to-end drift evaluation.The sequence is described as a moderate-distance trajectory in a confined, structured environment.Source: B. Dataset Description · pp. 4–5
  • BajanQueen-02 spans about 600 meters over 55 minutes across the shipwreck, testing depth variation and sparse visual features.Source: B. Dataset Description · p. 5
  • Folkestone-01 covers about 280 meters over 1,252 seconds on a shallow reef, where dynamic lighting challenges the vision frontend despite a relatively flat trajectory.Source: B. Dataset Description · p. 5

C. Evaluation of Return-to-Origin Trajectory

  • Return-to-origin evaluation uses BajanQueen-01 because global ground truth is unavailable, disables global loop closures, and compares SVIn+ with SVIn2, sonar odometry, and offline SfM.The methods were extensively tuned for a fair comparison.Source: C. Evaluation of Return-to-Origin… · p. 5
  • Sonar odometry shows significantly higher drift in confined shipwreck compartments, while offline SfM has discontinuities where environmental conditions interrupt visual tracking.The passage attributes sonar odometry drift to low-frequency acoustic updates and noise, including multipath effects.Source: C. Evaluation of Return-to-Origin… · p. 5
  • 0.52 m return-to-origin error for SVIn+ versus 0.81 m for SVIn2; SVIn+ also yields a coherent point cloud showing three corridors and a room.The ArUco marker board is closer in the SVIn+ trajectory than in the SVIn2 baseline.Source: C. Evaluation of Return-to-Origin… · p. 5

D. Evaluation on Long-Duration Field Sequences

Long-duration field sequences show consistent SVIn+ trajectories across shipwreck and coral-reef deployments, while the ablation study finds the lowest drift with combined depth and DVL integration. The evaluations also distinguish the modalities’ contributions and show 3D sonar point clouds as a source of environmental mapping detail.

D. Evaluation on Long-Duration Field Sequences

  • The start-to-end BajanQueen-01 trajectory comparison shows reduced accumulated drift with SVIn+ relative to SVIn2 and Sonar Odom.The evaluation examines the complete start-to-end loop.Source: Figure 4 · p. 5
  • Across the 215 m BajanQueen-02 shipwreck and 285 m Folkestone-01 coral reef transits, SVIn+ maintains a highly consistent global depth profile and visually coherent reconstructions despite dynamic lighting.Source: D. Evaluation on Long-Duration Field… · p. 5

E. Ablation study

  • DVL measurements constrain x-y drift, depth integration bounds vertical error, and their tightly coupled combination achieves the lowest overall start-to-end positional drift.These modality effects were assessed in a shallow-water lawnmower-pattern ablation.Source: E. Ablation study · p. 6
  • SVIn+ achieves 0.21 m mean error, a 60.4% overall error reduction on BajanQueen-01, the best result among the evaluated configurations.The comparison uses a fixed environmental marker.Source: E. Ablation study · p. 6
  • 3D sonar scans produce a dense local map that complements the 3D points extracted from visual measurements.The paper identifies the resulting maps as useful for subsequent path planning and autonomous tasks.Source: E. Ablation study · p. 7

VI. LESSONS LEARNED AND FUTURE WORK

SVIn+ combines tightly coupled multi-sensor estimation with a decoupled KISS-ICP sonar front end, and field experiments report drift reduction in complex underwater environments. The paper identifies sensor degeneracy as a remaining robustness challenge and proposes global place recognition and online spatial calibration as future work.

VI. LESSONS LEARNED AND FUTURE WORK

  • Extreme conditions can make individual modalities completely unreliable, including when 3D-sonar registration produces sparse measurements.The authors propose adaptive switching for proactive failure detection and resilience.Source: §VI · p. 7
  • Future work proposes multi-modal global place recognition and online extrinsic calibration to address re-localization, accumulated error, and dynamic sensor-alignment variation.The calibration example given is DVL misalignment.Source: Figure 11 · p. 7

VII. CONCLUSIONS

  • SVIn+ jointly optimizes visual-inertial, water-pressure, and DVL measurements, while KISS-ICP supplies decoupled 6D relative-pose factors from 3D sonar.The hierarchical architecture is presented as a way to leverage sonar structure while maintaining computational efficiency.Source: §VII · p. 7, Figure 12 · p. 8
  • Field experiments in a shipwreck and coral reef show that SVIn+ limits accumulated drift relative to the baseline SVIn2 framework.The conclusion describes this as a foundation for future global loop-closure and SLAM capabilities.Source: Figure 12 · p. 8

Related papers

Loading 2610.36454v1…