DTVIRM-Swarm: A Distributed and Tightly Integrated Visual-Inertial-UWB-Magnetic System for Anchor Free Swarm Cooperative Localization

Highlights

This study proposes DTVIRM-Swarm, a distributed, anchor-free cooperative localization system for UAV swarms that operates without GNSS or pre-deployed anchors. It introduces a novel MDS-MAP initialization method, achieving faster and more accurate swarm pose initialization than conventional methods. The system demonstrates superior accuracy and strong robustness, maintaining operation during vision loss where other methods fail. It provides a practical, scalable solution for GNSS-denied environments, enabling complex applications like formation flying under severe conditions.

What are the main findings?

• Superior Localization Accuracy and Efficiency: The proposed system and its novel MDS-MAP initialization method significantly outperform state-of-the-art alternatives, achieving faster and more accurate swarm pose initialization than conventional methods, culminating in real-world accuracy that is higher than leading VIO and collaborative methods.
• Enhanced Robustness in Degraded Conditions: The tightly-coupled framework demonstrates exceptional resilience, maintaining stable operation and reliable positioning during complete vision deprivation, a scenario in which comparable state-of-the-art systems fail entirely.

What are the implications of the main findings?

• A Practical Enabler for GNSS-Denied Operations: This work provides a viable, infrastructure-independent solution for multi-robot collaborative localization, effectively addressing a critical gap for deploying low-cost UAV swarms in complex environments where traditional GNSS or anchor-dependent systems are infeasible.
• A Pathway to Scalable and Resilient Applications: The system’s design principles, including its efficient initialization and robust sensor fusion, pave the way for scalable applications involving large-scale swarms or heterogeneous robot teams, while its performance under harsh conditions expands the operational envelope for autonomous systems to more severe scenarios.

Abstract

Accurate Unmanned Aerial Vehicle (UAV) positioning is vital for swarm cooperation. However, this remains challenging in situations where Global Navigation Satellite System (GNSS) and other external infrastructures are unavailable. To address this challenge, we propose to use only the onboard Microelectromechanical System Inertial Measurement Unit (MIMU), Magnetic sensor, Monocular camera and Ultra-Wideband (UWB) device to construct a distributed and anchor-free cooperative localization system by tightly fusing the measurements. As the onboard UWB measurements under dynamic motion conditions are noisy and discontinuous, we propose an adaptive adjustment method based on chi-squared detection to effectively filter out inconsistent and false ranging information. Moreover, we introduce the pose-only theory to model the visual measurement, which improves the efficiency and accuracy for visual-inertial processing. A sliding window Extended Kalman Filter (EKF) is constructed to tightly fuse all the measurements, which is capable of working under UWB or visual deprived conditions. Additionally, a novel Multidimensional Scaling-MAP (MDS-MAP) initialization method fuses ranging, MIMU, and geomagnetic data to solve the non-convex optimization problem in ranging-aided Simultaneous Localization and Mapping (SLAM), ensuring fast and accurate swarm absolute pose initialization. To overcome the state consistency challenge inherent in the distributed cooperative structure, we model not only the UWB noisy uncertainty but also the neighbor agent’s position uncertainty in the measurement model. Furthermore, we incorporate the Covariance Intersection (CI) method into our UWB measurement fusion process to address the challenge of unknown correlations between state estimates from different UAVs, ensuring consistent and robust state estimation. To validate the effectiveness of the proposed methods, we have established both simulation and hardware test platforms. The proposed method is compared with state-of-the-art (SOTA) UAV localization approaches designed for GNSS-challenged environments. Extensive experiments demonstrate that our algorithm achieves superior positioning accuracy, higher computing efficiency and better robustness. Moreover, even when vision loss causes other methods to fail, our proposed method continues to operate effectively.

1. Introduction

Accurate localization is critical for efficient operations and precise control in aerial robotics, particularly in multi-UAV systems where collaborative tasks, formation flying, and mission execution depend on it. However, achieving reliable localization remains challenging in GNSS-denied environments [1]. To overcome this, researchers have investigated multimodal sensor fusion approaches. Among these, visual-inertial odometry (VIO) has emerged as a widely studied method that integrates camera and IMU data for relative motion estimation and localization [2,3].

VIO operates by capturing environmental images through cameras to extract and track visual features, while IMUs measure acceleration and angular velocity to estimate pose changes. Despite its advantages, VIO suffers from two primary limitations [4]. First, its performance heavily depends on visual conditions: feature extraction becomes unreliable in texture-sparse areas, and sudden environmental changes introduce tracking errors, degrading localization accuracy. Second, the computational complexity of real-time image processing and sensor fusion strains the limited processing capabilities of UAVs, further restricting accuracy and practicality [5,6].

To address these challenges in complex environments, multi-UAV collaborative localization methods have been developed. These approaches are broadly classified into centralized and distributed systems based on data processing strategies. Centralized methods rely on transmitting all UAV-collected data to a ground station or central node for unified computation. However, they require stable network connectivity and high-performance hardware at the central node, resulting in scalability and flexibility limitations. In contrast, distributed systems eliminate central nodes by enabling direct communication between neighboring UAVs, offering enhanced robustness and adaptability for dynamic environments. This advantage has made distributed methods increasingly prevalent in multi-UAV applications [7,8].

Current VIO-based collaborative localization methods often improve accuracy by sharing environmental characteristics observed across UAV clusters. While effective, such strategies impose high communication bandwidth requirements, which become problematic as UAV numbers or environmental complexity increases. Alternative approaches using pre-deployed UWB anchors enhance localization through anchor-based distance measurements. Nevertheless, these methods lack flexibility due to their dependence on carefully calibrated anchor infrastructure, making them impractical for unknown or dynamic environments where anchor deployment is costly or infeasible [9,10].

To address these challenges and meet the demands for low-cost, lightweight design, and real-time performance, this paper proposes a novel distributed anchor-free visual-inertial-UWB-magnetic cooperative localization system (DTVIRM-Swarm) for multi-UAVs based on the sliding window extended Kalman filter framework. Unlike existing distributed SLAM systems such as COVINS-G [11] and D2SLAM, which require significant communication bandwidth for map sharing, our approach leverages direct UWB ranging between UAVs without relying on pre-deployed anchors, achieving higher communication efficiency. Compared to COO-VIR [12], which still requires static UWB anchors, our system offers greater flexibility in unknown environments.

In this system, UAVs measure mutual distances using UWB technology without relying on UWB anchors. To fully utilize UWB measurements, multiple UWB keyframes are incorporated into the sliding window, and an adaptive adjustment method for UWB filtering errors based on chi-squared detection is employed to ensure robustness and real-time performance. Additionally, to further enhance accuracy, a visual observation model based on pose-only (PO) theory is adopted, improving the system’s adaptability to challenging visual environments [13,14].

The contributions of this paper are as follows:

• Distributed Anchor-Free Architecture with Efficient Initialization: Proposes a distributed anchor-free cooperative localization framework for multi-UAV swarms, eliminating the reliance on central nodes or pre-deployed anchors. Integrates ranging, geomagnetic, and MIMU data to develop an MDS-MAP initialization method, which addresses the non-convex optimization problem in ranging-aided SLAM and enables fast and accurate absolute pose initialization of the swarm, outperforming traditional GTSAM-based optimization methods by 40.6% in positioning accuracy and reducing computation time by over 80%.
• Optimized Sensor Measurement Processing: Targets the noise and discontinuity of UWB dynamic measurements, presents an adaptive adjustment method based on chi-squared detection, and fuses multiple UWB keyframes to enhance robustness. Adopts pose-only (PO) theory for visual measurement modeling, decouples 3D feature reconstruction, and achieves a balance between positioning accuracy and computational efficiency, reducing the computational burden compared to traditional VIO systems.
• Covariance Intersection (CI) for Consistent Measurement Fusion: Incorporates the CI method into UWB measurement fusion to address the challenge of unknown correlations between state estimates from different UAVs. This ensures consistent and robust state estimation even in dynamic environments with frequent communication delays and packet losses, enhancing the overall reliability of the cooperative localization system.
• Tightly-Coupled Multi-Sensor Fusion Framework: Constructs a sliding window EKF to fuse inertial, visual, UWB ranging, and geomagnetic data while explicitly modeling the position uncertainty of neighboring nodes. The framework maintains stable operation even in vision-deprived scenarios, breaking through the limitation of traditional methods that rely on continuous visual input and improving adaptability to complex environments. Compared to SOTA methods like VINS-Mono, OpenVINS, and SuperVINS, our system achieves an average positioning error reduction of 38.3%, 40.8%, and 29.7% respectively.

The structure of the paper is as follows. Section 2 presents a literature review. Section 3 introduces the proposed algorithm and the detailed mathematical model of collaborative localization. Section 4 presents simulation experiments and real experiments, and analyzes the results. Finally, Section 5 concludes the paper.

2. Related Work

In the field of UAV collaborative localization, a key issue is the relative position observation between UAVs. Currently, there are many methods to solve the problem of relative position observation between drones. Marker-based visual mutual observation methods usually extract and deploy markers on the drone, and use markers, such as ultraviolet LED lights and ultraviolet sensitive cameras, combined to perform relative positioning [15]. Unlabeled visual mutual observation methods often rely on convolutional neural networks (CNN) [16,17,18], which use machine learning-based techniques to extract relative distances, but are easily affected by changes in the appearance of targets and the environment and cannot provide accurate relative estimates.

Multi-robot SLAM methods can use map merging and loop closure between robots to obtain relative attitudes. This method requires the communication of a large amount of data and is not suitable for drone platforms with computational performance constraints [19]. 3D LiDAR and UWB can also directly measure relative distances. In [20], the fusion of 3D LiDAR, fisheye camera and UWB data is used to track drones flying above LiDAR unmanned ground vehicles (UGV). In [21], a distributed LiDAR inertial group range measurement method was proposed, which uses the reflectivity values from LiDAR data to directly detect collaborative drones, but using LiDAR as a relative position observation sensor is costly. In [22], a relative positioning method for micro drones based on VIO and LiDAR localization was proposed. Slave drones use LiDAR to observe the relative position and distance of the master drone, and integrate it with VIO to improve the positioning accuracy of slave drones. However, this method requires a high precision master drone node. If the master drone node is damaged, the entire drone cluster will not work normally.

To address the issue of substantial positioning errors in GNSS-challenged environments, particularly when visual features are sparse, researchers have proposed several methods. In terms of single drone positioning, ref. [23] fuses data from static UWB anchors, LiDAR odometers, IMUs and VIOs to improve the positioning accuracy of drones in GNSS-challenged environments, but in emergency situations, it is not feasible to place static UWB anchors in the area. In [24], a multi-sensor framework is proposed that uses ultra-wideband (UWB) technology and visual inertial measurement (VIO) to provide robust and low-drift positioning. In [25], a learning-based drone positioning method is proposed that uses fused vision, IMU, and UWB sensors, combined with visual inertia (VIO) and UWB branches, to predict global attitude, but these two methods still require the placement of multiple ground UWB anchors in advance.

In terms of multi-drone positioning, ref. [26] proposed a method that integrates UWB and VIO for collaborative positioning of two drones, and ref. [27] proposed a method for distributed formation estimation in large drone clusters. In [8], a distributed collaborative SLAM system was proposed, which innovatively manages near-field estimation and integrates multiple sensors and map data to achieve accurate near-field relative state estimation and consistent global trajectory far-field estimation. In [28], the authors fused detection results from CNN with UWB data and VIO for relative positioning in drone clusters. Ref. [12] fuses data from static UWB anchors, mutual ranging between drones, IMU and VIO to improve the positioning accuracy of drones in GNSS-challenged environments. However, this active vision-based approach cannot cope with urgent tasks.

The authors of [29] focused on collaborative localization of UGV and UAV teams, and their method relied mainly on UWB and VIO data and used 3D LiDAR detection during initialization. The authors of [30] utilized a heterogeneous team of UGVs carrying LiDAR and camera-equipped drones with the goal of detecting UGVs from airborne cameras on the drones and using them as a landmark for improving drone positioning. Ref. [31] proposed a new cooperative localization framework based on optimized belief propagation (BP) for use in GNSS denial areas, and ref. [32] proposed a technology to improve the accuracy of visual inertial odometer (VIO) by combining ultra-wideband (UWB) positioning technology. However, these methods all require the assistance of multiple UWB anchors, which is not conducive to practical use; ref. [33] proposes a multi-drone relative scheme based on distributed graph optimization (DGO), which combines on-board ultra-wideband (UWB) modules, cameras and inertial sensors, but the cameras need to observe other drones for position estimation, making it unsuitable for large-scale scenarios. Ref. [34] fuses the data from these two types of sensors via the Kalman filter to address the positioning drift problem in GPS-denied environments. Ref. [35] adopts the Information Consistency Filter (ICF) to address the state estimation association problem among multiple devices. Ref. [36] adopts a strategy of second-order Kalman filter preprocessing combined with EKF gating fusion to tackle the sensor observation data with intermittency and time delay.

Therefore, considering the requirements of low cost, lightweight, and real-time performance, we propose a distributed anchorless vision-inertial-UWB-magnetic multi-drone collaborative positioning system. UAVs measure the distance between drones through UWB. There is no need to rely on UWB anchors, and a filtering framework based on EKF is used to fuse multiple observations, which enhances the lightweight and real-time nature of the algorithm. In order to further improve the accuracy, we incorporate multiple UWB key frames into the sliding window, and adopt an adaptive adjustment method for filtering errors based on chi-squared detection. In addition, in terms of vision observation, we adopt a visual observation model based on PO constraints, which improves the system’s adaptability to challenging visual environments.

3. Methods

The proposed method is a distributed and anchor-free visual-inertial-UWB-magnetic cooperative localization system for UAVs. It consists of three main components: a sliding window extended Kalman filter framework for tightly fusing inertial, visual, Magnetic, and UWB ranging data; an adaptive adjustment method for UWB measurements based on chi-squared detection; and a visual observation model using pose-only (PO) theory.

3.1. Overview of the Architecture

The block diagram of the architecture of the proposed system is shown in Figure 1. Each UAV is equipped with an IMU, a magnetometer, a camera and a UWB sensor, and the UAVs can share each other’s position and relative distance information through communication links and UWB.

In the structural block diagram shown in Figure 1, feature tracking and extraction are used to obtain environmental feature information, IMU data is used for state propagation and enhancement to predict the motion state of the UAV, and basic views and sensor data are used to build a visual observation model based on pure pose (PO) theory to improve the accuracy of the algorithm. After the magnetometer is calibrated by the magnetic sensor, it outputs heading angle information for subsequent fusion positioning. Then, the information predicted by the filter is used to perform chi-squared detection on the UWB data and filter out outliers. Finally, the state estimate of the UAV is updated by fusing multiple sensor information in a tight combination method.

3.2. Multi-Sensor Time Synchronization and Delay Handling

In a multi-sensor fusion system for cooperative localization, precise time synchronization is fundamental to estimation accuracy. The sensors in this system, including IMU, camera, magnetometer (MAG), UWB, and inter-UAV position information, operate at different sampling frequencies and exhibit varying latencies. In distributed scenarios, wireless communication delays introduce additional temporal uncertainties. We propose a time synchronization and delay handling scheme built upon the sliding window EKF framework, leveraging IMU’s high-frequency propagation capability for temporal alignment.

3.2.1. Time Synchronization for Onboard Sensors

All sensor measurements carry precise timestamps. When a measurement arrives at time $t_m$, the system state at this instant is obtained through IMU-driven interpolation. The detailed steps of the algorithm are illustrated in Algorithm 1. Given filter states ${\widehat{\mathit{x}}}_k$ at $t_k$ and ${\widehat{\mathit{x}}}_{k+1}$ at $t_{k+1}$ where $t_k<t_m<t_{k+1}$:

$${\widehat{\mathit{x}}}_m={\widehat{\mathit{x}}}_k+{\displaystyle \frac{t_m-t_k}{t_{k+1}-t_k}}\left({\widehat{\mathit{x}}}_{k+1}-{\widehat{\mathit{x}}}_k\right)$$

(1)

The synchronized residual is then computed as:

$$\mathit{r}=\mathbf{z}-\mathbf{h}\left({\widehat{\mathit{x}}}_m\right)$$

(2)

where $\mathbf{h}(·)$ is the measurement model.

Algorithm 1 Multi-Sensor Time Synchronization and Delay Handling.

Require:  Sliding window states $\{{\mathit{x}}_{k-N+1},\dots ,{\mathit{x}}_k\}$, timestamps, measurement $\mathbf{z}$ with timestamp $t_m$ Ensure:  Aligned state ${\widehat{\mathit{x}}}_m$ or updated state ${\mathit{x}}_k$   1: Onboard Sensor Synchronization   2: Find states ${\mathit{x}}_k$ at $t_k$ and ${\mathit{x}}_{k+1}$ at $t_{k+1}$ ($t_k<t_m<t_{k+1}$)   3: ${\widehat{\mathit{x}}}_m\leftarrow {\mathit{x}}_k+{\displaystyle \frac{t_m-t_k}{t_{k+1}-t_k}}({\mathit{x}}_{k+1}-{\mathit{x}}_k)$   4: $\mathit{R}\leftarrow \mathbf{z}-\mathbf{h}\left({\widehat{\mathit{x}}}_m\right)$   5: Inter-UAV Delay Handling   6: $\mathrm{\Delta }t\leftarrow t_r-t_c$   7: if  $\mathrm{\Delta }t>{\mathrm{threshold}}_{\max }$  then   8:        Discard measurement   9: else 10:       ${\mathit{x}}_d\leftarrow \mathrm{Buffer}\left[t_d\right]$, $\nu \leftarrow {\mathbf{z}}_d-{\mathbf{h}}_d\left({\mathit{x}}_d\right)$ 11:       ${\mathbf{K}}_d\leftarrow {\mathit{p}}_d{\mathbf{H}}_d^T{({\mathbf{H}}_d{\mathit{p}}_d{\mathbf{H}}_d^T+{\mathit{R}}_d)}^{-1}$ 12:       ${\tilde{\mathit{x}}}_d\leftarrow {\mathit{x}}_d+{\mathbf{K}}_d\nu$ 13:       ${\mathrm{\Phi }}_{k,d}\leftarrow \mathbf{I}+\mathbf{F}\mathrm{\Delta }t$ 14:       ${\mathit{x}}_k\leftarrow {\mathrm{\Phi }}_{k,d}{\tilde{\mathit{x}}}_d$ 15: end if 16: return Updated state

3.2.2. Delay Handling for Inter-UAV Information

In distributed cooperative localization, inter-UAV information suffers from communication delays. Let $t_c$ be the generation timestamp and $t_r$ the reception timestamp, with delay $\mathrm{\Delta }t=t_r-t_c$. The delay handling mechanism consists of three steps.

State Storage: A sliding window buffer maintains historical states $\{{\mathit{x}}_{k-N+1},\dots ,{\mathit{x}}_k\}$ with timestamps $\{t_{k-N+1},\dots ,t_k\}$.

Backward Correction: Retrieve the historical state ${\mathit{x}}_d$ at timestamp $t_d=t_c$ from the buffer and compute the innovation:

$$\delta {\mathit{x}}_d={\mathit{K}}_d\left({\mathbf{z}}_d-{\mathbf{h}}_d\left({\mathit{x}}_d\right)\right)$$

(3)

where ${\mathbf{K}}_d$ is the Kalman gain at $t_d$. The corrected state is ${\tilde{\mathit{x}}}_d={\mathit{x}}_d+\delta {\mathit{x}}_d$.

Forward Propagation: Propagate ${\tilde{\mathit{x}}}_d$ from $t_d$ to the current time $t_k$ using the state transition matrix:

$${\mathit{x}}_k={\mathrm{\Phi }}_{k,d}{\tilde{\mathit{x}}}_d+{\mathbf{G}}_{k,d}{\mathbf{w}}_d$$

(4)

where ${\mathrm{\Phi }}_{k,d}\approx \mathbf{I}+\mathit{F}\mathrm{\Delta }t$ with $\mathit{F}={\displaystyle \frac{\partial \mathbf{f}}{\partial \mathit{x}}}$ being the Jacobian of the system dynamics.

For UWB ranging from neighboring UAVs with delay, the measurement model is:

$${\mathbf{z}}_{\mathrm{UWB}}^{\mathrm{delayed}}=\parallel {\mathit{p}}_k^{\mathrm{local}}-{\mathit{p}}_d^{\mathrm{neighbor}}\parallel +v_{\mathrm{UWB}}$$

(5)

where ${\mathit{p}}_d^{\mathrm{neighbor}}$ is the stored position at $t_d$, with uncertainty explicitly modeled in the measurement noise.

This scheme achieves comprehensive temporal alignment: onboard sensors are synchronized through IMU interpolation, while inter-UAV delays are handled through backward correction and forward propagation, ensuring accurate cooperative localization under communication delays.

3.3. Swarm Absolute Pose and Attitude Initialization

In multi-UAV swarm systems, accurate initialization of absolute positions and orientations is crucial for subsequent collaborative localization tasks. Traditional methods often rely on GNSS or pre-deployed infrastructure, which are not available in GNSS-denied environments. To address this challenge, we propose a novel swarm absolute pose initialization method based on ranging and geomagnetic information.

Our approach utilizes IMU (Inertial Measurement Unit) to estimate roll and pitch angles, magnetometer (compass) to estimate yaw angle, and inter-node ranging information to estimate relative positions. These measurements are fused to estimate the absolute positions and orientations of the UAV swarm in the ENU (East-North-Up) coordinate system.

The initialization process consists of two main steps:

First, individual UAV attitude estimation: Each UAV independently estimates its attitude angles (roll, pitch, and yaw) in a static state. Roll and pitch angles are estimated using accelerometer measurements:

$${\mathrm{roll}}_0=arctan2\left({\displaystyle \frac{g_b\left[1\right]}{g_b\left[2\right]}}\right),{\mathrm{pitch}}_0=arctan2\left({\displaystyle \frac{-g_b\left[0\right]}{\sqrt{g_b{\left[1\right]}^2+g_b{\left[2\right]}^2}}}\right)$$

(6)

where ${\mathbf{g}}_b={[g_x,g_y,g_z]}^T$ is the average gravity vector measured by the accelerometer in the body frame.

Yaw angle is estimated using calibrated magnetometer measurements. The raw magnetometer readings are first calibrated to correct for environmental disturbances:

$$\left[\begin{array}{c}{\mathit{M}}_{xx}\\ {\mathit{M}}_{yx}\\ {\mathit{M}}_{zx}\end{array}\right]=\left[\begin{array}{ccc}{k}_{xx}& k_{xy}& k_{xz}\\ k_{yx}& k_{yy}& k_{yz}\\ k_{zx}& k_{zy}& k_{zz}\end{array}\right]·\left(\left[\begin{array}{c}{\mathit{M}}_x\\ {\mathit{M}}_y\\ {\mathit{M}}_z\end{array}\right]-\left[\begin{array}{c}{b}_{mx}\\ b_{my}\\ b_{mz}\end{array}\right]\right)$$

(7)

where $k_{ij}$ are scale factors and cross-axis sensitivity coefficients, and $b_m$ are bias parameters. The calibrated magnetometer measurements are then transformed to the navigation frame:

$${\mathit{h}}_n=\mathit{R}{({\mathrm{roll}}_0,{\mathrm{pitch}}_0,0)}^{-1}·{\mathit{m}}_b$$

(8)

and the yaw angle is computed as:

$${\mathrm{yaw}}_0=arctan2\left({\displaystyle \frac{h_n\left[1\right]}{h_n\left[0\right]}}\right)-D$$

(9)

where D is the magnetic declination obtained from geomagnetic maps.

Second, initialization of absolute position: The first UAV in the swarm (i = 1) is selected as the ENU coordinate origin ${\mathit{P}}_1={[0,0,0]}^T$. Then move the drone for a short distance, and combine the output of VIO and the geomagnetic heading angle to obtain the displacement vector of the drone in the ENU coordinate system. Then, the MDS-MAP method proposed in this paper can be used to obtain the absolute position coordinates of the entire UAV swarm under the ENU coordinate system. The pseudocode of Algorithm 2 is as follows.

This algorithm takes the number of UAV nodes, pre- and post-movement distance matrices of Node 1, and its absolute displacement as inputs. It solves relative coordinates via MDS, aligns coordinate systems using non-moving nodes, corrects orientation, and applies global translation to output all nodes’ absolute positions, providing fast, robust, and accurate initialization for swarm collaborative localization. There is also a commonly used cluster position initialization method, which is to directly use ranging information to build residuals, build a joint optimization problem and solve it. Its ranging residual construction method is defined as ${\sum }_{i,j}\parallel d_{ij}-\parallel P_i-P_j{\parallel \parallel }^2$ . Obviously, this approach will introduce a large amount of non-convexity into the optimization problem, which may cause the optimization problem to fall into a local optimal solution, resulting in a decrease in final accuracy. However, the method proposed in this paper not only avoids the problem of non-convex optimization by using algebraic methods but also greatly simplifies the calculation and improves the computational efficiency. We will also conduct experimental verification and analysis in the subsequent experimental evaluation process.

Algorithm 2 MDS-MAP Absolute Localization Algorithm.
1: function MDS_MAP_Localization(N, ${\mathbf{D}}_{\mathrm{pre}}$, ${\mathbf{D}}_{\mathrm{post}}$, $\mathrm{\Delta }{\mathbf{X}}_{\mathrm{abs}}$) N: Number of UAVs,
2: ${\mathbf{D}}_{\mathrm{pre}}$: Distance matrix before Node 1 moves,
3: ${\mathbf{D}}_{\mathrm{post}}$: Distance matrix after Node 1 moves,
4: $\mathrm{\Delta }{\mathbf{X}}_{\mathrm{abs}}$: Node 1’s absolute displacement ${\mathbf{X}}_{\mathrm{abs}}$: Absolute positions of all UAVs
5:      1. Compute Relative Coordinates via MDS
6:      $J={\mathbf{I}}_N-{\displaystyle \frac{1}{N}}{\mathbf{1}}_N{\mathbf{1}}_N^T$	▹ Centralization matrix
7:      ${\mathbf{B}}_{\mathrm{pre}}=-{\displaystyle \frac{1}{2}}J{\mathbf{D}}_{\mathrm{pre}}^{2}J$, $[{\mathbf{V}}_{\mathrm{pre}},{\mathrm{\Lambda }}_{\mathrm{pre}}]=\mathrm{eig}\left({\mathbf{B}}_{\mathrm{pre}}\right)$
8:      ${\mathrm{\Lambda }}_{\mathrm{pre}}^{\mathrm{sorted}}=\mathrm{sort}(\mathrm{diag}\left({\mathrm{\Lambda }}_{\mathrm{pre}}\right),\text{’}\mathrm{descend}\text{’})$
9:      ${\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}={\mathbf{V}}_{\mathrm{pre}}(:,1:2)\times \sqrt{max({\mathrm{\Lambda }}_{\mathrm{pre}}^{\mathrm{sorted}}(1:2),0)}$
10:      ${\mathbf{B}}_{\mathrm{post}}=-{\displaystyle \frac{1}{2}}J{\mathbf{D}}_{\mathrm{post}}^{2}J$, $[{\mathbf{V}}_{\mathrm{post}},{\mathrm{\Lambda }}_{\mathrm{post}}]=\mathrm{eig}\left({\mathbf{B}}_{\mathrm{post}}\right)$
11:      ${\mathrm{\Lambda }}_{\mathrm{post}}^{\mathrm{sorted}}=\mathrm{sort}(\mathrm{diag}\left({\mathrm{\Lambda }}_{\mathrm{post}}\right),\text{’}\mathrm{descend}\text{’})$
12:      ${\mathbf{X}}_{\mathrm{rel},\mathrm{post}}={\mathbf{V}}_{\mathrm{post}}(:,1:2)\times \sqrt{max({\mathrm{\Lambda }}_{\mathrm{post}}^{\mathrm{sorted}}(1:2),0)}$
13:      2. Align Coordinate Systems
14:      $S=\{2,3,...,N\}$	▹ Non-moving nodes
15:      ${\mathbf{c}}_{\mathrm{pre}}=\mathrm{mean}\left({\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}(S,:)\right)$, ${\mathbf{c}}_{\mathrm{post}}=\mathrm{mean}\left({\mathbf{X}}_{\mathrm{rel},\mathrm{post}}(S,:)\right)$
16:      ${\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}^{\mathrm{cent}}={\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}-{\mathbf{c}}_{\mathrm{pre}}$, ${\mathbf{X}}_{\mathrm{rel},\mathrm{post}}^{\mathrm{cent}}={\mathbf{X}}_{\mathrm{rel},\mathrm{post}}-{\mathbf{c}}_{\mathrm{post}}$
17:      $\mathbf{P}={\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}^{\mathrm{cent}}{(S,:)}^T{\mathbf{X}}_{\mathrm{rel},\mathrm{post}}^{\mathrm{cent}}(S,:)$, $[\mathbf{U},\Sigma ,\mathbf{V}]=\mathrm{svd}\left(\mathbf{P}\right)$, $\mathbf{R}=\mathbf{U}{\mathbf{V}}^T$
18:      ${\mathbf{X}}_{\mathrm{rel},\mathrm{post}}^{\mathrm{align}}={(\mathbf{R}\times {{\mathbf{X}}_{\mathrm{rel},\mathrm{post}}^{\mathrm{cent}}}^T)}^T+{\mathbf{c}}_{\mathrm{pre}}$
19:      3. Estimate Absolute Heading and Correct
20:      $\mathrm{\Delta }{\mathbf{X}}_{\mathrm{rel}}={\mathbf{X}}_{\mathrm{rel},\mathrm{post}}^{\mathrm{align}}(1,:)-{\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}(1,:)$
21:     ${\theta }_{\mathrm{rel}}=\mathrm{atan}2(\mathrm{\Delta }{\mathbf{X}}_{\mathrm{rel}}\left(2\right),\mathrm{\Delta }{\mathbf{X}}_{\mathrm{rel}}\left(1\right))$, ${\theta }_{\mathrm{abs}}=\mathrm{atan}2(\mathrm{\Delta }{\mathbf{X}}_{\mathrm{abs}}\left(2\right),\mathrm{\Delta }{\mathbf{X}}_{\mathrm{abs}}\left(1\right))$
22:     $\widehat{\theta }={\theta }_{\mathrm{abs}}-{\theta }_{\mathrm{rel}}$
23:     ${\mathbf{R}}_{\mathrm{yaw}}=\left[\begin{array}{cc}cos\widehat{\theta }& -sin\widehat{\theta }\\ sin\widehat{\theta }& cos\widehat{\theta }\end{array}\right]$
24:     ${\mathbf{X}}_{\mathrm{rel},\mathrm{rot}}={({\mathbf{R}}_{\mathrm{yaw}}\times {\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}^T)}^T$
25:     4. Compute Absolute Positions
26:     $\mathsf{t}=\mathrm{\Delta }{\mathbf{X}}_{\mathrm{abs}}-({\mathbf{X}}_{\mathrm{rel},\mathrm{rot}}(1,:)-{\mathbf{X}}_{\mathrm{rel},\mathrm{pre}}(1,:))$	▹ Global translation
27:     ${\mathbf{X}}_{\mathrm{abs}}={\mathbf{X}}_{\mathrm{rel},\mathrm{rot}}+\mathsf{t}$
return  ${\mathbf{X}}_{\mathrm{abs}}$
28: end function

3.4. Filter State and Propagation

In the proposed multi-UAV collaborative navigation system, we employ the sliding window EKF for state estimation, which is developed from OpenVINS [37]. The state of any drone in the drone cluster is defined as follows: The error state vector $\delta \mathit{x}$ consists of the current INS error parameter $\delta {\mathit{x}}_I$, vision measurement key frame $\delta {\mathit{x}}_{K{F}_{Visual}}$, UWB measurement key frame $\delta {\mathit{x}}_{K{F}_{UWB}}$, magnetic measurement key frame $\delta {\mathit{x}}_{K{F}_{MAG}}$, and single time offset $\delta {\mathbf{c}}_I$ between the IMU and the camera clock. They can be defined as:

$$\delta \mathit{x}={\left[\delta {\mathit{x}}_I,\delta {\mathit{x}}_{K{F}_{\mathrm{Visual}}},\delta {\mathit{x}}_{K{F}_{\mathrm{UWB}}},\delta {\mathit{x}}_{K{F}_{\mathrm{MAG}}},{\delta c}_I\right]}^T$$

(10)

where:

$$\delta {\mathit{x}}_I=\left[\delta {\theta }_b^w,\delta {\mathit{p}}_b^w,\delta {\mathit{v}}^w,\delta {\mathit{b}}_g,\delta {\mathit{b}}_a\right]$$

(11)

$$\delta {\mathit{x}}_{K{F}_{Visual}}=\left[\delta {\mathit{x}}_{K{F}_{Visual},0},\delta {\mathit{x}}_{K{F}_{Visual},1},\dots ,\delta {\mathit{x}}_{K{F}_{Visual},N-1}\right]$$

(12)

$$\delta {\mathit{x}}_{K{F}_{UWB}}=\left[\delta {\mathit{x}}_{K{F}_{UWB},0},\delta {\mathit{x}}_{K{F}_{UWB},1},\dots ,\delta {\mathit{x}}_{K{F}_{UWB},M-1}\right]$$

(13)

$$\delta {\mathit{x}}_{K{F}_{MAG}}=\left[\delta {\mathit{x}}_{K{F}_{MAG},0},\delta {\mathit{x}}_{K{F}_{MAG},1},\dots ,\delta {\mathit{x}}_{K{F}_{MAG},M-1}\right]$$

(14)

Here, the current inertial localization error state $\delta {\mathit{x}}_I$ includes the attitude error $\delta {\mathit{\theta }}_b^w$, position error $\delta {\mathit{p}}_b^w$, speed error $\delta {\mathit{v}}^w$, gyroscope deviation $\delta {\mathit{b}}_g$ and the accelerometer deviation $\delta {\mathit{b}}_a$ of the drone. The vision measurement key frame $\delta {\mathit{x}}_{K{F}_{Visual}}$ and the UWB measurement key frame $\delta {\mathit{x}}_{K{F}_{UWB}}$ are defined as follows:

$$\delta {\mathit{x}}_{K{F}_{Visual,n}}=\left[\delta {\mathit{\theta }}_{b_n}^w,\delta {\mathit{p}}_{b_n}^w\right]$$

(15)

$$\delta {\mathit{x}}_{K{F}_{UWB,m}}=\left[\delta {\mathit{\theta }}_{b_m}^w,\delta {\mathit{p}}_{b_m}^w\right]$$

(16)

$$\delta {\mathit{x}}_{K{F}_{MAG,l}}=\left[\delta {\mathit{\theta }}_{b_l}^w,\delta {\mathit{p}}_{b_l}^w\right]$$

(17)

where $\delta {\mathit{\theta }}_{b_n}^w$ and $\delta {\mathit{p}}_{b_n}^w$ are the IMU attitude and position errors at the n-th vision key frame time. $\delta {\mathit{\theta }}_{b_m}^w$ and $\delta {\mathit{p}}_{b_m}^w$ are the IMU attitude and position errors at the m-th UWB key frame time. $\delta {\mathit{\theta }}_{b_l}^w$ and $\delta {\mathit{p}}_{b_l}^w$ are the IMU attitude and position errors at the l-th MAG key frame time. The true state $\mathit{x}$ can be obtained from the estimated state $\widehat{\mathit{x}}$ and the error state $\delta \mathit{x}$:

$$\mathit{x}=\widehat{\mathit{x}}⊞\delta \mathit{x}$$

(18)

For the attitude error, the operator ⊞ is given by

$$\mathit{R}=\widehat{\mathit{R}}\mathrm{Exp}\left(\delta \mathit{\theta }\right)\approx \widehat{\mathit{R}}(\mathit{I}+{\left(\delta \theta \right)}_{\times })$$

(19)

For other states, the operator ⊞ is equivalent to Euclidean addition. When the IMU measurement is available, the INS mechanization is conducted to output the high-frequency prior pose. Meanwhile, the forward propagation of the whole error state and its covariance is similar to the OpenVINS [37] and will not be repeated here. When a new key frame is added to the sliding window, state augmentation and covariance update are needed to enhance the state into the state vector, and the corresponding covariance ${\mathit{P}}_{n\times n}$ is augmented as:

$${\mathit{P}}_{(n+6)\times (n+6)}=\left[\begin{array}{c}{\mathit{I}}_{n\times n}\\ {\mathit{J}}_{6\times n}\end{array}\right]{\mathit{P}}_{n\times n}{\left[\begin{array}{c}{\mathit{I}}_{n\times n}\\ {\mathit{J}}_{6\times n}\end{array}\right]}^T$$

(20)

where ${\mathit{J}}_{6\times n}$ is the Jacobian matrix, which represents the relationship between the new added state and the original state. When the sliding window exceeds its maximum length, it will be marginalized and the state and covariance of the oldest keyframe will be directly deleted [37].

3.5. Visual Measurement Based on PO Theory

In order to simplify the complexity of the 3D reconstruction process in the traditional VIO system, to reduce the amount of computation, and to avoid the accuracy limitations imposed by direct 3D reconstruction, with reference to the PO theory [38], we reconstructed the measurement model represented only by pixel coordinates and relative positional pose. In PO theory, the description of multi-view geometry can be realized by using only camera poses. This means that instead of directly estimating the 3D coordinates of the feature points in the scene, we infer their positional relationships from the relative poses between the cameras. Assuming that the projection of feature point $\mathit{p}$ in image i can be represented as ${\mathit{p}}_i=\mathit{K}\left[{\mathit{R}}_{ij}\right|{\mathit{t}}_{ij}]{\mathit{p}}_j$, where $\mathit{K}$ is the camera internal reference matrix, ${\mathit{R}}_{ij}$ and ${\mathit{t}}_{ij}$ are the rotation and translation from camera i to camera j, respectively, the reprojection error ${\mathit{e}}_i$ can be represented by the camera poses in PO theory without directly using the 3D coordinates of the feature points.   

$${\mathit{e}}_i={\mathit{p}}_i-\mathit{K}\left[{\mathit{R}}_{ij}\right|{\mathit{t}}_{ij}]{\mathit{p}}_j$$

(21)

Therefore, the geometric description of multiple views based on PO theory can be expressed as follows.

$$D(j,k)=\left\{d(j,i)\mid {\mathit{p}}_{c_i}=d{(j,k)}^{c_i}{\mathit{R}}_{c_j}{\mathit{p}}_{c_j}+{}^{c_i}\mathit{t}_{c_j}\mid 1\le i\le n,i\ne j\right\}$$

(22)

where $d(j,i)$ is the constraint between images i and j, ${\mathit{p}}_{c_i}$ is the normalized coordinates of the feature points in images i, and ${}^{c_i}\mathit{R}_{c_j}$ and ${}^{c_i}\mathit{t}_{c_j}$ are the rotation and translation from image i to image j, respectively. Thus the reprojection error can be redefined as follows [38,39].

$$r_{c_i}^{\left(l\right)}=∥{\mathit{p}}_{c_i}^{\left(l\right)}-{\widehat{\mathit{p}}}_{c_i}^{\left(l\right)}∥=∥{\displaystyle \frac{{}^{c_i}\mathit{p}_{f_l}^{\left(PO\right)}}{{\mathit{e}}_3^T{}^{c_i}\mathit{p}_{f_l}^{\left(PO\right)}}}-{\mathit{p}}_{c_i}^{\left(l\right)}∥$$

(23)

where $r_{c_i}^{\left(l\right)}$ is the reprojection error of the l-th feature point in the i-th image, ${}^{c_i}\mathit{p}_{f_l}^{\left(PO\right)}$ is the projection obtained by using only the camera pose and 2D features, and ${\mathit{e}}_3^T$ is a transformation vector for converting 3D vectors to 2D. Thus the measurement model based on PO theory can be represented as follows [13].

$${\mathit{r}}_{c_i}^{\left(l\right)}\approx {\mathit{H}}_{x_i}^{\left(l\right)}\delta \mathit{x}+{\mathit{n}}_i^{\left(l\right)}$$

(24)

$$\begin{array}{cc}\hfill {\mathit{H}}_{x_i}^{\left(l\right)}& ={\displaystyle \frac{\partial {\mathit{r}}_{C_i}^{\left(l\right)}}{\partial \delta \mathit{x}}}={\displaystyle \frac{\partial {\mathit{r}}_{C_i}^{\left(l\right)}}{\partial {}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}}·{\displaystyle \frac{\partial {}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}{\partial \delta \mathit{x}}}\hfill \\ \hfill & =\left[\begin{array}{ccc}{\displaystyle \frac{1}{e_3^T{}^{c_i}\mathit{p}_{f_l}^{\left(PO\right)}}}& 0& -{\displaystyle \frac{e_1^T{}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}{{\left(e_3^T{}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}\right)}^2}}\\ 0& {\displaystyle \frac{1}{e_3^T{}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}}& -{\displaystyle \frac{e_2^T{}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}{{\left(e_3^T{}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}\right)}^2}}\end{array}\right]\hfill \\ \hfill & ·\left[\begin{array}{cccc}{\mathbf{0}}_{3\times 15}& {\displaystyle \frac{\partial {}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}{{\partial }_{b_m}^G\varphi }}& {\displaystyle \frac{\partial {}^{C_i}\mathit{p}_{f_l}^{\left(PO\right)}}{{\partial \delta }^G{\mathit{p}}_{b_m}}}& \dots \end{array}\right]\hfill \end{array}$$

(25)

As a result, the new measurement model is represented only in pixel coordinates and system attitude and is fully decoupled from the 3D features, avoiding the effects of inaccurate 3D reconstruction processes. The PO-based visual measurement model significantly reduces the computational complexity and avoids the error accumulation caused by 3D point reconstruction while maintaining high positioning accuracy.

3.6. Collaborative Localization with Anchor-Free UWB Measurement and Magnetic Assisted

This section presents our collaborative localization framework that tightly integrates anchor-free UWB measurements and magnetic heading constraints. The framework shown in Figure 2 addresses the challenges of unknown correlations between UAV state estimates and robustly handles measurement noise and uncertainties.

3.6.1. Magnetic Heading Assisted Localization

Magnetic sensors provide valuable absolute heading information that helps constrain the unobservable yaw dimension identified in our observability analysis. Here we detail the magnetic heading measurement model and robustness enhancements.

Magnetic Heading Measurement Model

The geomagnetic heading provides an absolute measurement of the yaw angle in the IMU state, with the following measurement model:

$${\psi }_o\left(t\right)=\psi \left(t\right)+n_m\left(t\right)$$

(26)

where $n_m\left(t\right)$ is Gaussian white noise and ${\psi }_o\left(t\right)$ is the observed heading at time t. The residual between the estimated heading ${\psi }_e\left(t\right)$ and geomagnetic heading in the filter is:

$$\mathit{r}={\psi }_o\left(t\right)-{\psi }_e\left(t\right)$$

(27)

The Jacobian matrix for the heading measurement is straightforward:

$${\mathbf{H}}_{\mathrm{mag}}=\left[\begin{array}{ccc}0& 0& 1\end{array}\right]$$

(28)

Robust Magnetic Heading Estimation

To enhance the reliability of magnetic heading estimation in challenging environments, we implement five key strategies with corresponding mathematical formulations:

1.

Magnetic Calibration

We perform automatic pre-mission calibration to compensate for hard and soft iron distortions. The calibration process estimates the following parameters:

-

Hard iron bias: $\mathit{b}={[b_x,b_y,b_z]}^T$

-

Soft iron matrix: $\mathit{S}=\left[\begin{array}{ccc}{s}_{xx}& s_{xy}& s_{xz}\\ s_{yx}& s_{yy}& s_{yz}\\ s_{zx}& s_{zy}& s_{zz}\end{array}\right]$

The calibrated magnetic field vector ${\mathit{m}}_{cal}$ is computed as:

$${\mathit{m}}_{cal}=\mathit{S}({\mathit{m}}_{raw}-\mathit{b})$$

(29)

2.

Adaptive Magnetic Weighting

The weight $w_{mag}$ for magnetic measurements is dynamically adjusted based on consistency with IMU and visual data:

$$w_{mag}=exp\left(-{\displaystyle \frac{\parallel {\psi }_{mag}-{\psi }_{ref}{\parallel }^2}{2{\sigma }_{consist}^2}}\right)$$

(30)

where ${\psi }_{ref}$ is the reference heading from IMU/visual fusion and ${\sigma }_{consist}$ is the consistency threshold.

3.

Magnetic Anomaly Detection

We detect magnetic anomalies by comparing current measurements with geomagnetic map predictions:

$$\zeta ={\displaystyle \frac{\parallel {\mathit{m}}_{cal}-{\mathit{m}}_{map}\parallel }{{\sigma }_{map}}}$$

(31)

Measurements are rejected if $\zeta >{\zeta }_{thresh}$, where ${\zeta }_{thresh}$ is the anomaly threshold.

4.

Heading Complementary Filter

A complementary filter fuses magnetic heading ${\psi }_{mag}$ with gyroscope-derived heading ${\psi }_{gyro}$:

$${\psi }_{comp}\left(t\right)=\alpha ·{\psi }_{comp}(t-1)+(1-\alpha )·{\psi }_{mag}\left(t\right)+\beta ·\omega \left(t\right)$$

(32)

where $\omega \left(t\right)$ is the gyroscope angular velocity, and $\alpha ,\beta$ are filter gains ($0<\alpha <1$, $\beta >0$).

5.

Multi-Sensor Heading Fusion

When magnetic measurements are unreliable, we fuse heading estimates from multiple sensors using weighted least squares:

$${\psi }_{fused}={\left(\sum _{k=1}^K{w}_k{\mathit{H}}_k^T{\mathit{R}}_k^{-1}{\mathit{H}}_k\right)}^{-1}\sum _{k=1}^K{w}_k{\mathit{H}}_k^T{\mathit{R}}_k^{-1}{\psi }_k$$

(33)

where K is the number of sensors, ${\psi }_k$ are individual heading estimates, ${\mathit{R}}_k$ are their covariance matrices, and $w_k$ are adaptive weights.

3.6.2. Anchor-Free UWB Measurement Model

UWB sensors provide direct peer-to-peer distance measurements between neighboring UAVs, which are crucial for maintaining relative position consistency in the swarm.

Basic UWB Measurement Model

For UAV i and its neighbor j, the UWB ranging measurement is modeled as:

$$d_{ij}=\parallel {\mathit{p}}_i-{\mathit{p}}_j\parallel +v_{uwb,ij}$$

(34)

where ${\mathit{p}}_i$ and ${\mathit{p}}_j$ are the positions of UAV i and j in the global frame, and $v_{uwb,ij}$ is Gaussian measurement noise.

Cumulative UWB Measurement with Neighbor Uncertainty

In distributed systems, neighboring UAVs’ position estimates contain uncertainties. We model these uncertainties by constructing a cumulative UWB measurement for UAV i with M neighbors:

$${\mathit{r}}_i=\sum _{j\in {\mathcal{N}}_i}{w}_{ij}\left(\parallel {\tilde{\mathit{p}}}_G^i-{\tilde{\mathit{p}}}_G^j\parallel -d_{ij}+n_{ij}\right)$$

(35)

where ${\mathcal{N}}_i$ is the set of neighboring UAVs, $w_{ij}$ are dynamically adjusted weights (satisfying ${\sum }_{j\in {\mathcal{N}}_i}{w}_{ij}=1$) and ${\tilde{\mathit{p}}}_G^i$ represents the estimated position.

The Jacobian matrix for this cumulative measurement is:

$${\mathbf{H}}_{UW{B}_i}=\sum _{j\in {\mathcal{N}}_i}{w}_{ij}\left[{\displaystyle \frac{x_i-x_j}{d_{ij}}}{\displaystyle \frac{y_i-y_j}{d_{ij}}}{\displaystyle \frac{z_i-z_j}{d_{ij}}}-{\displaystyle \frac{x_i-x_j}{d_{ij}}}-{\displaystyle \frac{y_i-y_j}{d_{ij}}}-{\displaystyle \frac{z_i-z_j}{d_{ij}}}\right]$$

(36)

Uncertainty Propagation from Neighbors

We explicitly model the uncertainty introduced by neighboring UAVs’ position errors. The observation noise matrix for the cumulative UWB measurement is:

$${\mathbf{R}}_{UW{B}_i}={\delta }_d^2+\sum _{j\in {\mathcal{N}}_i}{w}_{ij}^2{\mathbf{H}}_{UW{B}_{ij}}{\mathit{P}}_{p_G^j}{\mathbf{H}}_{UW{B}_{ij}}^T$$

(37)

where ${\delta }_d^2$ is the UWB ranging noise variance, and ${\mathit{P}}_{p_G^j}$ is the position covariance matrix of UAV j.

3.6.3. Covariance Intersection (CI) for Consistent UWB Fusion

To address unknown correlations between UAV state estimates, we incorporate Covariance Intersection (CI) into our UWB measurement fusion process.

CI Fusion Principle

When fusing state estimate ${\widehat{\mathit{x}}}_j$ with covariance ${\mathit{P}}_j$ from UAV j with local estimate ${\widehat{\mathit{x}}}_i$ and ${\mathit{P}}_i$, CI computes the fused state ${\widehat{\mathit{x}}}_{CI}$ and covariance ${\mathit{P}}_{CI}$ as:

$$\begin{array}{}\mathrm{(38)}& \hfill {\mathit{P}}_{CI}^{-1}& =(1-\alpha ){\mathit{P}}_i^{-1}+\alpha {\mathit{P}}_j^{-1}\hfill \mathrm{(39)}& \hfill {\widehat{\mathit{x}}}_{CI}& ={\mathit{P}}_{CI}\left((1-\alpha ){\mathit{P}}_i^{-1}{\widehat{\mathit{x}}}_i+\alpha {\mathit{P}}_j^{-1}{\widehat{\mathit{x}}}_j\right)\hfill \end{array}$$

The optimal mixing parameter ${\alpha }^{*}$ minimizes the trace of ${\mathit{p}}_{CI}$ while maintaining consistency:

$${\alpha }^{*}=arg\underset{\alpha \in [0,1]}{min}\mathrm{tr}\left({\mathit{P}}_{CI}\left(\alpha \right)\right)$$

(40)

CI-Enhanced Measurement Noise

The CI-derived covariance is incorporated into the UWB measurement noise matrix:

$${\mathbf{R}}_{UW{B}_i}^{CI}={\delta }_d^2+\sum _{j\in {\mathcal{N}}_i}{w}_{ij}^2{\mathbf{H}}_{UW{B}_{ij}}{\mathbf{P}}_{C{I}_j}{\mathbf{H}}_{UW{B}_{ij}}^T$$

(41)

The CI method ensures that the fused state estimate is consistent even when the correlations between UAV estimates are unknown or time-varying, enhancing the robustness and consistency of our cooperative localization system.

3.6.4. Integrated UWB Range Update Algorithm

Algorithm 3 integrates CI fusion, chi-squared validation, and EKF update into a comprehensive UWB range update process:

This integrated algorithm ensures robust and consistent fusion of UWB measurements in our anchor-free collaborative localization system, effectively constraining the divergence of unobservable dimensions identified in our observability analysis.

The ranging-aided SLAM (RA-SLAM) problem introduces additional non-convexity due to distance measurement models, making it challenging to obtain globally optimal solutions. The maximum a posteriori (MAP) estimation of RA-SLAM is a non-convex optimization problem that heavily relies on good initial values. Traditional methods for solving this problem often face challenges such as local optima and high computational complexity.

Algorithm 3 UWB Range Update with Chi-squared Test and Covariance Intersection Fusion.
1:  function $UW{B}_{R}ang{e}_{U}pdate$(${\mathbf{Z}}_k$, ${\widehat{\mathbf{X}}}_i$, $P_i$, ${\{{\widehat{\mathbf{X}}}_j,P_j\}}_{j\in {\mathcal{N}}_i}$) ${\mathbf{Z}}_k$: UWB measurement, ${\widehat{\mathbf{X}}}_i$: Local state estimate,       $P_i$: Local covariance, ${\{{\widehat{\mathbf{X}}}_j,P_j\}}_{j\in {\mathcal{N}}_i}$: Neighbor states and covariances, ${\widehat{X}}_i^{updated}$: Updated state estimate,          $P_i^{updated}$: Updated covariance
2:      1. Covariance Intersection Fusion
3:      for all $j\in {\mathcal{N}}_i$ do
4:            ${\alpha }^{*}=arg{min}_{\alpha \in [0,1]}\mathrm{tr}\left({\mathit{P}}_{CI}\left(\alpha \right)\right)$	▹ Optimal fusion weight
5:            ${\mathit{P}}_{C{I}_j}^{-1}=(1-{\alpha }^{*}){\mathit{P}}_i^{-1}+{\alpha }^{*}{\mathit{P}}_j^{-1}$	▹ Fused covariance
6:            ${\widehat{\mathit{X}}}_{C{I}_j}={\mathit{P}}_{C{I}_j}\left((1-{\alpha }^{*}){\mathit{P}}_i^{-1}{\widehat{\mathit{X}}}_i+{\alpha }^{*}{\mathit{P}}_j^{-1}{\widehat{\mathit{X}}}_j\right)$	▹ Fused state
7:            ${\mathbf{R}}_{UW{B}_{ij}}^{CI}={\delta }_d^2+w_{ij}^2{\mathbf{H}}_{UW{B}_{ij}}{\mathit{P}}_{C{I}_j}{\mathbf{H}}_{UW{B}_{ij}}^T$	▹ CI-enhanced noise
8:     end for
9:     ${\mathbf{R}}_{UWB}={\sum }_{j\in {\mathcal{N}}_i}{w}_{ij}{\mathbf{R}}_{UW{B}_{ij}}^{CI}$	▹ Total noise matrix
10:     2. Chi-squared Measurement Validation
11:     $\mathit{r}={\mathbf{Z}}_k-\mathit{h}\left({\widehat{\mathbf{X}}}_i\right)$	▹ Innovation
12:     $\mathbf{S}=\mathbf{H}{\mathit{P}}_i{\mathbf{H}}^T+{\mathit{R}}_{UWB}$	▹ Innovation covariance
13:     ${\chi }^2={\mathit{r}}^T{\mathbf{S}}^{-1}\mathit{r}$	▹ Chi-squared statistic
14:     3. Conditional EKF Update
15:     if ${\chi }^2<{\chi }_{thresh}^2$ then
16:           $[{\widehat{\mathbf{X}}}_i^{updated},P_i^{updated}]=\mathrm{EKFUpdate}({\widehat{\mathbf{X}}}_i,P_i,\mathit{r},\mathbf{H},\mathbf{S})$
17:     else
18:           $[{\widehat{\mathbf{X}}}_i^{updated},P_i^{updated}]=[{\widehat{\mathbf{X}}}_i,P_i]$	▹ Reject unreliable measurement
19:    end if            return  ${\widehat{\mathbf{X}}}_i^{updated},P_i^{updated}$
20:  end function

3.7. UWB Measurements Adaptive Adjustment Based on Chi-Squared Detection

In practical applications, UWB ranging measurements are often affected by non-line-of-sight (NLOS) propagation, multipath effects, and other environmental factors, resulting in outliers that can severely degrade localization performance. To address this issue, we propose an adaptive adjustment method for UWB measurements based on chi-squared detection, with alternative strategies when the chi-squared test fails.

For each UWB measurement, the innovation (residual) is computed as:

$${\mathbf{y}}_k={\mathbf{Z}}_k-{\mathbf{H}}_k{\widehat{\mathbf{X}}}_{k|k-1}$$

(42)

where ${\mathbf{Z}}_k$ is the measurement vector, ${\mathbf{H}}_k$ is the observation matrix, and ${\widehat{\mathbf{X}}}_{k|k-1}$ is the predicted state vector. The innovation covariance matrix and chi-squared statistic are then computed as:

$${\mathbf{S}}_k={\mathbf{H}}_k{P}_{k|k-1}{\mathbf{H}}_k^T+{\mathit{R}}_k$$

(43)

$${\lambda }_k={\mathbf{y}}_k^T{\mathbf{S}}_k^{-1}{\mathbf{y}}_k$$

(44)

where $P_{k|k-1}$ is the a priori error covariance matrix of the Kalman filter, and $R_k$ is the observation noise covariance matrix. In this article, the threshold of the chi-squared test is set to 95%, and the dimension of the residual vector is 6, so the corresponding threshold is 12.59.

When the chi-squared test fails (i.e., ${\lambda }_k\ge {\chi }^2\left(0.95\right)$), our system implements a weighted measurement update strategy instead of completely rejecting the measurement. The weight assigned to the measurement is inversely proportional to the chi-squared statistic:

$$w_{uwb}=exp\left(-{\displaystyle \frac{{\lambda }_k-{\chi }_{thresh}^2}{2{\sigma }_{uwb}^2}}\right)$$

(45)

where ${\sigma }_{uwb}$ is the UWB measurement noise parameter. This approach allows for slightly inconsistent measurements to still contribute to the state update with reduced influence, while strongly inconsistent measurements are effectively downweighted.

Finally, each UAV uses the standard EKF update formula with the computed weight to update UWB measurements and obtain state estimates after collaborative positioning.

3.8. Observability Analysis for Ranging + Odometry Swarm Systems

We conduct a rigorous observability analysis for anchor-free UAV swarms with only inter-UAV ranging and on-board odometry, deriving key properties for professional readers. For a swarm with N UAVs, the state vector $\mathbf{x}={[{\mathbf{x}}_1^T,{\mathbf{x}}_2^T,\dots ,{\mathbf{x}}_N^T]}^T$ includes each UAV’s position ${\mathbf{p}}_i={[x_i,y_i,z_i]}^T$, velocity ${\mathbf{v}}_i$, attitude ${\mathbf{q}}_i$, and IMU biases ${\mathbf{b}}_i$. Measurements include:

• On-board odometry (relative motion)
• Inter-UAV UWB ranging $d_{ij}=\parallel {\mathit{p}}_i-{\mathit{p}}_j\parallel +v_{uwb,ij}$

Consider a global transformation $\mathit{\tau }={[\delta x_G,\delta y_G,\delta z_G,\delta {\psi }_G]}^T$ representing 3D translation and yaw rotation. Applying this to the swarm state:

$$\begin{array}{}\mathrm{(46)}& \hfill {\mathit{p}}_i^{\prime }& ={\mathit{R}}_y\left(\delta {\psi }_G\right){\mathit{p}}_i+{[\delta x_G,\delta y_G,\delta z_G]}^T\hfill \mathrm{(47)}& \hfill {\mathit{v}}_i^{\prime }& ={\mathit{R}}_y\left(\delta {\psi }_G\right){\mathit{v}}_i\hfill \mathrm{(48)}& \hfill {\mathit{q}}_i^{\prime }& ={\mathit{q}}_i\otimes {\mathit{q}}_y\left(\delta {\psi }_G\right)\hfill \end{array}$$

This transformation leaves all measurements invariant:

-

Odometry measurements depend only on relative motion, so ${\mathit{v}}_i^{\prime }$ produces identical measurements to ${\mathit{v}}_i$

-

Ranging measurements are preserved: $\parallel {\mathit{p}}_i^{\prime }-{\mathit{p}}_j^{\prime }\parallel =\parallel {\mathit{p}}_i-{\mathit{p}}_j\parallel$ (rotation matrices preserve Euclidean distance)

The observability matrix $\mathcal{O}$ for the linearized system is constructed as:

$$\mathcal{O}=\left[\begin{array}{c}\mathbf{H}\\ \mathbf{H}\mathbf{F}\\ \mathbf{H}{\mathbf{F}}^2\\ ⋮\\ \mathbf{H}{\mathbf{F}}^{n-1}\end{array}\right]$$

(49)

where $\mathbf{H}$ is the combined measurement matrix, $\mathbf{F}$ is the block-diagonal state transition matrix, and $n=16N$ is the total state dimension.

Since $\mathit{\tau }$ leaves all measurements invariant, it lies in the null space of $\mathcal{O}$. This yields:

Theorem 1. 

For an anchor-free UAV swarm with only inter-UAV ranging and on-board odometry, and without initial absolute pose information, the absolute heading (yaw) and absolute position (3D translation) are unobservable, resulting in 4 unobservable dimensions.

The rank of $\mathcal{O}$ is thus at most $16N-4$, confirming the unobservable subspace.

While the 4 unobservable dimensions are fundamental, the divergence rate of these directions can be constrained. This is the core motivation for our absolute accuracy improvement mechanism: through strategic sensor fusion and drift mitigation, we can significantly slow the divergence of the unobservable directions, thereby improving absolute positioning accuracy without changing the fundamental observability properties.

3.9. Analysis of Absolute Accuracy Improvement Mechanism in Anchor-Free Collaborative Localization

Building upon the observability analysis, we now analyze how our proposed system improves the absolute positioning accuracy of anchor-free UAV swarms, even in the presence of 4 unobservable dimensions. While the fundamental unobservability cannot be eliminated without external absolute references, our research demonstrates that the absolute positioning accuracy can be significantly enhanced and network drift can be mitigated through three key mechanisms: (1) accurate absolute pose initialization using MDS-based methods fused with ranging, IMU, and geomagnetic information; (2) continuous accuracy improvement through multi-constraint fusion in a sliding window EKF framework; and (3) explicit network drift mitigation strategies. These mechanisms effectively constrain the divergence rate of the unobservable directions, leading to improved absolute positioning accuracy.

3.9.1. Swarm Initialization with Absolute Pose Estimation

As detailed in Section 3.2, our MDS-MAP initialization approach utilizes ranging measurements, IMU data, and geomagnetic information to establish accurate absolute positions for all UAVs at the beginning of the mission. This initialization process is crucial as it provides each UAV with a globally consistent reference frame, which serves as the foundation for subsequent collaborative localization. The accurate initialization of the swarm’s absolute pose significantly enhances the overall absolute positioning accuracy of the UAV cluster. By establishing a precise initial reference frame, the accumulated errors in subsequent navigation can be minimized, leading to more reliable and accurate positioning throughout the mission.

3.9.2. Continuous Accuracy Enhancement Through Multi-Constraint Fusion

During system operation, the sliding window EKF framework continuously fuses multiple sensor constraints to improve estimation accuracy and maintain long-term stability. The augmented state vector includes keyframes from visual and UWB measurements:

$$\delta \mathit{x}={\left[\delta {\mathit{x}}_I,\delta {\mathit{x}}_{K{F}_{\mathrm{Visual}}},\delta {\mathit{x}}_{K{F}_{\mathrm{UWB}}},\delta {\mathit{x}}_{K{F}_{\mathrm{MAG}}}\right]}^T$$

(50)

The observation model combines visual, ranging, and geomagnetic measurements:

$${\mathbf{z}}_k=\left[\begin{array}{c}{\mathbf{z}}_{vis}\\ {\mathbf{z}}_{uwb}\\ {\mathbf{z}}_{mag}\end{array}\right]=\left[\begin{array}{c}{\mathbf{h}}_{vis}\left(\mathit{x}\right)\\ {\mathbf{h}}_{uwb}\left(\mathit{x}\right)\\ {\mathbf{h}}_{mag}\left(\mathit{x}\right)\end{array}\right]+\left[\begin{array}{c}{\mathbf{n}}_{vis}\\ {\mathbf{n}}_{uwb}\\ {\mathbf{n}}_{mag}\end{array}\right]$$

(51)

This multi-constraint fusion significantly increases the rank of the information matrix, thereby improving the observability and accuracy of the system state estimation. The increased redundancy in measurements also enhances robustness against individual sensor failures or noise. Specifically:

• Visual constraints provide high-frequency relative motion information and help correct drift in the inertial navigation system.
• UWB ranging constraints offer direct peer-to-peer distance measurements, which are particularly valuable for maintaining inter-UAV positional consistency in the swarm.
• Geomagnetic constraints supply absolute heading information, which is essential for maintaining global orientation consistency.

3.9.3. Network Drift Mitigation Strategies

Building upon our observability analysis, we now focus on how our system constrains the divergence rate of the 4 unobservable dimensions. While the fundamental unobservability cannot be eliminated, our proposed strategies effectively mitigate the drift in these unobservable directions, thereby improving the absolute positioning accuracy. We implement three key strategies that directly address the divergence of the unobservable dimensions:

• Absolute Heading Constraint: By continuously incorporating geomagnetic measurements into the fusion framework, we provide a stable absolute reference for the swarm’s heading. While this does not change the fundamental unobservability of the yaw angle, it effectively constrains the divergence rate of the absolute heading. The geomagnetic measurements act as a virtual anchor for the yaw direction, preventing the entire swarm from rotating uncontrollably and maintaining consistent heading estimates across all UAVs.
• Relative Distance Consistency: The UWB ranging measurements between neighboring UAVs provide strong constraints on the relative positions of the swarm members. These constraints effectively limit the divergence of the unobservable translational dimensions. While the entire swarm can still translate rigidly, the relative distance constraints ensure that this translation occurs in a coordinated manner, preventing the swarm from dispersing and maintaining the relative formation. This coordinated behavior significantly reduces the effective drift rate of the absolute position estimates.
• Sliding Window Marginalization: The sliding window mechanism maintains a history of past states and measurements, allowing the system to detect and correct drift over time through loop closure-like effects. By marginalizing older states while retaining their information in the covariance matrix, the system creates implicit constraints that limit the divergence of all states, including the unobservable dimensions. The sliding window effectively extends the temporal horizon of the system, providing long-term consistency constraints that slow down the drift of the unobservable directions.

Collectively, these strategies effectively constrain the divergence rate of the 4 unobservable dimensions, even though they cannot eliminate the fundamental unobservability. By limiting the drift rate, our system significantly improves the absolute positioning accuracy of the swarm, making it suitable for practical applications in GNSS-denied environments.

3.10. Scalability Analysis

Through a series of key design considerations, this system achieves favorable scalability for large-scale unmanned aerial vehicle (UAV) swarms. Specifically, the Pose-Only (PO) theory improves computational efficiency by eliminating the need for 3D feature reconstruction; the sliding-window EKF limits the number of estimated states; and localized processing ensures a linear scaling of computational load. In addition, the system mitigates communication latency and packet loss via time alignment, synchronization, and delay compensation within the sliding window. For future deployment expansion, in terms of communication overhead, each UAV only communicates with neighboring UAVs within its communication range, thus ensuring a linear scaling of communication burden. A dynamic neighbor selection strategy further restricts communication to the nearest K neighbors. Regarding UWB signal management, the system implements the Time Division Multiple Access (TDMA) protocol and Frequency Hopping Spread Spectrum (FHSS) technology to avoid signal collisions in dense swarms. Analysis results show that this system can support swarms of 100 UAVs with appropriate parameter tuning, and is expected to accommodate even larger swarms through additional optimizations such as hierarchical communication and UWB modulation enhancement.

4. Experimental Evaluation

This section provides a comprehensive performance evaluation of our proposed collaborative navigation framework and benchmarks it against the SOTA VIO method and collaborative positioning method, which are commonly used for drone positioning in GNSS-denied situations. Experimental validation was conducted using datasets collected from both the AirSim simulation platform and real-world environments. All evaluations were performed on a laptop configuration (13th Gen Intel® Core™ i9-13900HX 2.20 GHz processor) to ensure practical applicability. This paper employs EVO (Python package for the evaluation of odometry and SLAM), version 1.12.0, for data assessment. When creating visualizations, this study utilizes Python 3.9 and the Matplotlib plotting library (version 3.6.3).

4.1. Swarm Position Initialization Simulation Experiment

To validate the effectiveness of the proposed MDS-MAP-based swarm absolute position initialization method, we conducted comparative experiments against the traditional GTSAM-based optimization approach that builds ranging information residuals. We simulated 12 nodes, set the ranging error to 0.5 m, 500 Montocaro experiments, and the final results were averaged. Multiple Monte Carlo simulations were performed to evaluate both methods in terms of average positioning accuracy and computational time.

The traditional approach formulates the initialization problem as a non-linear optimization task by constructing ranging residuals defined as ${\sum }_{i,j}\parallel d_{ij}-\parallel P_i-P_j{\parallel \parallel }^2$, where $d_{ij}$ represents the measured distance between UAVs i and j, and $P_i$, $P_j$ are their respective absolute positions. This optimization problem is solved using the GTSAM library, which iteratively minimizes the residuals to estimate the absolute positions.

In contrast, our proposed MDS-MAP method avoids the non-convexity issues inherent in the optimization approach by employing algebraic methods based on Multidimensional Scaling (MDS) and coordinate alignment techniques. This approach directly computes the relative positions using MDS, aligns the coordinate systems using non-moving nodes, estimates the absolute heading, and finally solves for the absolute positions through global translation. Experimental results, as shown in

Table 1. Comparison of Swarm Absolute Position Initialization Methods.

Method	Mean Error (m)	Variance	95% CI	Mean Time (ms)
GTSAM	0.34	0.045	[0.32, 0.36]	152.6
MDS-MAP (Ours)	0.19	0.012	[0.18, 0.20]	25.8

.

The results in Table 1 clearly show that our proposed MDS-MAP method achieves 44.1% reduction in positioning error and more than 80% reduction in computation time compared to the GTSAM-based optimization approach. To provide more reliable statistical results, we have included error variance and 95% confidence intervals based on 500 Monte Carlo simulations. These improvements are attributed to the elimination of non-convex optimization challenges and the adoption of efficient algebraic computations instead of iterative optimization procedures.

4.2. Swarm Cooperative Localization Simulation Experiment

To verify the effectiveness of the new architecture, we conducted simulation experiments based on the AirSim platform. Since the AirSim platform does not have UWB data simulation, we added Gaussian white noise with a mean of 0.3 m based on the true distance to simulate and generate UWB data. The parameter settings of the simulation experiment are shown in

Table 2. Sensors carried by the UAV platform in simulation experiments.

Sensors	Data Frequency	Parameters
Camera	10 Hz	Resolution: 752 × 480 pixels
		View angle: 90°
IMU	100 Hz	In-run bias stability of Gyroscope: ${10}^{\circ }/\mathrm{h}$
		Noise density of Gyroscope: $1^{\circ }/\mathrm{s}/\sqrt{\mathrm{Hz}}$
		In-run bias stability of Accelerometer: $2000\text{µ}\mathrm{g}$
		Noise density of Accelerometer: $200\text{µ}\mathrm{g}/\sqrt{\mathrm{Hz}}$
UWB	10 Hz	Range accuracy: 0.3 m
		Maximum range: 500 m
Magnetometer	50 Hz	Resolution: 0.1°

, and the simulation scenarios and ground truth trajectories are shown in Figure 3.

In the simulation experiment, “VINS-Mono” and “OpenVINS” represent the two SOTA VIO methods in [37,40], “SuperVINS” represents the SOTA VIO method based deep learning proposed in [41], “COO_VIR” represents the collaborative localization method proposed in [12], “COVINS-G” represents the collaborative localization method proposed in [11], and “COO_OUR” represents the method proposed in this paper. The statistics of drone positioning root-mean-square error (RMSE) results are shown in

Table 3. RMSE of the position in extended simulation experiments with 6 UAVs, in meters.

Method	UAV1	UAV2	UAV3	UAV4	UAV5	UAV6	Avg.
VINS-Mono	6.12	11.45	9.21	8.76	10.34	9.87	9.30
OpenVINS	5.87	12.01	8.67	9.12	11.05	10.43	9.53
SuperVINS	5.21	9.78	7.98	8.23	9.12	8.65	8.16
COVINS-G	5.43	9.23	7.56	7.89	8.92	8.15	7.86
COO_VIR	4.89	8.67	7.12	7.45	8.21	7.73	7.35
COO_OUR	3.78	6.23	5.89	6.12	6.45	5.97	5.74

. Since this paper is aimed at drones operating in a large-scale outdoor environment and requires real-time positioning information output, when comparing it with OpenVINS and VINS-Mono algorithms, loop back repositioning is not considered.

As shown in Table 3, in the simulation experiment with 6 UAVs, our method outperforms all other methods. This indicates that our method not only achieves excellent positioning accuracy for each individual UAV but also maintains stable performance in the multi-UAV system, verifying its robustness and effectiveness in extended simulation scenarios.

4.3. Vision Loss Experiment

To verify the efficacy of the proposed algorithm in scenarios with visual loss, we conducted experiments by introducing partial visual loss into the simulation framework established in the previous section, as illustrated in Figure 4, the curves with different colors in the figure represent the trajectories of six different drones respectively, which is consistent with Figure 3. The bold part of the curve indicates visual disconnection, and the duration of visual disconnection is 10 s. Unless otherwise specified, the experimental settings, including but not limited to parameters, configurations, and environmental conditions, were consistent with those outlined in the preceding simulation experiment.

In the simulated vision loss experiment, the UAV’s positioning results are shown in

Table 4. Positioning RMSE (m) in the vision loss simulation experiment.

Method	UAV1	UAV2	UAV3	UAV4	UAV5	UAV6	Avg.
COVINS	Failed	Failed	Failed	Failed	Failed	Failed	Failed
COO_VIR	12.78	22.98	18.76	19.32	22.54	20.43	19.47
COO_OUR	5.72	7.55	6.81	7.34	7.59	6.44	6.91

. It can be seen that COVINS completely failed during the vision loss period as it relies entirely on continuous visual input. The COO_VIR method showed significant performance degradation with an average RMSE of 19.47 m, which is more than triple the error compared to our method. This degradation occurs because these methods heavily depend on the continuous operation of the VIO system, and when vision is lost, the VIO system fails to function properly, causing significant deviation in the overall system’s positioning results. In contrast, our proposed method can effectively handle vision deprivation, maintaining an average accuracy of 6.91 m, which is comparable to the results achieved in normal conditions as shown in Table 3. This demonstrates that our system maintains robust performance even under adverse visual conditions by leveraging the coupled fusion of UWB ranging and IMU data.

4.4. UWB Abnormal Experiment

To further evaluate the robustness of the proposed method under abnormal UWB conditions, we conducted additional simulation experiments that emulate two typical UWB failures in GNSS-denied outdoor environments: (1) UWB signal loss (no ranging output) and (2) UWB non-line-of-sight (NLOS) ranging bias. Unless otherwise specified, the simulation settings (sensor rates, trajectories, and evaluation protocol) are identical to those in the swarm cooperative localization simulation experiment (Table 2).

(1)

UWB signal loss

In this experiment, the UWB ranging packets are assumed to be completely lost during several time intervals, i.e., no distance measurements are available for the EKF update. Practically, this is implemented by dropping the UWB measurements so that the filter performs propagation and updates only with the remaining sensors (IMU, vision, and magnetometer). The UWB loss intervals are highlighted by the bold red segments in Figure 5.

(2)

UWB NLOS with positive bias (3–5 m)

Relevant studies have demonstrated that in NLOS scenarios, UWB ranging measurements may generate values larger than the true ones owing to the multipath effect. To model NLOS-induced ranging errors, we add a positive bias to the true inter-UAV distance during NLOS intervals. Specifically, for each inter-UAV link i–j, the corrupted ranging measurement is generated as

$${\tilde{d}}_{ij}=d_{ij}^{true}+b_{ij}+n_{ij},b_{ij}\sim \mathcal{U}(3,5)\mathrm{m},n_{ij}\sim \mathcal{N}(0,{\sigma }_d^2)$$

(52)

where $d_{ij}^{true}$ is the ground-truth distance, $b_{ij}$ is a random positive bias uniformly sampled from 3 to 5 m, and $n_{ij}$ is zero-mean Gaussian noise with ${\sigma }_d=0.3\mathrm{m}$, consistent with the nominal UWB setting. The NLOS intervals are highlighted by the bold black segments in Figure 5.

In the simulated UWB abnormality experiment, the UAV’s positioning results are shown in

Table 5. Positioning RMSE (m) in the UWB abnormality simulation experiment.

Method	UAV1	UAV2	UAV3	UAV4	UAV5	UAV6	Avg.
COO_VIR	8.23	11.12	10.89	12.01	12.34	13.76	11.39
COO_OUR	4.12	6.56	6.34	7.11	7.67	7.03	6.47
COO_OUR (without_UWB_AA)	5.89	8.43	8.12	9.26	9.87	8.75	8.39

. It can be seen that COO_VIR achieved an average RMSE of 11.39 m. This degradation occurs because these methods heavily depend on UWB ranging data for collaborative positioning, and when UWB data is lost or contains large errors, the system’s positioning accuracy is significantly affected.

Our proposed method with UWB adaptive adjustment (UWB_AA) strategy can effectively handle UWB abnormalities, maintaining an average accuracy of 6.47 m. To demonstrate the effectiveness of our chi-squared detection based UWB_AA strategy, we conducted an ablation experiment by removing this strategy, resulting in COO_OUR (without_UWB_AA) with an average RMSE of 8.39 m. This shows that the chi-squared detection based UWB_AA strategy contributes to a significant accuracy improvement of 1.92 m in UWB abnormality scenarios.

The superior performance is attributed to two key factors: (1) our system adopts a multi-modal fusion approach that integrates visual, inertial, and magnetic sensor data, which can compensate for the loss or errors of UWB data; (2) our chi-squared detection method effectively filters out UWB outliers, further improving the system’s robustness to UWB abnormalities.

To further demonstrate the effectiveness of our chi-squared detection method in identifying abnormal UWB measurements, we conducted a statistical analysis of the UWB data processing results. The abnormality criterion is defined as follows: if the error between the UWB measurement and the ground truth is greater than 1 m, the measurement is considered abnormal. The statistical results for the simulation experiment are shown in

Table 6. Statistical analysis of UWB anomaly detection performance in simulation experiment.

Statistic Index	Simulation Experiment
Anomaly ratio in raw UWB data	20.0%
Correctly identified anomaly ratio	18.6%
Anomaly detection rate	93%

.

Table 6 shows that in the simulation experiment, our chi-squared detection method achieves a high anomaly detection rate of 93%. Specifically, 20.0% of the raw UWB data are abnormal, and our method correctly identifies 18.6% of them. Since the same proportion of UWB measurement anomalies was set for each UAV in the simulation experiment, a single set of statistics is sufficient to represent the overall performance. This result clearly demonstrates the effectiveness of our chi-squared detection method in filtering out abnormal UWB measurements, which is crucial for improving the robustness and accuracy of the cooperative localization system in simulation scenarios.

4.5. Physical System Construction

In order to verify the practical application capabilities of the algorithm, this research builds a distributed multi-drone collaborative localization hardware platform. The collaborative localization platform integrates multi-modal sensors and carries out clock synchronization, and has a built-in embedded computing module that can process sensor data in real time. The platform can be mounted on a small quad-rotor drone, and realizes status information sharing and UWB ranging data interaction through a wireless communication module.

The hardware architecture is shown in Figure 6, which mainly includes the following modules: (1) Vision module: Equipped with FLIR Blackfly S USB3 global shutter camera, microsecond time synchronization between IMU and camera is achieved through hardware trigger signals (FLIR Systems, Inc., Wilsonville, OR, USA). (2) Inertial module: MEMS-IMU is used to output 200 Hz raw data of gyroscope, accelerometer, and magnetometer (Ruiyan Xinchuang Technology Co., Ltd., Chengdu, Sichuan, China). (3) UWB module: The ranging frequency is 10 Hz, and the accuracy of 0.3 m in dynamic environments is achieved through TDoA technology (Air Recycling Technology Co., Ltd., Shenzhen, Guangdong, China). (4) RTK positioning module: Integrated RTK-GNSS module (positioning accuracy of 2 cm) as a truth reference and is only enabled during the algorithm evaluation stage (Ruiyan Xinchuang Technology Co., Ltd., Chengdu, Sichuan, China). (5) Main processor: Select the RK3588 (Rockchip Electronics Co., Ltd., Fuzhou, Fujian, China) embedded computing module to meet the real-time computing requirements of the sliding window EKF framework. The parameters of each module are shown in

Table 7. Sensors carried by the UAV platform in real-world experiments.

Sensors	Data Frequency	Parameters
Camera	10 Hz	Resolution: 1280 × 1024 pixels
IMU	100 Hz	In-run bias stability of Gyroscope: ${10}^{\circ }/\mathrm{h}$
		Noise density of Gyroscope: $1^{\circ }/\mathrm{s}/\sqrt{\mathrm{Hz}}$
		In-run bias stability of Accelerometer: $2000\text{µ}\mathrm{g}$
		Noise density of Accelerometer: $200\text{µ}\mathrm{g}\sqrt{\mathrm{Hz}}$
UWB	10 Hz	Range accuracy: 0.3 m
		Maximum range: 500 m
Magnetometer	50 Hz	Resolution: 0.1°
Barometer	20 Hz	Accuracy: 10 cm

. The UAVs are each equipped with a compact communication station, which provides a communication bandwidth of no less than 50 Mbps and a communication range exceeding 10 km. Since the proposed cooperative method only requires the sharing of positional data and ranging information, without the need for transmitting image data, this communication setup adequately fulfills the operational requirements.

4.6. Real-World Experiment

To prove the practicality of the system proposed in this paper in real-world scenarios, we conducted a real-world collaborative localization experiment. A total of three drones were flown during the experiment. The flight trajectory of the drones is shown in Figure 7b. The sensor parameters carried by the drone are shown in Table 7. The experimental environment is an outdoor scene, as shown in Figure 7a. The drones are equipped with high-precision RTK positioning equipment as the ground truth reference. In the real-world experiment, the images captured by the drone camera are shown in Figure 8. It can be seen that compared with indoor UAVs, outdoor UAVs operate at higher altitudes, where only downward-facing cameras can capture continuous scenes and feature points are typically tens of meters away. This renders the accuracy of VIO significantly susceptible to environmental factors and reduced, thereby necessitating multi-UAV collaboration to improve accuracy.

In order to verify several innovative points of the method proposed in this paper, especially the novel processing methods for both UWB and visual measurements, ablation experiments with different settings were also carried out.

Table 8. RMSE of the position in real-world experiments, in meters.

Method	UAV1	UAV2	UAV3	Avg.
VINS-Mono	4.01	4.29	4.03	4.11
OpenVINS	3.97	4.06	3.41	3.81
SuperVINS	3.85	3.87	3.57	3.76
COO_VIR	3.04	3.49	2.57	3.03
COVINS-G	3.22	3.57	2.64	3.14
COO (ours_without_PO)	2.06	2.44	2.23	2.24
COO (ours_without_UWB)	2.43	2.95	2.38	2.59
COO (ours_without_MAG)	1.72	2.33	2.26	2.10
COO (ours_without_UWB_AA)	1.98	2.67	2.54	2.40
COO (ours)	1.53	2.08	1.97	1.86

shows a detailed evaluation of positioning errors for real-world experiments, where “COO (ours)” represents the algorithm proposed in this paper, “COO (ours_without_PO)” represents that the algorithm proposed in this paper does not use PO constraints, “COO (ours_without_UWB)” represents that the algorithm proposed in this paper does not use UWB measurement, “COO (ours_without_MAG)” represents that the algorithm proposed in this paper without Magnetic Assisted, and “COO (ours_without_UWB_AA)” represents that the algorithm proposed in this paper does not use the chi-squared test based UWB adaptive adjustment (UWB_AA) strategy. Figure 9 shows a comparison of the running times of different algorithms to evaluate the operating efficiency. It can be seen that our algorithm can maintain high operating efficiency thanks to its lightweight architecture and filter-based advantages. Moreover, the 3588 board (16 GB RAM) on our device runs the algorithm with a memory usage of ≤10% and a CPU usage of ≤30% to meet real-time demands. The system is powered by an independent 12 V/6000 mAh lithium battery, supporting continuous operation for ≥1 h. To sum up, compared with the open source SOTA VIO positioning method and multi-UAV collaborative positioning method, the algorithm proposed in this paper shows excellent positioning performance and computational efficiency.

Furthermore, in real-world experiments, the test scenario for unmanned aerial vehicles (UAVs) is a relatively flat urban square. In contrast, the scenario in simulation experiments is a complex terrain environment that includes undulating buildings, grasslands, rivers and other elements, with a larger motion scale. Consequently, the accuracy of the results obtained from real-world experiments is higher than that of the results from simulation experiments.

Additionally, we conducted a statistical analysis of the UWB data processing results in the real-world experiment. Using the same abnormality criterion (error > 1 m), the statistical results are shown in

Table 9. Statistical analysis of UWB anomaly detection performance in real-world experiment.

Statistic Index	UAV1	UAV2	UAV3	Average
Anomaly ratio in raw UWB data	12.2%	8.5%	6.8%	9.2%
Correctly identified anomaly ratio	11.0%	7.5%	6.2%	8.2%
Anomaly detection rate	90.2%	88.2%	91.2%	89.9%

.

Table 9 shows the UWB anomaly detection performance for each UAV and the average performance in the real-world experiment. On average, our chi-squared detection method achieves a high anomaly detection rate of 89.9% across all UAVs. This result further confirms the effectiveness of our chi-squared detection method in real-world scenarios, demonstrating its robustness and practical applicability even when different UAVs experience varying UWB measurement conditions.

5. Conclusions

This paper addresses the challenge of degraded navigation and positioning accuracy in GNSS-challenged environments for unmanned platforms, where visual environmental features are sparse and GNSS communication is constrained. We propose a distributed anchor-free visual-inertial-UWB-Magnetic multi-UAV cooperative localization system. In this system, UWB measurements between UAVs are utilized to estimate inter-drone distances without relying on pre-deployed UWB anchors. A sliding window EKF framework is adopted to fuse multi-modal observations, enhancing the algorithm’s lightweight design and real-time performance. To further improve accuracy, a PO visual observation model is introduced, which strengthens the system’s adaptability to challenging visual environments with limited features. Additionally, a novel MDS (Multidimensional Scaling)-MAP initialization method fuses ranging, IMU, and geomagnetic data to solve the non-convex optimization problem in ranging-aided SLAM, ensuring fast and accurate swarm absolute pose initialization. The improvement mechanism of absolute positioning accuracy in cooperative positioning without anchor points is analyzed, multiple keyframes are integrated into the sliding window, ensuring robustness and precision. Different from the current Vision-Inertial-UWB Cooperative Localization System based on the VIO system, we adopt a tightly-coupled Cooperative Localization System based on IMU, which can effectively deal with vision loss.

Extensive simulation and real-world experiments demonstrate that the proposed method significantly outperforms state-of-the-art localization approaches in GNSS-denied scenarios. In particular, our system achieved an average positioning accuracy of 5.74 m in an outdoor 50-m flight simulation experiment and 1.86 m in the actual experiment, which is a significant improvement over existing methods. Importantly, our method can effectively deal with situations of vision loss, maintaining an accuracy of 6.91 m even when other approaches fail completely. Notably, our chi-squared test-based UWB adaptive adjustment (UWB_AA) strategy effectively filters abnormal UWB measurements, achieving over 88% anomaly detection rate in both simulation and real-world experiments. Ablation experiments confirm it improves accuracy in UWB abnormality scenarios, enhancing the system’s robustness and accuracy.

The successful deployment of our system on small-scale UAV platforms validates its practicality and applicability in real-world lightweight drone systems.

In future research work, we will further explore the application of our system in challenging environments such as large-scale deployments and day-night transitions to obtain a more comprehensive and powerful drone cluster positioning system.