💻 computer science

Degradation-Aware Cooperative Multi-Modal GNSS-Denied Localization Leveraging LiDAR-Based Robot Detections

This paper proposes a novel adaptive multi-modal cooperative localization framework for GNSS-denied environments that fuses asynchronous VIO, LIO, and 3D inter-robot detections via a factor-graph formulation, utilizing degradation-aware weighting and interpolation techniques to significantly improve localization accuracy for heterogeneous robot teams.

Original authors: Václav Pritzl, Xianjia Yu, Tomi Westerlund, Petr Štěpán, Martin Saska

Published 2026-05-28

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Václav Pritzl, Xianjia Yu, Tomi Westerlund, Petr Štěpán, Martin Saska

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 a team of robots trying to navigate a world where GPS signals are completely blocked—like being lost in a deep cave or a dense forest. To find their way, they usually rely on their own "senses," like cameras or laser scanners. But just like human senses, these robot sensors can fail. A camera might get blinded by a sudden flash of light or a dark tunnel, and a laser scanner might get confused in a featureless white room.

This paper presents a clever solution: a team of robots helping each other find their way by sharing what they see, even if they are using different types of sensors.

Here is a breakdown of how it works, using simple analogies:

1. The Problem: The "Specialist" Dilemma

Imagine you are leading a hiking team. You have two types of hikers:

The Laser-Scanner Hiker: Great at seeing walls and trees in the dark, but useless if the air is foggy or the room is empty.
The Camera Hiker: Great at spotting colorful patterns and textures, but gets lost in the dark or in a blank white room.

If you force every hiker to carry both a heavy laser scanner and a camera, the team becomes slow, heavy, and expensive. Instead, the authors propose a team where some hikers carry only lasers and others carry only cameras. They work together, but they need a way to trust each other's reports, especially when one of them starts seeing things that aren't there (sensor degradation).

2. The Solution: The "Trustworthy Teammate" System

The authors created a mathematical system (called a Factor Graph) that acts like a smart team leader. This leader constantly asks: "Who is seeing clearly right now, and who is struggling?"

The Laser Hiker (LiDAR): When the laser scanner is working well, the team leader trusts it completely. But if the laser gets confused (like in a foggy field), the leader checks a "health score" (based on the math of how well the laser is matching the environment). If the score is bad, the leader says, "Okay, ignore the laser for a moment; listen to the camera hikers instead."
The Camera Hiker (Visual): Cameras are great, but they can drift over time (like a compass slowly spinning off course). The authors invented a new way to measure how "confused" the camera is. They look at how much the camera's estimate of its position changes from one second to the next. If the camera is jumping around wildly, the team leader knows to trust it less. They use a mathematical tool called the Wasserstein distance (think of it as a "distance meter" between two guesses) to decide how much weight to give the camera's data.

3. The "Handshake" (Inter-robot Detection)

How do the robots know where the others are?

The Laser Hiker has a 3D laser scanner that can spot the Camera Hiker as a distinct object in space.
The Camera Hiker doesn't need to see the Laser Hiker's face; it just needs to know, "Hey, I see a robot over there."

The team leader uses these "handshakes" (detections) to tie the robots together. Even if the Laser Hiker is lost in a foggy field, if it can still see the Camera Hiker, and the Camera Hiker knows where it is, the team leader can use that connection to pull the Laser Hiker back to the correct path.

4. The "Time Travel" Factor (Interpolation)

Robots don't always talk at the exact same millisecond. One might send a message at 10:00:01, and the other at 10:00:03.

The Analogy: Imagine two people taking photos of a moving car at slightly different times. To compare the photos, you have to guess where the car was at the exact moment the other photo was taken.
The Paper's Trick: The authors created a special mathematical "bridge" (an interpolation factor) that allows the team leader to smoothly connect these slightly out-of-sync messages without needing to wait for perfect timing.

5. What Happens When Things Go Wrong? (The "Blind Spot" Analysis)

The authors did a deep dive into what happens when sensors fail completely. They found two main "blind spots":

If the Laser Hiker goes blind: The team can still figure out where the Laser Hiker is if there are at least two Camera Hikers to look at. If there is only one Camera Hiker, the Laser Hiker might get confused about which way it is facing (its "yaw").
If the Camera Hiker goes blind: The team can't figure out which way the Camera Hiker is facing just by looking at it. The Camera Hiker needs to be able to see someone else to know its own direction.

6. The Results: Real-World Proof

The team tested this with real robots:

Indoors: A robot dog (UGV) with a laser and a small drone (UAV) with a camera. When they blocked the camera's view (making it "blind"), the laser robot helped the drone stay on course. When the laser robot walked into a featureless area, the drone helped the laser robot.
Outdoors: They flew drones around a field and between houses. In one test, they artificially "broke" the laser scanner's view. The system immediately switched to relying on the other drones and successfully kept the broken robot on its path, reducing its error from huge distances (meters) to tiny fractions of a meter.

Summary

This paper isn't about making robots smarter individually; it's about making them smarter as a team. By letting robots with different sensors (lasers vs. cameras) help each other, and by having a smart system that knows when to trust which sensor, the team can navigate difficult, GPS-free environments much more accurately than any single robot could alone.

Technical Summary: Degradation-Aware Cooperative Multi-Modal GNSS-Denied Localization Leveraging LiDAR-Based Robot Detections

Problem Statement
Accurate long-term localization in Global Navigation Satellite System (GNSS)-denied environments is critical for autonomous robots. While single robots often rely on fusing multiple sensor modalities (e.g., LiDAR and cameras) to mitigate individual sensor degradations, carrying all sensor types on a single platform increases size, weight, and power demands. Distributing sensors across a heterogeneous team of robots (e.g., LiDAR-equipped robots and camera-equipped robots) improves deployability but introduces significant challenges. Specifically, fusing asynchronous, multi-modal data from independently moving platforms requires handling unsynchronized measurements, anonymous inter-robot detections, and varying levels of sensor reliability without global position references. Existing tightly-coupled multi-robot methods often demand high bandwidth and complex data association, while standard loosely-coupled approaches may fail to adaptively weight inputs when specific sensors degrade.

Methodology
The authors propose a novel adaptive, loosely-coupled, multi-modal multi-robot cooperative localization framework formulated as a factor graph. The system integrates data from a LiDAR-Inertial Odometry (LIO) robot (Robot $X$ ) and one or more Visual-Inertial Odometry (VIO) robots (Robot $Y$ , $Z$ , etc.), utilizing direct 3D detections of robots by the LiDAR-equipped platform.

Key technical components include:

Factor Graph Formulation: The problem is modeled using $SE(3)$ pose variables and factors representing priors, relative odometry, and inter-robot detections. All reference frames are constrained to be gravity-aligned (zero roll and pitch), reducing the state to 4 Degrees of Freedom (DOF) per pose.
Interpolation-Based Detection Factor: To handle asynchronous odometry and detection data, the authors introduce a novel quaternary factor. This factor connects temporally adjacent pose variables of the detecting and detected robots via linear interpolation on the $SE(3)$ manifold. This allows the fusion of measurements at arbitrary timestamps without requiring high-frequency odometry propagation or strict synchronization.
Adaptive Weighting of LIO: The reliability of LiDAR-based localization is evaluated by analyzing the minimum eigenvalue of the approximate Hessian of the scan-matching cost function. If the eigenvalue falls below a threshold (indicating degeneracy, e.g., in featureless environments), the covariance of the LIO relative pose is significantly inflated, effectively down-weighting the unreliable data.
Adaptive Weighting of VIO via Wasserstein Distance: For VIO, which outputs covariance matrices, the authors propose a novel method to weight relative pose factors. They calculate the 2-Wasserstein distance between the probability distributions (covariance matrices) of consecutive VIO filter outputs. This distance serves as a proxy for the uncertainty of the relative pose, accounting for unknown cross-covariances between consecutive filter steps. The variance of the VIO relative pose factor is scaled proportionally to this Wasserstein distance, allowing the system to adapt to sudden changes in visual feature availability or tracking quality.
Observability Analysis: The paper provides a theoretical analysis of the system's observability under various degradation scenarios. It demonstrates that without global priors, the system has unobservable translation and yaw. Crucially, it identifies that in the event of LIO degradation, a single detected robot leaves one DOF (yaw and perpendicular translation) unobservable, whereas detecting multiple robots resolves this ambiguity. Conversely, VIO degradation renders the yaw of the detected robot unobservable.

Key Contributions

Adaptive Loosely-Coupled Fusion: A method that fuses LIO and VIO data with direct 3D inter-robot detections without requiring place recognition or identical sensor suites on all robots.
Interpolation-Based Factor: A novel factor enabling efficient fusion of unsynchronized measurements from distinct robots.
Wasserstein-Distance Weighting: A new approach to weighting VIO relative poses based on the Wasserstein distance between consecutive filter covariances, enabling robust adaptation to variable VIO uncertainty without modifying the underlying odometry algorithm.
Theoretical Observability Analysis: A rigorous analysis of the cooperative localization problem under odometry degradations, identifying specific unobservable directions and the conditions (e.g., number of detected robots) required to resolve them.

Experimental Results
The method was evaluated on real-world datasets involving heterogeneous teams of Unmanned Ground Vehicles (UGVs) and Unmanned Aerial Vehicles (UAVs) in both indoor (motion-capture ground truth) and outdoor (RTK ground truth) environments.

Performance under Degradation: In scenarios where VIO was artificially degraded (e.g., blacked-out camera images), the cooperative method reduced the 3D Absolute Trajectory Error (ATE) of the UAV from 4.671 m to 0.101 m. In outdoor scenarios with significant LIO degradation (featureless ground), the method reduced the position error of the LiDAR-equipped UAV from 74.591 m to 10.373 m.
Wasserstein Distance Validation: A strong positive correlation was found between the Wasserstein distance and the actual localization error, validating its use as a weighting metric. An ablation study confirmed that the adaptive weighting based on Wasserstein distance significantly outperformed static variance settings, particularly in preventing estimation failure during sudden VIO accuracy drops.
Observability Verification: Experiments confirmed the theoretical findings: when the LiDAR robot's odometry degraded, the system exhibited drift in the unobservable direction if only one robot was detected, but this drift was mitigated when two robots were detected. Similarly, the yaw of a VIO-only robot remained unobservable even with cooperative localization, consistent with the theoretical analysis.

Significance and Claims
The paper claims that the proposed approach provides a robust solution for heterogeneous robot teams operating in GNSS-denied environments. By leveraging direct 3D detections and adaptively weighting sensor inputs based on real-time degradation analysis, the method significantly improves localization accuracy compared to individual odometry algorithms when sensors are compromised. The authors emphasize that the approach enables cooperation without requiring every robot to carry a full sensor suite, thereby enhancing deployability. The work is modest in its scope, focusing on the specific case of LIO/VIO fusion with LiDAR-based detections, and explicitly notes that while the method handles variable uncertainties well, it cannot resolve the yaw of a robot that lacks its own yaw-determining sensors (like a VIO-only robot) without additional external information.