💻 computer science

STERN: Simultaneous Trajectory Estimation and Relative Navigation for Autonomous Underwater Proximity Operations

This paper introduces STERN, a versatile factor graph-based framework for simultaneous trajectory estimation and relative navigation that unifies diverse autonomous underwater proximity operations, demonstrating its flexibility and real-time potential through simulations and real-world acoustic homing experiments.

Original authors: Aldo Terán Espinoza, Antonio Terán Espinoza, John Folkesson, Clemens Deutsch, Niklas Rolleberg, Peter Sigray, Jakob Kuttenkeuler

Published 2026-04-09

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Aldo Terán Espinoza, Antonio Terán Espinoza, John Folkesson, Clemens Deutsch, Niklas Rolleberg, Peter Sigray, Jakob Kuttenkeuler

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to meet a friend for a secret rendezvous in the middle of a vast, dark ocean. You are both underwater, and you can't see each other. You can't use cell phones because water blocks radio waves. The only way to talk is by shouting (using sound waves), but the sound is fuzzy, echoes, and takes time to travel.

This is the challenge faced by Autonomous Underwater Vehicles (AUVs)—robotic submarines that need to find each other, dock, or recharge without human help.

This paper introduces a new "brain" for these robots called STERN (Simultaneous Trajectory Estimation and Relative Navigation). Here is how it works, explained simply:

1. The Problem: The "Blind Date" in the Ocean

Usually, when a robot submarine wants to dock with a charging station or meet a "mother ship," it has to guess where the other one is.

The Old Way: If the target (the other ship) is sitting still, it's easy. It's like finding a parked car.
The Hard Way: If the target is moving (like a ship sailing through the water), it's like trying to meet a friend who is walking around a dark room while you are also walking. If you just guess where they were a moment ago, you'll miss them.

Most existing robot brains are good at finding parked cars but terrible at catching moving targets. They get confused and drift off course.

2. The Solution: A "Shared Mental Map" (Factor Graphs)

The authors propose a new way to think about the problem using something called Factor Graphs.

The Analogy: The Detective's Corkboard
Imagine a detective trying to solve a mystery. They have a corkboard with photos (variables) and red strings connecting them (facts/evidence).

The Photos: One photo is "Where I am," another is "Where my friend is," another is "How fast my friend is moving."
The Strings: These are the clues. "I heard a sound from the left," "My friend said they are moving North," "I know I walked 10 meters."

In the past, detectives (robots) would look at one clue at a time and make a quick guess. If they made a mistake early on, the whole case fell apart.

STERN's Approach:
STERN puts all the clues on the board at once. It looks at the whole picture simultaneously.

It asks: "If I am here, and I heard a sound from there, and my friend is moving at this speed, does everything make sense together?"
If the clues don't match, it adjusts the photos (the estimated positions) until the whole board makes perfect sense.

3. The Four Stages of the Mission

The paper breaks the mission into four phases, like levels in a video game:

Remote Phase: You are far away. You are just waiting for a signal.
Long Distance: You hear a faint shout (acoustic signal). You know the general direction, but it's blurry. You are guessing the friend's speed and direction.
Terminal Phase: You are close! Now you can use cameras or lasers (high-tech sensors) to see exactly where they are.
Joint Phase: You grab hands (dock) and do your work.

STERN is designed to handle the tricky Long Distance phase where the target is moving, which is the hardest part.

4. How It Handles the "Moving Target"

The big innovation here is that STERN doesn't just track the robot; it tracks the target's movement at the same time.

The Metaphor: The Dance Partner
Imagine you are dancing with a partner in the dark.

Old Robots: They assume the partner is standing still. If the partner steps forward, the robot bumps into empty space.
STERN: It assumes the partner is dancing. It constantly updates its guess: "My partner moved forward, turned left, and is now moving faster." It builds a mental model of the partner's dance moves.

Even if the partner suddenly stops or changes direction, STERN is flexible enough to say, "Okay, the dance changed," and adjust the map instantly without crashing.

5. Real-World Testing

The authors didn't just write code; they tested it.

Simulation: They ran thousands of virtual meetings in a computer.
Real Life: They took a real robot submarine (named LoLo) and a service boat in a fjord in Sweden. The boat acted as the "moving target."
The Result: Even though the boat didn't always move perfectly straight (it drifted with the wind), STERN was able to figure out where the boat was, how fast it was going, and which way it was facing. It was much better than the robot's standard navigation system, which got lost quickly.

Why This Matters

This technology is like giving underwater robots a "sixth sense."

For Science: It allows robots to stay underwater longer because they can find charging stations on their own.
For Industry: It helps inspect oil rigs or pipelines without needing a human to steer a cable (which limits how far the robot can go).
For the Future: It paves the way for fleets of robots to work together underwater, communicating and docking like a school of fish, all without human intervention.

In a nutshell: This paper teaches underwater robots how to play "catch" with a moving target in the dark, using a smart, flexible mental map that updates itself with every sound and movement, ensuring they don't miss the catch.

1. Problem Statement

The paper addresses the challenges of Autonomous Underwater Vehicle (AUV) proximity operations (prox-ops), specifically focusing on the relative navigation required for an AUV (the "chaser") to rendezvous with a moving target (the "target," e.g., a mothership or another AUV).

The Gap: Existing research largely focuses on static targets (docking with fixed stations) or uses tracking methods that couple control directly to perception without explicit state estimation of the target's dynamics. There is a lack of generalized frameworks for dynamically active targets (moving vessels) where the chaser must simultaneously estimate its own trajectory and the target's pose, velocity, and heading using sparse, noisy acoustic data.
The Challenge: Underwater environments lack GPS. Long-range communication relies on acoustics (USBL), which suffer from high latency, low sampling rates, and significant noise. Furthermore, the target's motion is often unknown or only partially known, making the estimation problem non-linear and highly coupled.

2. Methodology: The STERN Framework

The authors propose STERN (Simultaneous Trajectory Estimation and Relative Navigation), a framework based on Factor Graphs to model the information fusion problem.

A. Conceptual Framework

The authors abstract underwater docking into four phases (Remote, Long-distance, Terminal, Joint) and model the entire process as a single probabilistic graphical model.

Factor Graph Representation: The problem is modeled as a bipartite graph with two chains of variable nodes:
1. Chaser Chain: Represents the AUV's state history (pose, velocity).
2. Target Chain: Represents the target's state history (position, and for dynamic targets, velocity and heading).
Flexibility: This representation allows the system to handle arbitrary sensor suites and target dynamics (passive vs. active) within the same mathematical structure.

B. Specific Scenario: Long-Distance Acoustic Homing

The paper details a specific implementation for a resource-constrained AUV homing to a moving mothership during the long-distance phase (acoustic-only sensing).

State Variables:
- Chaser ( $x_C$ ): 6-DOF pose and velocity in the world frame.
- Target ( $x_T$ ): 3D position, constrained to a 2D plane (constant depth assumption), plus constant velocity ( $v$ ) and heading ( $\theta$ ).
Measurement Factors:
- Relayed USBL: The target measures the relative range/bearing and relays it to the AUV. The graph models the time-delay and coordinate transformation required to use this data.
- Inertial Navigation: Raw IMU (pre-integrated), DVL (velocity), and pressure sensor (depth) measurements are used as unary or binary factors to constrain the chaser's trajectory.
- Motion Model Factor: A custom factor ( $\phi_{mm}$ ) constrains the target's state evolution based on a constant velocity and heading assumption. This allows the system to "predict" the target's motion between acoustic pings.
Optimization: The system solves for the Maximum A Posteriori (MAP) estimate using iSAM2 (incremental Smoothing and Mapping) via the GTSAM library. It supports filtering, fixed-lag smoothing, and full smoothing.

C. Implementation Architecture

Front-End: Handles sensor preprocessing, including outlier rejection for USBL data using RANSAC (Random Sample Consensus) to filter spurious acoustic fixes.
Back-End: Constructs the factor graph using GTSAM, incorporating custom factors for depth and the target's motion model. It runs incrementally to provide real-time estimates.

3. Key Contributions

Generalized Framework: Introduced STERN, a factor-graph-based framework that unifies various underwater prox-ops scenarios (static vs. dynamic targets, different sensor suites) under a single modeling paradigm.
Dynamic Target Modeling: Specifically addressed the under-researched problem of homing to a dynamically active target (moving mothership) by explicitly estimating the target's velocity and heading alongside its position.
Relayed USBL Handling: Developed a method to handle relayed acoustic measurements (where the target measures and sends data to the chaser), accounting for transmission delays and coordinate frame rotations.
Real-World Validation: Provided a complete roadmap for deploying this estimator on a real AUV (LoLo), including timing analysis, initialization procedures, and field testing in open water.

4. Results and Evaluation

The authors validated the approach using both simulated data (Stonefish simulator) and real-world field experiments in Baggensfjärden, Sweden.

Simulations:
- Tested various USBL noise levels and motion model compliance (tightness of the constant velocity/heading assumption).
- Finding: The system is robust. Even with noisy USBL data and imperfect motion assumptions, the estimator converges. "Fixed-lag smoothing" and "full smoothing" generally outperformed pure filtering, though filtering showed better adaptability to oscillatory target motions.
Field Experiments (7 deployments, 9 datasets):
- Performance: The STERN estimator successfully tracked the target's trajectory, velocity, and heading, significantly outperforming dead-reckoning (which suffered from drift).
- Accuracy: Position errors were generally within a few meters (e.g., mean error ~3–6m depending on the test), with smoothed estimates consistently better than real-time estimates.
- Initialization: The system successfully initialized the target's state using the first few USBL measurements, though initialization errors in heading were observed if the initial measurements were noisy.
- Real-Time Feasibility: Timing analysis showed the entire pipeline (sensor processing to state update) takes an average of 8.7 ms per keyframe on a standard laptop CPU, leaving ample headroom for real-time operation at 0.5 Hz USBL rates.

5. Significance and Future Work

Significance: This work bridges the gap between theoretical SLAM/estimation and practical underwater robotics. It demonstrates that complex relative navigation tasks with moving targets can be solved using modular, probabilistic factor graphs without requiring high-end sensors or perfect target models. It enables AUVs to autonomously recover from moving vessels, a critical capability for extending mission endurance.
Limitations: The current motion model assumes constant velocity and heading, which can be violated in rough seas. The USBL noise model is simplified, and initialization can be sensitive to initial outliers.
Future Directions:
- Replacing the constant dynamics assumption with more flexible models (e.g., Gaussian Processes).
- Extending the framework to the terminal phase (hybrid sensing with cameras/optical sensors) for full 6-DOF docking.
- Closing the loop by integrating the estimator output directly into the AUV's motion control and planning modules.
- Developing more robust noise models for acoustic measurements.

In conclusion, the paper presents a robust, flexible, and computationally efficient solution for autonomous underwater rendezvous with moving targets, leveraging the power of factor graphs to unify disparate sensor data and dynamic constraints.