Cooperative Localization

Communication is a core enabler for multi-robot systems (MRS), providing the mechanism through which robots exchange state information, coordinate actions, and satisfy safety constraints. While many MRS autonomy algorithms assume reliable and timely message delivery, realistic wireless channels introduce delay, erasures, and ordering stalls that can degrade performance and compromise safety-critical decisions of the robot task. In this paper, we investigate how transport-layer reliability mechanisms that mitigate communication losses and delays shape the autonomy-communication loop. We show that conventional non-coded retransmission-based protocols introduce long delays that are misaligned with the timeliness requirements of MRS applications, and may render the received data irrelevant. As an alternative, we advocate for adaptive and causal network coding, which proactively injects coded redundancy to achieve the desired delay and throughput, enabling relevant data delivery for the robotic task. Specifically, this method adapts to channel conditions between robots and causally tunes the communication rates via efficient algorithms. We present two case studies: cooperative localization under delayed and lossy inter-robot communication, and a safety-critical overtaking maneuver where timely vehicle-to-vehicle message availability determines whether an ego vehicle can abort to avoid a crash. Our results demonstrate that coding-based communication significantly reduces in-order delivery stalls, keeps cooperative-localization accuracy close to the ideal baseline, and satisfies the overtaking abort deadline in 80% of the simulated runs, compared with 60% for a retransmission-based baseline. The study highlights the need to jointly design autonomy algorithms and communication mechanisms, and positions network coding as a principled tool for dependable MRS operation over wireless networks.

Decentralized cooperative localization (DCL) is a promising approach for nonholonomic mobile robots operating in GPS-denied environments with limited communication infrastructure. This paper presents a DCL framework in which each robot performs localization locally using an Extended Kalman Filter, while sharing measurement information during update stages only when communication links are available and companion robots are successfully detected by LiDAR. The framework preserves cross-correlation consistency among robot state estimates while handling asynchronous sensor data with heterogeneous sampling rates and accommodating accelerations during dynamic maneuvers. Unlike methods that require pre-aligned coordinate systems, the proposed approach allows robots to initialize with arbitrary reference-frame orientations and achieves automatic alignment through transformation matrices in both the prediction and update stages. To improve robustness in feature-sparse environments, we introduce a dual-landmark evaluation framework that exploits both static environmental features and mobile robots as dynamic landmarks. The proposed framework enables reliable detection and feature extraction during sharp turns, while prediction accuracy is improved through information sharing from mutual observations. Experimental results in both Gazebo simulation and real-world basement environments show that DCL outperforms centralized cooperative localization (CCL), achieving a 34% reduction in RMSE, while the dual-landmark variant yields an improvement of 56%. These results demonstrate the applicability of DCL to challenging domains such as enclosed spaces, underwater environments, and feature-sparse terrains where conventional localization methods are ineffective.

Cooperative localization (CL) enables accurate position estimation in multi-robot systems operating in GPS-denied environments. This paper presents a comparative study of five CL approaches: Centralized Cooperative Localization (CCL), Decentralized Cooperative Localization (DCL), Sequential Cooperative Localization (StCL), Covariance Intersection (CI), and Standard Cooperative Localization (Standard-CL). All methods are implemented in ROS and evaluated through Monte Carlo simulations under two conditions: weak data association and robust detection. Our analysis reveals fundamental trade-offs among the methods. StCL and Standard-CL achieve the lowest position errors but exhibit severe filter inconsistency, making them unsuitable for safety-critical applications. DCL demonstrates remarkable stability under challenging conditions due to its measurement stride mechanism, which provides implicit regularization against outliers. CI emerges as the most balanced approach, achieving near-optimal consistency while maintaining competitive accuracy. CCL provides theoretically optimal estimation but shows sensitivity to measurement outliers. These findings offer practical guidance for selecting CL algorithms based on application requirements.

Persistent air ground robot teams require nano UAVs that can leave a mobile UGV, perform short scouting or inspection tasks, and reliably return to a compact landing interface. This final docking phase remains difficult: it demands centimeter-scale relative localization on a moving platform, while nano UAVs provide only a few grams of payload and severe onboard sensing, power, and computation constraints. We address this problem with an infrastructure-less magneto inductive anchor-tag localization system for heterogeneous UAV-UGV teaming. Unlike passive magnetic docking aids or approaches exploiting environmental magnetic features, the proposed system actively generates a frequency multiplexed AC magnetic field on the UGV and uses it as an onboard localization reference for closed loop flight. The nano UAV carries only a lightweight passive receive coil and estimates its 3D position directly in the UGV frame, providing a local close range reference for hovering, tracking, and landing without external anchors, visual fiducials, GNSS, or motion capture online. The magnetic estimate is fused with the native onboard sensing stack, enabling operation under nano UAV SWaP constraints. Experiments with a Unitree A1 quadruped and a Crazyflie nano UAV, evaluated against motion capture ground truth, show centimeter level accuracy in static hovering and landing and approximately 8-11 cm RMSE during UGV motion, while the flow only baseline frequently violates the safety bound and fails the task.

The cooperative localization (CL) problem in heterogeneous robotic systems with different measurement capabilities is investigated in this work. In practice, heterogeneous sensors lead to directed and sparse measurement topologies, whereas most existing CL approaches rely on multilateral localization with restrictive multi-neighbor geometric requirements. To overcome this limitation, we enable pairwise relative localization (RL) between neighboring robots using only mutual measurement and odometry information. A unified data-driven adaptive RL estimator is first developed to handle heterogeneous and unidirectional measurements. Based on the convergent RL estimates, a distributed pose-coupling CL strategy is then designed, which guarantees CL under a weakly connected directed measurement topology, representing the least restrictive condition among existing results. The proposed method is independent of specific control tasks and is validated through a formation control application and real-world experiments.

Cooperative localization is essential for swarm applications like collaborative exploration and search-and-rescue missions. However, maintaining real-time capability, robustness, and computational efficiency on resource-constrained platforms presents significant challenges. To address these challenges, we propose D-GVIO, a buffer-driven and fully decentralized GNSS-Visual-Inertial Odometry (GVIO) framework that leverages a novel buffering strategy to support efficient and robust distributed state estimation. The proposed framework is characterized by four core mechanisms. Firstly, through covariance segmentation, covariance intersection and buffering strategy, we modularize propagation and update steps in distributed state estimation, significantly reducing computational and communication burdens. Secondly, the left-invariant extended Kalman filter (L-IEKF) is adopted for information fusion, which exhibits superior state estimation performance over the traditional extended Kalman filter (EKF) since its state transition matrix is independent of the system state. Thirdly, a buffer-based re-propagation strategy is employed to handle delayed measurements efficiently and accurately by leveraging the L-IEKF, eliminating the need for costly re-computation. Finally, an adaptive buffer-driven outlier detection method is proposed to dynamically cull GNSS outliers, enhancing robustness in GNSS-challenged environments.

Accurate relative localization is critical for multi-robot cooperation. In robot groups, measurements from different robots arrive asynchronously and with clock time-offsets. Although Continuous-Time (CT) formulations have proved effective for handling asynchronous measurements in single-robot SLAM and calibration, extending CT methods to multi-robot settings faces great challenges in achieving high-accuracy, low-latency, and high-frequency performance. In particular, existing CT methods suffer from the inherent query-time delay of unclamped B-splines and high optimization latency. This paper proposes CT-RIO, a novel Continuous-Time Relative-Inertial Odometry framework. We adopt Clamped Non-Uniform B-splines (C-NUBS) to represent states, eliminating the query-time delay. We further augment C-NUBS with closed-form extension and shrinkage operations that preserve the spline shape, making it suitable for online estimation and enabling flexible knot management. This flexibility leads to the concept of a knot-keyknot strategy, which supports spline extension at high frequency while retaining sparse keyknots for adaptive relative motion modeling. We then formulate a reference-centric sliding-window relative localization problem that operates purely on relative kinematics and inter-robot constraints. To enable low-latency and high-frequency estimation, we decompose the tightly coupled optimization into robot-wise subproblems and solve them in parallel using asynchronous block coordinate descent. Extensive experiments show that CT-RIO converges from time-offsets as large as 264 ms to sub-millisecond within 3 s, and achieves RMSEs of 0.046 m and 1.8 degree. It consistently outperforms evaluated published methods, with improvements of up to 60% under high-speed motion.

Reliable localization is a fundamental requirement for multi-robot systems operating in GPS-denied environments. Visual-inertial odometry (VIO) provides lightweight and accurate motion estimation but suffers from cumulative drift in the absence of global references. Ultra-wideband (UWB) ranging offers complementary global observations, yet most existing UWB-aided VIO methods are designed for single-robot scenarios and rely on pre-calibrated anchors, which limits their robustness in practice. This paper proposes a distributed collaborative visual-inertial-ranging odometry (DC-VIRO) framework that tightly fuses VIO and UWB measurements across multiple robots. Anchor positions are explicitly included in the system state to address calibration uncertainty, while shared anchor observations are exploited through inter-robot communication to provide additional geometric constraints. By leveraging a right-invariant error formulation on Lie groups, the proposed approach preserves the observability properties of standard VIO, ensuring estimator consistency. Simulation results with multiple robots demonstrate that DC-VIRO significantly improves localization accuracy and robustness, while simultaneously enabling anchor self-calibration in distributed settings.

Controlling a team of robots in a coordinated manner is challenging because centralized approaches (where all computation is performed on a central machine) scale poorly, and globally referenced external localization systems may not always be available. In this work, we consider the problem of range-aided decentralized localization and formation control. In such a setting, each robot estimates its relative pose by combining data only from onboard odometry sensors and distance measurements to other robots in the team. Additionally, each robot calculates the control inputs necessary to collaboratively navigate an environment to accomplish a specific task, for example, moving in a desired formation while monitoring an area. We present a block coordinate descent approach to localization that does not require strict coordination between the robots. We present a novel formulation for formation control as inference on factor graphs that takes into account the state estimation uncertainty and can be solved efficiently. Our approach to range-aided localization and formation-based navigation is completely decentralized, does not require specialized trajectories to maintain formation, and achieves decimeter-level positioning and formation control accuracy. We demonstrate our approach through multiple real experiments involving formation flights in diverse indoor and outdoor environments.

Relative localization is essential for cooperation in autonomous multi-robot systems. Existing approaches either rely on shared environmental features or inertial assumptions, or they degrade under pairwise non-line-of-sight conditions and outliers in complex environments. Robustly and efficiently fusing inter-robot bearings, distances, and inertial measurements for tens of robots remains challenging. We present CREPES-X (Cooperative RElative Pose Estimation System with multiple eXtended features), a hierarchical relative localization framework that enhances speed, accuracy, and robustness under challenging conditions, without requiring any global information. The hardware packs infrared (IR) LEDs, an IR camera, an ultra-wideband module, and an IMU into a cube no larger than $6\,\text{cm}$ on each side. On this hardware, a two-stage hierarchical estimator meets different latency, accuracy, and robustness requirements. The single-frame estimator returns instantaneous relative poses from a closed-form solution with bearing outlier rejection. The multi-frame estimator then refines these poses with IMU pre-integration under robocentric relative kinematics, using loosely- and tightly-coupled optimization. Extensive simulations and real-world experiments validate the effectiveness of CREPES-X, demonstrating robustness of up to $90\%$ bearing outliers, resilience in challenging conditions, and RMSE of $7.0\,\text{cm}$ and $2.2^\circ$ in real-world datasets.

Reflections of active markers in the environment are a common source of ambiguity in onboard visual relative localization. This work presents a novel approach that exploits these typically unwanted reflections for onboard relative localization in heterogeneous multi-UAV teams operating over specular, near-planar surfaces such as calm water and flat indoor floors. Using only a camera, active markers, and a single-beam rangefinder, the method requires no prior knowledge of robot size, marker configuration, or surface reflectance properties, and propagates the uncertainty introduced by surface irregularities, including those of dynamic water surfaces. We further characterize, analytically and experimentally, how surface tilt biases the estimate, and identify the slope range over which the level-surface assumption holds. Indoor multi-UAV and outdoor experiments validate the localization accuracy, demonstrating reliable operation across varying lighting conditions and greater effective range than state-of-the-art vision-based active-marker methods, with higher accuracy beyond 30 m. The project website with the video is available under the following link: https://timlakemann.github.io/reflection-based-relative-localization/.

Cooperative Localization is a promising approach to achieving safe quadrotor formation flight through precise positioning via low-cost inter-drone sensors. This paper develops an observability-aware control principle tailored to quadrotor formation flight with range-only inter-drone measurements. The control principle is based on a novel approximation of the local observability Gramian (LOG), which we name the Short-Term Local Observability Gramian (STLOG). The validity of STLOG is established by proving its link to directional estimation precision in nonlinear systems. We propose the Observability Predictive Controller (OPC), a receding-horizon controller that generates optimal inputs to enhance information gain in weakly observable state directions by maximizing the minimum eigenvalue of the STLOG. This reduces the risk of estimator divergence due to the unbounded growth of uncertainty in weakly observed state components. Monte Carlo simulations and flight experiments are conducted with quadrotors in a GNSS-denied ferrying mission, showing that the OPC improves positioning confidence and estimator robustness.