Performance Analysis of Visual–Inertial–Range Cooperative Localization for Unmanned Autonomous Vehicle Swarm

Abstract

The swarm of small UAVs is an emerging technology that will enable abundant cooperative tasks. To tackle the positioning problem for the UAV swarm, cooperative localization (CL) has been intensively studied since it uses relative measurement to improve the positioning availability and accuracy for the swarm in GPS-denied environments. Besides relying on inter-UAV range measurement, traditional CL algorithms need to place anchors as location references, which limits their applicability. To implement an infrastructure-less swarm navigation system, a consumer-grade camera together with an inertial device can provide rich environment information, which can be recognized as a kind of local location reference. This paper aims to analyze the fundamental performance of visual–inertial–range CL, which is also a popular metric for UAV planning and sensing optimizing, especially for resource-limited environments. Specifically, a closed-form Fisher information matrix (FIM) of visual–inertial–range CL is constructed in ${\mathbb{R}}^n\times \mathrm{SO}\left(n\right)$ manifold. By introducing an equivalent FIM and utilizing of the sparsity of the FIM, the performance of pose estimation can be efficiently calculated. A series of numerical simulations validate its effectiveness for analyzing the CL performance.

1. Introduction

With advancements in onboard computing and sensing, the small unmanned autonomous vehicle (UAV), especially the quadrotor, can become smaller, cheaper, and more intelligent. In some applications, small autonomous UAVs are demonstrating capabilities that rival or even surpass those of humans [1,2]. To leverage the ability of small-sized UAVs, the swarm of UAVs combines multiple UAVs to cooperate through action coordination, making those more flexible and robust compared to a single large UAV due to its distributed nature. Compared to the single agent, the UAV swarm can perform much more complicated tasks with higher efficiency, such as coordinated surveillance [3,4,5,6], cooperative exploring, and mapping [7,8].

The swarm of small-sized UAVs is particularly well-suited for exploring unknown environments due to its high degree of flexibility. A straightforward question is how to locate the swarm with low delay and high accuracy to enable the UAV swarm to operate more automatically and intelligently. In complex environments, obtaining accurate position information of the UAVs becomes critical and challenging [9,10]. The traditional, cheap method is using a GPS to find the UAVs’ global positions. However, a GPS signal is vulnerable and will be strongly attenuated by obstacles in urban cities or indoor environments. Utilizing the inter-UAV measurement method, cooperative localization (CL) can improve positioning availability and accuracy for the UAV swarm. Through these years, the research community has published a significant number of works about CL, including performance limit analysis [11], ranging optimization [12], dynamic positioning algorithms [10,13], etc. Conventionally, the setup for the CL system includes anchors to provide localization references [14,15,16]. However, the deployment of anchors for localization can be burdensome, particularly in applications wherein operation is required in unfamiliar environments. Moreover, anchors are static, which implies that each anchor can only serve a limited area. Therefore, it is not practical to deploy anchors for localization when the UAVs operate in a large area, or some place where the signal of the anchor is greatly attenuated, such as inside buildings with multiple floors, or for the small UAV swarm flying above a large area. On the other side, some works focus on anchorless, i.e., infrastructure-less CL. They use inter-UAV measurement to compute the relative position among the UAVs [10,17,18]. While these works can be applied to the formation control in a large field, they cannot obtain the position referred to the environment, which is key information for cooperative rescue, path-planning, etc.

To avoid the problem of anchor deployment, one intuitive way is to utilize the environment feature to obtain the global location, shown as Figure 1. The environment visual feature is an attractive option. When the ambient light condition is suitable, even a cheap camera can offer rich visual features [19]. The visual–inertial–range sensor combination for CL offers a cost-effective solution, and it boasts significant advantages in terms of size, weight, and power (SWaP) compared to sensors like Lidar and Radar. A significant number of advanced yet practical works have been proposed to make use of consumer-grade cameras and IMU to implement localization and navigation, such as visual-based simultaneous localization and mapping (SLAM) [20] and visual inertial odometry (VIO) [21,22]. Therefore, CL can exploit environment information as position reference at a very low cost. Meanwhile, from the SLAM side view, the range information can compensate for the localization drift, and provide more efficient relative position information in the UAV swarm [15].

A most relevant topic for visual–inertial-aided CL in our paper is the cooperative SLAM (C-SLAM) in the robotics community. As stated in [23], C-SLAM enables collaborative behaviors by building a collective representation of the environment and a shared situational awareness. To localize the UAV itself, C-SLAM studies odometry, inter-UAV loop closure across different UAVs, etc. Given that UAVs use map features as a reference for self-localization (similar to our approach), our work can be viewed as a special case of C-SLAM. On the basis of the traditional C-SLAM, our uniqueness lies in the incorporation of relative range measurement, which yields additional localization information. Meanwhile, for a SLAM algorithm, it implicitly emphasizes building the map simultaneously for other missions. On the contrary, our work focuses on the state estimation in the current mission, which is more like VIO.

An important research field is to evaluate the performance limit of the localization system, which is the cornerstone for understanding and optimizing the localization accuracy. Evaluating the localization performance plays an essential role for optimizing automatic planning and control applications in challenging environments, which is called active localization or active SLAM. By actively selecting a sensing strategy to improve localization accuracy under a resource-limited scenario, the UAV swarm is able to make its planning and control more robust and agile, as well as estimate the surrounding map more accurately. Related works have been numerously studied in single UAVs for agile flying [24] and active SLAM [25,26,27] with position performance evaluation. In [24], Zhang et al. utilized the Fisher information matrix (FIM) to evaluate the performance of localization in a given map. By formulating the Fisher information and sensor visibility in the whole action space, the localization information can be summarized into a discrete grid, which helps to generate motion planning to maximize localization information. In [26], Khosoussi et al. considered the connection between the performance of a SLAM and its graph-based representation, and characterized the log-determinant of the FIM as having a strong relation with the tree-connectivity of the graph. In [25], Chen et al. analyzed the FIM for a pose-graph SLAM of which the parameter space is the product manifold ${\mathbb{R}}^n\times \mathrm{SO}\left(n\right)$ and stated that the structure of FIM has a strong connection with the corresponding weighted Laplacian matrix. Then, some approximate metrics were introduced to reduce the computation complexity. These works all prove that FIM is a powerful tool to optimize the UAV localization.

However, the existing performance analysis methods and metrics only consider the single-agent scenario, and the range and raw visual feature measurements are not considered. Although some studies in the literature address the performance of a multiple-UAVs system [28,29], they simply use the same metric for a single agent, which cannot directly merge the inter-UAV range information. Therefore, this paper focuses on computing the FIM and other derived performance metrics for CL in the UAV swarm with visual–inertial–range information. Our main contributions can be summarized as follows:

• We compute the closed-form FIM for visual–inertial–range CL in ${\mathbb{R}}^n\times \mathrm{SO}\left(n\right)$ manifold and validate its correctness and reachability.
• We reveal the sparsity structure of the FIM and its relations with the measurement connections.
• We introduce the equivalent Fisher information matrix (EFIM) to overcome the computational intractable when the number of visual features grows.

The numerical results validate the FIM calculation and prove that using environment visual features can provide both relative and absolute (relative to the environment) relevant localization information for CL.

2. System Model and Problem Formulation

Typically, each UAV in a swarm is equipped with IMU, a monocular camera, and UWB to obtain navigation information. We assume that sensors of different UAVs are homogeneous for simplicity.

Consider a UAV $m\in \mathcal{M}$, where $\mathcal{M}=\{1,2,\dots ,M\}$, and denote its position, altitude, and pose at time t as ${\mathit{x}}_t^m$, ${\mathit{R}}_t^m$, ${\mathit{p}}_t^m=\{{\mathit{x}}_t^m,{\mathit{R}}_t^m\}$, respectively. ${\mathit{R}}_t^m$ is a $2\times 2$ or $3\times 3$ matrix, and it is an element of special orthogonal group SO(2) or SO(3). In a short-term estimation process, the visual odometry method leverages IMU measurements for both rotation and motion scale, allowing us to model it as a pose-graph measurement, called intra-UAV state transition measurement. Therefore, the state transition measurement can be modeled as

$$\begin{array}{cc}\hfill {\widehat{\mathit{p}}}_t& ={\mathit{R}}_{t-1}^{\top }({\mathit{x}}_t-{\mathit{x}}_{t-1})+{\mathit{y}}_t\hfill \\ \hfill {\widehat{\mathit{H}}}_t& ={\mathit{Z}}_t{\mathit{R}}_t{\mathit{R}}_{t-1}^{\top }\hfill \end{array}$$

(1)

where ${\mathit{y}}_t$ is a zero-mean Gaussian noise, and ${\mathit{Z}}_t$ is a random rotation that follows an isotropic Langevin distribution with mean of I [30,31]. It is worth noting that our short-term visual odometry only considers camera measurements within a short odometry scope, excluding visual feature tracking and descriptor matching, differing from the following camera measurement. It is valid that some popular visual–inertial odometry algorithms use different camera information for short-term and long-term pose estimation. In the short-term ego-motion tracking, the direct methods use pixel regions instead of feature points to improve efficiency and robustness [32,33,34]. Meanwhile, for long-term estimation, the feature-based camera measurements are used for loop detection and inter-UAV information exchange [22,34]. Furthermore, when the visual information is too poor to support high-accuracy short-term ego-motion tracking, we can set the magnitudes of noise ${\mathit{y}}_t$ and ${\mathit{Z}}_t$ to be larger or be time-related to simulate the odometry noise.

Instead of the traditional pinhole camera model, we model the camera feature-based measurement as a bearing vector. This model provides a simpler expression and allows us to analyze the performance for the arbitrary-FOV camera [24]. Denote the world coordinate of the feature as ${\mathit{l}}_w$; then, the bearing measurement model can be described as

$${\mathit{l}}_{uv}=f_c{\mathit{R}}^{\top }\frac{{\mathit{l}}_w-\mathit{x}}{\Vert {\mathit{l}}_w-\mathit{x}\Vert }.$$

(2)

where $f_c$ is a scale that transforms the angle of direction to the image coordination, the units of which are pixels. We can see that the visual measurement is a function of the direction from the UAV to the visual feature. Therefore, it can be seen as a kind of AOA measurement. The measurement lies on the sphere with the radius of $f_c$. This bearing measurement is assumed to be contaminated by zero-mean Gaussian noise:

$${\widehat{\mathit{l}}}_{uv}={\mathit{l}}_{uv}+{\mathit{u}}_{uv}$$

(3)

Here, we assume that all the visual features in different time epochs can be matched correctly. It makes sense because we are deriving the performance limit for the visual–inertial–range CL, while the feature mismatching only makes the estimation worse.

The range measurement between the UAV m and n at time t is denoted as

$${\widehat{z}}_t^{mn}=\Vert {\mathit{x}}_t^m-{\mathit{x}}_t^n\Vert +u_r$$

(4)

where $u_r$ is a zero-mean Gaussian noise.

From a Bayesian perspective, the probability density function (pdf) of the above measurements can be expressed as

$$p\left(\mathit{y}|\mathit{\theta }\right)=p\left({\widehat{\mathit{l}}}_{uv}|\mathit{x},\mathit{R},{\mathit{l}}_w\right)p\left(\widehat{\mathit{p}},\widehat{\mathit{H}}|\mathit{x},\mathit{R}\right)p\left(\mathit{z}|\mathit{x}\right)$$

(5)

where $\mathit{y}\triangleq \left\{{\widehat{\mathit{l}}}_{uv},\widehat{\mathit{p}},\widehat{\mathit{H}},\widehat{\mathit{z}}\right\}$ is the measurement set, and $\mathit{\theta }\triangleq \left\{\mathit{x},\mathit{R},\mathit{l}\right\}$ represent the parameter set. Furthermore, with the assumption that all the measurement noise is mutually independent throughout the time sequence and UAVs, the pdf can be reformulated to

$$\begin{array}{cc}\hfill p\left(\mathit{y}|\mathit{\theta }\right)& =\prod _{t\in \mathcal{T}}\prod _{n\in \mathcal{N}}\prod _{l\in {\mathcal{L}}_t^n}p\left({\widehat{\mathit{l}}}_{uv}^{l,n,t}|{\mathit{x}}_t^n,{\mathit{R}}_t^n,{\mathit{l}}_w^l\right)\hfill \\ \hfill & \times \prod _{t\in \mathcal{T}}\prod _{n\in \mathcal{N}}p\left({\widehat{\mathit{p}}}_t^n|{\mathit{x}}_t^n,{\mathit{x}}_{t-1}^n,{\mathit{R}}_{t-1}^n\right)p\left({\widehat{\mathit{H}}}_t^n|{\mathit{R}}_{t-1}^n\right)\hfill \\ \hfill & \times \prod _{(m,n,t)\in {\mathcal{E}}_r}p\left({\widehat{z}}_t^{mn}|{\mathit{x}}_t^m,{\mathit{x}}_t^n\right)\hfill \end{array}$$

(6)

where $p\left({\widehat{\mathit{l}}}_{uv}^{l,n,t}|{\mathit{x}}_t^n,{\mathit{R}}_t^n,{\mathit{l}}_w^l\right)$ is the visual feature measurement, which is

$$\begin{array}{c}p({\widehat{\mathit{l}}}_{uv}^{l,m,t}|{\mathit{x}}_t^m,{\mathit{R}}_t^m,{\mathit{l}}_w)\hfill \\ \propto exp\left(-\frac{1}{2}\left({\widehat{\mathit{l}}}_{uv}^{l,m,t}-{\mathit{l}}_{uv}^{l,m,t}\right){\mathbf{\Sigma }}_{\mathit{\mu }}^{-1}{\left({\widehat{\mathit{l}}}_{uv}^{l,m,t}-{\mathit{l}}_{uv}^{\mathit{l},m,t}\right)}^{\top }\right).\hfill \end{array}$$

(7)

$p\left({\widehat{\mathit{p}}}_t^n|{\mathit{x}}_t^n,{\mathit{x}}_{t-1}^n,{\mathit{R}}_{t-1}^n\right)$ and $p\left({\widehat{\mathit{H}}}_t^n|{\mathit{R}}_{t-1}^n\right)$ stand for the state transition measurement PDF:

$$\begin{array}{c}p\left({\widehat{\mathit{p}}}_t^n|{\mathit{x}}_t^n,{\mathit{x}}_{t-1}^n,{\mathit{R}}_{t-1}^n\right)\hfill \\ \propto exp(-\frac{1}{2}\left({\mathit{R}}_{t-1}^{\top }({\mathit{x}}_t-{\mathit{x}}_{t-1})-{\widehat{\mathit{p}}}_t\right)\hfill \\ \times {\mathbf{\Sigma }}_t^{-1}{\left({\mathit{R}}_{t-1}^{\top }({\mathit{x}}_t-{\mathit{x}}_{t-1})-{\widehat{\mathit{p}}}_t\right)}^{\top }).\hfill \end{array}$$

(8)

$p\left({\widehat{z}}_{mn}^t|{\mathit{x}}_t^m,{\mathit{x}}_t^n\right)$ represents the ranging measurement PDF:

$$\begin{array}{c}\hfill p\left({\widehat{z}}_t^{mn}|{\mathit{x}}_t^m,{\mathit{x}}_t^n\right)\propto exp\left(-\frac{1}{2{\sigma }_z^2}{\left({\widehat{z}}_{mn}-z_{mn}\right)}^2\right).\end{array}$$

(9)

Moreover, we can represent the measurement process with a graph model, shown in Figure 2. Each node represents a UAV state and each edge is a certain measurement.

3. Calculation of Fisher Information Matrix

FIM is a metric to evaluate the quality of the information obtained by a UAV from specific measurements attenuated by noise. With the log-likelihood function $L(\mathit{y};\mathit{\theta })\triangleq logp\left(\mathit{y}\right|\mathit{\theta })$ and the orthonormal basis system, the $(i,j)$-th element of FIM is given by

$${\mathit{F}}_{i,j}={\mathbb{E}}_{\mathit{y}|\mathit{\theta }}\left[〈gradL(\mathit{y};\mathit{\theta }),{\mathbf{e}}_i〉\times 〈gradL(\mathit{y};\mathit{\theta }),{\mathbf{e}}_j〉\right]$$

(10)

where ${\mathbf{e}}_i$ is the i-th basis of the tangent space of the parameters, and “grad” is the gradient operator. Since the FIM is the expectation over the measurement space y, it is a function of parameter $\mathit{\theta }$.

3.1. The FIM Expression for 2-D Case

The structure of FIM for the 2-D case is a sum of three parts:

$${\mathcal{I}}_{2\mathrm{D}}=\left[\begin{array}{cc}{\mathcal{I}}_{{\mathit{l}}_w}& {\mathcal{I}}_{{\mathit{l}}_w\mathit{\vartheta }}\\ {\mathcal{I}}_{{\mathit{l}}_w\mathit{\vartheta }}^{\top }& {\mathcal{I}}_{\mathit{\vartheta }}\end{array}\right]={\mathcal{I}}_{\mathrm{IN}}+{\mathcal{I}}_{\mathrm{VF}}+{\mathcal{I}}_{\mathrm{RA}}$$

(11)

where

$$\begin{array}{cc}\hfill {\mathit{l}}_w& =\{{\mathit{l}}_w^1,{\mathit{l}}_w^2,\dots ,{\mathit{l}}_w^L\}\hfill \\ \hfill \mathit{\vartheta }& =\{{\mathit{p}}_1,{\mathit{p}}_2,\dots ,{\mathit{p}}_T\}\hfill \\ \hfill {\mathit{p}}_t& =\{{\mathit{x}}_t^1,{\mathit{x}}_t^2,\dots ,{\mathit{x}}_t^M,{\mathit{R}}_t^1,{\mathit{R}}_t^2,\dots ,{\mathit{R}}_t^M\}\hfill \end{array}$$

(12)

and ${\mathcal{I}}_{\mathrm{IN}}$, ${\mathcal{I}}_{\mathrm{VF}}$, ${\mathcal{I}}_{\mathrm{RA}}$ represent for the partial FIM from inter-UAV state transition, visual feature, and ranging measurement, respectively.

3.1.1. FIM for Intra-UAV State Transition Measurement

The intra-UAV state transition measurement of one UAV is equivalent to a pose graph with only links between consecutive states.

To make the expression of ${\mathcal{I}}_{\mathrm{IN}}$ more clear, we reorder the states to

$$\begin{array}{cc}\hfill \mathit{\vartheta }& =\{{\mathit{p}}^1,{\mathit{p}}^2,\dots ,{\mathit{p}}^M\}\hfill \\ \hfill {\mathit{p}}^m& =\{{\mathit{x}}_1^m,{\mathit{x}}_2^m,\dots ,{\mathit{x}}_T^m,{\mathit{R}}_1^m,{\mathit{R}}_2^m,\dots ,{\mathit{R}}_T^m\}\hfill \end{array}$$

(13)

Then, the new FIM ${\mathcal{I}}_{\mathrm{INO}}$ can be given by [25]

$$\begin{array}{cc}\hfill {\mathcal{I}}_{\mathrm{INO}}& =\mathrm{diag}(\mathbf{0},{\mathcal{I}}_{\mathit{\vartheta }}^{\mathrm{INO}})\hfill \\ \hfill {\mathcal{I}}_{\mathit{\vartheta }}^{\mathrm{INO}}& =\mathrm{diag}\left({\mathcal{I}}_{{\mathit{p}}^m}^{\mathrm{INO}}\right)\hfill \\ \hfill {\mathcal{I}}_{{\mathit{p}}^m}^{\mathrm{INO}}& =\left[\begin{array}{cc}{\mathit{L}}_w^{{\mathbb{R}}^2}\left(m\right)& {\Delta }_w\left(m\right)\\ {\Delta }_w^{\top }\left(m\right)& {\mathit{L}}_w^{\mathrm{SO}\left(2\right)}\left(m\right)+\mathrm{diag}\left({\psi }_m\right)\end{array}\right]\hfill \end{array}$$

(14)

where ${\mathit{L}}_w^{{\mathbb{R}}^2}\left(m\right)={\mathit{L}}_{w_{\mathbb{R}}}\otimes \mathbf{I}$ corresponding to the position state of UAV m. ${\mathit{L}}_{w_{\mathbb{R}}}$ is a weighted Laplacian matrix, of which the weight of the pose graph edge $(i,i+1),i=1,2,\dots ,T-1$ is ${\sigma }_{i,i+1}^{-2}$. ${\Delta }_w\left(m\right)$ is also a Laplacian-like matrix of which the $(i_1,i_2)$th $2\times 1$ block matrix is given by

$${\left({\Delta }_w\left(m\right)\right)}_{i_1,i_2}=\left\{\begin{array}{cc}\sum _{j\in V_{i_1}^{+}}{\sigma }_{i_{1}j}^{-2}{\mathbf{E}}^{\top }({\mathit{x}}_{i_1}^m-{\mathit{x}}_j^m)\hfill & i_1=i_2\hfill \\ {\sigma }_{i_2{i}_1}^{-2}{\mathbf{E}}^{\top }({\mathit{x}}_{i_1}^m-{\mathit{x}}_{i_2}^m)\hfill & (i_2,i_1)\in \mathcal{E}\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right.$$

(15)

$L_w^{\mathrm{SO}\left(2\right)}\left(m\right)$ is a weighted Laplacian matrix with the weight for $(i,j)$th entry as $2{\kappa }_{i,j}^m\frac{I_1\left(2{\kappa }_{i,j}^m\right)}{I_0\left(2{\kappa }_{i,j}^m\right)}$. The $i_1$th entry for $\psi \left(m\right)$ is ${\left(\psi \left(m\right)\right)}_{i_1}={\sum }_{k\in V_{i_1}^{+}}{\sigma }_{i_{1}k}^{-2}\Vert {{\mathit{x}}_k^m-{\mathit{x}}_{i_1}^m}^2\Vert$. E is the basis of the tangent space of the manifold $\mathrm{SO}\left(2\right)$ at its origin, which is defined as

$$\mathbf{E}=\frac{1}{\sqrt{2}}\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$$

(16)

${\mathcal{I}}_{\mathrm{INO}}$ is well-defined according to the above equations, and we can obtain the final FIM ${\mathcal{I}}_{\mathrm{IN}}$ by column and row transformation to order the parameters back.

3.1.2. FIM for Visual Measurement

Only ${\mathcal{I}}_{\mathrm{VF}}$ has a none-zero partial block matrix ${\mathcal{I}}_{{\mathit{l}}_w}^{\mathrm{VF}}$ and ${\mathcal{I}}_{{\mathit{l}}_w\mathit{\vartheta }}^{\mathrm{VF}}$, where ${\mathcal{I}}_{{\mathit{l}}_w}^{\mathrm{VF}}$ is a block diagonal matrix:

$$\begin{array}{cc}\hfill {\mathcal{I}}_{{\mathit{l}}_w}^{\mathrm{VF}}& =\mathrm{diag}\left({\mathcal{I}}_{{\mathit{l}}_w}^{\mathrm{VF}}(i,i)\right)\hfill \\ \hfill {\mathcal{I}}_{{\mathit{l}}_w}^{\mathrm{VF}}(i,i)& =\sum _{l_i\in {\mathcal{L}}_t^m,(m,t)\in \mathcal{M}\times \mathcal{T}}\frac{{\zeta }_t^{m,i}}{{\Vert {\mathit{l}}_w^i-{\mathit{x}}_t^m\Vert }^2}{\mathbf{q}}_t^{m,i}{\left({\mathbf{q}}_t^{m,i}\right)}^{\top }\hfill \end{array}$$

(17)

and

$$\begin{array}{cc}\hfill {\mathcal{I}}_{{\mathit{l}}_w^i,{\mathit{x}}_t^m}^{\mathrm{VF}}& =-{\delta }_{l_i\in {\mathcal{L}}_t^m}\frac{{\zeta }_t^{m,i}}{{\Vert {\mathit{l}}_w^i-{\mathit{x}}_t^m\Vert }^2}{\mathbf{q}}_t^{m,i}{\left({\mathbf{q}}_t^{m,i}\right)}^{\top }\hfill \\ \hfill {\mathcal{I}}_{{\mathit{l}}_w^i,{\mathit{R}}_t^m}^{\mathrm{VF}}& ={\delta }_{l_i\in {\mathcal{L}}_t^m}\frac{{\zeta }_t^{m,i}}{\Vert {\mathit{l}}_w^i-{\mathit{x}}_t^m\Vert }{\mathbf{q}}_t^{m,i}.\hfill \end{array}$$

(18)

${\mathcal{I}}_{{\mathit{x}}_j^m,{\mathit{x}}_k^n}^{\mathrm{VF}}$, ${\mathcal{I}}_{{\mathit{x}}_k^n,{\mathit{R}}_j^m}^{\mathrm{VF}}$ and ${\mathcal{I}}_{{\mathit{R}}_k^n,{\mathit{R}}_j^m}^{\mathrm{VF}}$ are given by the following expressions:

$$\begin{array}{cc}\hfill {\mathcal{I}}_{{\mathit{x}}_j^m,{\mathit{x}}_k^n}^{\mathrm{VF}}& ={\delta }_{j=k}{\delta }_{m=n}\sum _{l_i\in {\mathcal{L}}_j^m}\frac{{\zeta }_j^{m,i}}{{\Vert {\mathit{l}}_w^i-{\mathit{x}}_j^m\Vert }^2}{\mathbf{q}}_j^{m,i}{\left({\mathbf{q}}_j^{m,i}\right)}^{\top }\hfill \\ \hfill {\mathcal{I}}_{{\mathit{x}}_j^n,{\mathit{R}}_k^m}^{\mathrm{VF}}& ={\delta }_{j=k}{\delta }_{m=n}\sum _{l_i\in {\mathcal{L}}_j^m}-\frac{{\zeta }_j^{m,i}}{\Vert {\mathit{l}}_w^i-{\mathit{x}}_j^m\Vert }{\mathbf{q}}_j^{m,i}\hfill \\ \hfill {\mathcal{I}}_{{\mathit{R}}_j^n,{\mathit{R}}_k^m}^{\mathrm{VF}}& ={\delta }_{j=k}{\delta }_{m=n}\sum _{l_i\in {\mathcal{L}}_j^m}\frac{1}{2}{\zeta }_j^{m,i}\hfill \end{array}$$

(19)

${\delta }_{[★]}=1$ if and only if the condition ★ is true; otherwise, ${\delta }_{[★]}=0$. ${\mathbf{q}}_t^{m,i}$ is a normalized vector perpendicular to the direction of ${\mathit{l}}_w^i-{\mathit{x}}_t^m$. ${\zeta }_t^{m,i}$ is a scaler that is defined as

$${\zeta }_t^{m,i}\triangleq \frac{f_c^2}{{\sigma }_{\mathit{u}}^2}$$

(20)

According to Equations (17)–(19), ${\mathcal{I}}_{\mathrm{VF}}$ is a sparse matrix: a visual feature and a pose state are coupled, i.e., the corresponding element is non-zero, if and only if they are linked together in the measurement graph.

3.1.3. FIM for Range Measurement

${\mathcal{I}}_{\mathrm{RA}}$ only has relations with the position states, so the partial block matrix ${\mathcal{I}}_{{\mathit{l}}_w}^{\mathrm{RA}}=\mathbf{0}$. Another partial block matrix ${\mathcal{I}}_{\mathit{\vartheta }}^{\mathrm{RA}}$ is a block diagonal matrix, which can be written as

$$\begin{array}{cc}\hfill {\mathcal{I}}_{\mathit{\vartheta }}^{\mathrm{RA}}& =\mathrm{diag}\left({\mathcal{I}}_{{\mathit{p}}_t}^{\mathrm{RA}}\right)\hfill \\ \hfill {\mathcal{I}}_{{\mathit{p}}_t}^{\mathrm{RA}}& =\mathrm{diag}({\mathcal{I}}_{{\mathit{x}}_t}^{\mathrm{RA}},\mathbf{0})\hfill \end{array}$$

(21)

where ${\mathcal{I}}_{{\mathit{x}}_t}^{\mathrm{RA}}$ is a Laplacian-like matrix of which the weight of $(m,n)$th element is a $2\times 2$ matrix

$${\mathit{w}}_{mn}^r=\frac{1}{{\sigma }_z^2}{\mathit{\varrho }}_t^{m,n}{\left({\mathit{\varrho }}_t^{m,n}\right)}^{\top }$$

(22)

where ${\mathit{\varrho }}_t^{m,n}$ is orthonormal vector $\frac{{\mathit{x}}_t^n-{\mathit{x}}_t^m}{\Vert {\mathit{x}}_t^n-{\mathit{x}}_t^m\Vert }$.

3.2. The FIM Expression for the 3-D Case

For the 3-D case, the FIM expression maintains a similar structure as in the 2-D case. The expressions of ${\mathcal{I}}_{\mathrm{VF}}$ and ${\mathcal{I}}_{\mathrm{RA}}$ stay unchanged; only the vector dimension should change correspondingly. The main difference between the 3-D and 2-D cases is the expression of ${\mathcal{I}}_{\mathrm{IN}}$, which is caused by the manifold structure of states and measurements. When the states are reordered as in Equation (13), ${\mathcal{I}}_{\mathrm{IN}}$ for the 3-D case can be given by [25]

$$\begin{array}{cc}\hfill {\mathcal{I}}_{\mathrm{INO}}& = \mathrm{diag}(\mathbf{0},{\mathcal{I}}_{\mathit{\vartheta }}^{\mathrm{INO}})\hfill \\ \hfill {\mathcal{I}}_{\mathit{\vartheta }}^{\mathrm{INO}}& = \mathrm{diag}\left({\mathcal{I}}_{{\mathit{p}}^m}^{\mathrm{INO}}\right)\hfill \\ \hfill {\mathcal{I}}_{{\mathit{p}}^m}^{\mathrm{INO}}& =\left[\begin{array}{cc}{\mathit{L}}_w^{{\mathbb{R}}^3}\left(m\right)& {\Delta }_w^{3\mathrm{D}}\left(m\right)\\ {\Delta }_w^{3\mathrm{D}\top }\left(m\right)& {\mathit{L}}_w^{\mathrm{SO}\left(3\right)}\left(m\right)+\mathrm{diag}\left({\mathbf{\Psi }}_m\right)\end{array}\right]\hfill \end{array}$$

(23)

${\mathit{L}}_w^{{\mathbb{R}}^3}\left(m\right)$ and ${\mathit{L}}_w^{\mathrm{SO}\left(3\right)}\left(m\right)$ have a same structure as ${\mathit{L}}_w^{{\mathbb{R}}^2}\left(m\right)$ and ${\mathit{L}}_w^{\mathrm{SO}\left(2\right)}\left(m\right)$, respectively. The off-diagonal partial matrix ${\Delta }_w^{3\mathrm{D}}\left(m\right)$ has the following structure:

$$\begin{array}{cc}\hfill {\left({▵}_w^{3\mathrm{D}}\left(m\right)\right)}_{i,i_1}& =\left\{\begin{array}{cc}\left[\sum _{j\in V_i^{+}}{\mathit{\varsigma }}_{ij}^1\left(m\right)\sum _{j\in V_i^{+}}{\mathit{\varsigma }}_{ij}^2\left(m\right)\sum _{j\in V_i^{+}}{\mathit{\varsigma }}_{ij}^3\left(m\right)\right]\hfill & i=i_1\hfill \\ \left[-{\mathit{\varsigma }}_{i{i}_1}^1\left(m\right)-{\mathit{\varsigma }}_{i{i}_1}^2\left(m\right)-{\mathit{\varsigma }}_{i{i}_1}^3\left(m\right)\right]\hfill & \left(i,i_1\right)\in \mathcal{E}\hfill \\ {\mathbf{0}}_{3\times 3}\hfill & \mathrm{otherwise}.\hfill \end{array}\right.\hfill \\ \hfill \end{array}$$

(24)

where ${\mathit{\varsigma }}_{ij}^k={\sigma }_{ij}^{-2}{\left({\mathit{x}}_i-{\mathit{x}}_j\right)}^{\top }{\mathit{R}}_i{\mathit{E}}_k{\mathit{R}}_i^{\top }$, $k=1,2,3$. ${\mathit{E}}_k$ is the basis vector of the tangent space of $\mathrm{SO}\left(3\right)$ at the origin point, i.e.,

$$\begin{array}{cc}\hfill & {\mathit{E}}_1=\frac{1}{\sqrt{2}}\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 1\\ 0& -1& 0\end{array}\right]\hfill \\ \hfill & {\mathit{E}}_2=\frac{1}{\sqrt{2}}\left[\begin{array}{ccc}0& 0& -1\\ 0& 0& 0\\ 1& 0& 0\end{array}\right]\hfill \\ \hfill & {\mathit{E}}_3=\frac{1}{\sqrt{2}}\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 0\end{array}\right]\hfill \end{array}$$

(25)

${\mathbf{\Psi }}_m$ in Equation (23) is given by

$$\begin{array}{cc}\hfill {\left({\mathbf{\Psi }}_m\right)}_i& =\left[\begin{array}{ccc}{\psi }_i^{11}\left(m\right)& {\psi }_i^{12}\left(m\right)& {\psi }_i^{13}\left(m\right)\\ {\psi }_i^{12}\left(m\right)& {\psi }_i^{22}\left(m\right)& {\psi }_i^{21}\left(m\right)\\ {\psi }_i^{13}\left(m\right)& {\psi }_i^{21}\left(m\right)& {\psi }_i^{33}\left(m\right)\end{array}\right]\hfill \\ \hfill {\psi }_i^{k\mathit{l}}\left(m\right)& =\sum _{j\in V_i^{+}}{\sigma }_{ij}^{-2}{\left({\mathit{x}}_i-{\mathit{x}}_j\right)}^{\top }{\mathit{R}}_i{\mathit{E}}_k{\mathit{E}}_{\mathit{l}}^{\top }{\mathit{R}}_i^{\top }\left({\mathit{x}}_i-{\mathit{x}}_j\right).\hfill \end{array}$$

(26)

4. Properties of the Fisher Information Matrix

4.1. The Geometric Property of the FIM

According to Equations (17)–(19) and (22), the information obtained from visual feature and range measurements is directional: the visual feature information combined with each information element collected from every linked UAV state. The visual feature information is perpendicular to the direction from the visual feature to the UAV, while the range measurement information is along the direction of two UAVs. Thus, a visual feature measurement can be regarded as an angle-of-arrival (AOA) measurement weakened by the square of the distance between the UAV and the visual feature. This is because we simplify the camera projection model into Equation (2). Nevertheless, the AOA assumption for the visual feature measurement is valuable, because we can regard it as a performance bound for a limited FOV scenario.

On the contrary, the information obtained by rotation, i.e., Equation (19), is invariant to the measurement distance, which means that visual feature measurement can provide valid information for rotation in long-distance scenarios.

4.2. The Structure of the FIM

The structure of the whole FIM is shown in Figure 3. We can see that the FIM is a sparse matrix. Each non-zero element means there is information obtained from certain measurements. Furthermore, some important properties of the FIM can be concluded as follows:

• Due to the independent nature of the visual feature, IMU, and ranging measurements, the whole FIM is the sum of the FIMs for these three parts, which has been identified by Equation (11).
• The rank of the FIM is less than its dimension, so it is rank-deficient, which means it is non-invertible because all mentioned measurements are unable to provide any global pose information for any UAV. Thus, we cannot calculate the corresponding Cramér–Rao lower bound (CRLB) directly, which is normally the inverse of the FIM. To avoid the rank-deficient problem, we can either obtain additional global information, or delete some rows and columns of the FIM, which is equivalent to assuming that the corresponding states are perfectly known.
• Range measurement only affects the correlation of the position states of corresponding UAVs at the ranging time epoch, so they cannot provide any information about rotation.
• Since the visual feature states are considered, the direct correlation of UAV states in FIM only exists between the consecutive states of the same UAV, which is caused by the IMU measurement, which forms the structure of C, as seen in Figure 3.
• The non-zero correlation element between visual feature and UAV state indicates the visual feature visibility for certain UAVs. In the following, we will see that, when the visual feature states are eliminated, the remaining part of the FIM ${\mathcal{I}}_{\mathrm{VF}}$ has a structure similar to the FIM of a pose graph, of which each link corresponds to the co-visibility of a same visual feature. This property enables us to efficiently calculate some metrics for measurement optimization.

Figure 3. The structure of the total FIM, depicted as the symmetric matrix (A). The upper-left block matrix of (A) represents the FIM of visual feature, which is block-diagonal. The lower-right block of (A) is a strip-like matrix, of which the diagonal part has the structure as (B), and the off-diagonal part has the structure as (C), which is block-diagonal. The non-zero elements of the upper-right block matrix of (A) indicate the visibility of the corresponding visual feature and UAV.

Figure 3. The structure of the total FIM, depicted as the symmetric matrix (A). The upper-left block matrix of (A) represents the FIM of visual feature, which is block-diagonal. The lower-right block of (A) is a strip-like matrix, of which the diagonal part has the structure as (B), and the off-diagonal part has the structure as (C), which is block-diagonal. The non-zero elements of the upper-right block matrix of (A) indicate the visibility of the corresponding visual feature and UAV.

From the above analysis, we can see that the structure of the FIM has some common properties with a pose graph and that the non-zero element reflects a valid measurement. For the partial FIM ${\mathcal{I}}_{\mathrm{IN}}$, it is equivalent to the one for a pose graph in which only the consecutive frames of the same UAV are connected. Nevertheless, compared to the FIM for pose-graph SLAM, which only contains UAV states, our form can reflect the uncertainty of visual features.

The number of visual features grows fast over time; thus, analyzing such a big matrix becomes intractable. A practical solution is to split the states of visual features out of the total estimation states. The equivalent Fisher information matrix (EFIM) is a useful tool to evaluate the estimation performance for a subset of states by using the Schur complement trick [11]. By definition, the EFIM for UAV states $\mathit{\vartheta }$ is given by

$${\mathit{J}}_e\left(\mathit{\vartheta }\right)={\mathcal{I}}_{\mathit{\vartheta }}-{\mathcal{I}}_{{\mathit{l}}_w\mathit{\vartheta }}^{\top }{\mathcal{I}}_{{\mathit{l}}_w}^{-1}{\mathcal{I}}_{{\mathit{l}}_w\mathit{\vartheta }}$$

(27)

The dimension of ${\mathit{J}}_e\left(\mathit{\vartheta }\right)$ is $3MT\times 3MT$ in the 2-D case and is invariant to the visual feature number. Like the FIM structure, we can also partition the total EFIM into three parts:

$${\mathit{J}}_e\left(\mathit{\vartheta }\right)={\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{\vartheta }\right)+{\mathit{J}}_e^{\mathrm{IN}}\left(\mathit{\vartheta }\right)+{\mathit{J}}_e^{\mathrm{RA}}\left(\mathit{\vartheta }\right)$$

(28)

where ${\mathit{J}}_e^{\mathrm{IN}}\left(\mathit{x}\right)$ and ${\mathit{J}}_e^{\mathit{R}A}\left(\mathit{x}\right)$ are simply the submatrices of ${\mathcal{I}}_{\mathrm{IN}}$ and ${\mathcal{I}}_{\mathrm{RA}}$, respectively. We evaluate the submatrices of ${\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{\vartheta }\right)$ for position, i.e., ${\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)$. Without loss of generality, we consider a scenario which contains a single time epoch and multiple UAVs. The multi-epoch scenario can be analyzed using the same method with state-reordering. According to Equation (27), the position-related EFIM by the visual feature measurement can be readily written as

$${\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)={\mathcal{I}}_{\mathit{x}}^{\mathrm{VF}}-{\left({\mathcal{I}}_{l_w,\mathit{x}}^{\mathrm{VF}}\right)}^T{\mathcal{I}}_{l_w}^{-1}{\mathcal{I}}_{l_w,\mathit{x}}^{\mathrm{VF}}$$

(29)

Substituting the expressions of the above symbols in Equation (29), the following statements are readily proved:

• ${\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)$ is a sparse-positive semidefinite matrix, and the non-zero block ${\left[{\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)\right]}_{m,n}$ indicates that m-th and n-th UAV have common visibility for some visual features.
• ${\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)$ is harmonic, i.e.,

  $$\sum _{m\in \mathcal{M}}{\left[{\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)\right]}_{m,n}=0$$

  (30)

We can prove that these properties also apply to the multi-epoch scenario. The above analysis implies that ${\mathit{J}}_e^{\mathrm{VF}}\left(\mathit{x}\right)$ is an extension version of a Laplacian matrix, which can be exploited for trajectory optimization [28].

4.3. CRLB for Visual–Inertial–Range Cooperative Localization

CRLB reflects the covariance bounds that an unbiased estimator can reach. Referring to [25,30], if the signal-to-noise ratio (SNR) is large enough, the CRLB for the 2-D case has the same expression as the one in the Cartesian coordination, i.e.,

$$\mathbf{C}(\mathit{\theta }\setminus {\mathit{\theta }}^{\prime })⪰{\mathcal{I}}_{2\mathrm{D}}^{-1}(\mathit{\theta }\setminus {\mathit{\theta }}^{\prime })$$

(31)

$\mathit{\theta }\setminus {\mathit{\theta }}^{\prime }$, which means that the states set ${\mathit{\theta }}^{\prime }$ is assumed to be known. For example, ${\mathit{\theta }}^{\prime }=\left\{{\mathit{l}}_w^1\right\}$ means that the first visual feature is assumed to be an anchor. Meanwhile, in the 3-D case, due to the unflatness property of manifold ${\mathbb{R}}^3\times \mathrm{SO}\left(3\right)$, the CRLB is not just the inverse of EFIM. However, according to [25,30], we can still approximate CRLB with the inverse of EFIM when the noise is relatively small.

However, due to the massive visual feature amount, it is preferred to only evaluate the CRLB for UAV states $\mathit{\vartheta }$. We can use the inversion of the EFIM to evaluate the performance bound for $\mathit{\vartheta }$, i.e.,

$${\mathbf{C}}^{†}(\mathit{\vartheta }\setminus {\mathit{\vartheta }}^{\prime })⪰{\mathit{J}}_e^{-1}(\mathit{\vartheta }\setminus {\mathit{\vartheta }}^{\prime })$$

(32)

5. Numerical Results

5.1. Correctness and Reachability of the CRLB

To evaluate the correctness and reachability of the CRLB, we construct some datasets with ground truth. Then, some measurement noises are applied based on the above assumptions. The noises that obey isotropic Langevin distribution are generated by the method described in [35].

We simulate $M=5$ UAVs to move along the lemniscate trajectory in 2-D space, as shown in Figure 4, denoted as Scenario 1. The time epoch T is set to 200. Each trajectory has a different initial position. The heading for each UAV is an integration of a normalized–distributed heading velocity. A total of 500 visual features are randomly generated around these trajectories. The FOV of the camera mounted on the UAV is set to ${90}^{\circ }$ and $f_c=250\mathrm{pixel}$ and ${\sigma }_{\mathit{u}}=0.5\mathrm{pixel}$. The range link is established between UAVs when their distance is less than $100\mathrm{m}$, and its std ${\sigma }_z$ is set to $0.05\mathrm{m}$. For the state transition measurement, we set ${\kappa }_{i,j}^m=50$ and ${\sigma }_{i,j}=0.1\mathrm{m}$ for all $i,j,m$.

Firstly, we evaluate the trajectory estimation performance for validating the correctness and reachability of the CRLB. We model the estimation problem as a classical bundle-adjustment problem. Then, the open-source Ceres Solver is exploited to solve this nonlinear least squares (NLS) optimization problem. A trust region method with a sparse normal Cholesky decomposition is applied as the optimization method. The initial point for this nonlinear optimization is set around the ground truth, by adding a small random value.

The root mean square error (RMSE) computed by N Monte Carlo simulations is used to evaluate the estimator performance, which is defined as

$${\mathrm{RMSE}}_{{\ast }_t^m}=\sqrt{\frac{1}{N}\sum _{i=1}^N{\Vert {}^i{\ast }_t^m-{\overline{\ast }}_t^m\Vert }_2^2}$$

(33)

“∗” stands for the UAV states, i.e., the position or rotation state. $\overline{\ast }$ is the ground truth of ∗. The Root CRLB (rCRLB) of UAV states is calculated with the first four known visual feature positions. The rCRLB can be calculated as the square root of the trace of CRLB for the corresponding states. For example, the rCRLB of position state ${\mathit{x}}_j^m$ is

$$\mathrm{rCRLB}\left({\mathit{x}}_j^m\right)=\sqrt{\mathrm{tr}\left({\mathbf{C}}_{{\mathit{x}}_j^m}^{†}\right)}$$

(34)

Firstly, the correctness and reachability of the performance bound in the 2-D case are evaluated, the results of which are shown in Figure 5, Figure 6 and Figure 7. For the first UAV, the rCRLB and RMSE are shown in Figure 5. As Figure 5 shows, the rCRLB and RMSE of the position state are almost equal. The mean difference between rCRLB and RMSE, i.e., $\left|\mathrm{rCRLB}-\mathrm{RMSE}\right|$ over time is $9.54\times {10}^{-4}\mathrm{m}$, and the relative difference between them is only $1.10\%$. This indicates that the CRLB is reachable using the NLS algorithm.

The rCRLB and RMSE for rotation are shown in Figure 6. The relative difference between the rCRLB and RMSE of rotation state is $2.02\%$.

We also evaluate the CRLB reachability with respect to the noise change. We define a noise magnitude parameter k, and the noise parameters ${\sigma }_{\mathit{u}}$, ${\sigma }_r$, ${\sigma }_{\mathit{y}}$, and $\kappa$ are then changed to $k{\sigma }_{\mathit{u}}$, $k{\sigma }_r$, ${\sigma }_{\mathit{y}}$, and $\frac{1}{k}\kappa$, respectively. The average rCRLB (arCRLB) and average RMSE (aRMSE) for different ks are compared. The aRMSE is defined as

$${\mathrm{aRMSE}}_{\ast }^m=\frac{1}{T}\sum _{t=1}^T{\mathrm{RMSE}}_{{\ast }_t^m}$$

(35)

The comparison result is shown as Figure 7. The average difference between arCRLB and aRMSE over ks is $0.71\%$. This shows that, for a relatively large noise, the CRLB is still reachable.

Then, we evaluate the reachability of the performance bound in the 3-D case. In this case, the trajectories of the UAVs are lemniscates with different heights, and all other parameters are the same as in the 2-D case. The 3-D trajectories and landmarks are shown in Figure 8.

Same as for the 2-D case, we compare the theoretic rCRLB and RMSE of NLS estimation using the Ceres Solver. With reference to the comparison results presented in Figure 9 and Figure 10, we can conclude that the CRLB of the UAV states can also be reached by NLS solver with smaller initial errors.

5.2. The Effect of the Visual Feature for CL

The loop closure is a powerful method in SLAM. It is performed when some visual features are covisible for two or more state factors. It can greatly reduce the long-term cumulative errors. In order to evaluate the effectiveness of the visual feature measurement for CL, we construct several simulations in the 2-D case as follows. We create simulation scenarios with/without visual loop closure, and then evaluate these absolute and relative position errors, respectively.

In contrast to absolute error, a relative position error only considers the error among UAVs. Therefore, it is useful to evaluate the accuracy of the formation. When the position error is relatively small, we can use the constraint CRLB to describe the relative positioning performance, which is given by [36]

$${\mathit{C}}_c={\mathit{U}}_c{\left({\mathit{U}}_c^{\top }{\mathit{J}}_e{\mathit{U}}_c\right)}^{-1}{\mathit{U}}_c^{\top }$$

(36)

where ${\mathit{J}}_e$ is the EFIM for the position states, i.e., ${\left\{{\mathit{x}}_t^m\right\}}_{t=1,2,\dots ,T}^{m=1,2,\dots ,M}$. ${\mathit{U}}_c$ is a semiunitary matrix whose columns form an orthonormal basis for the nullspace of the relative position function gradient ${\mathit{U}}_{NC}$, making ${\mathit{U}}_{\mathrm{NC}}^{\top }{\mathit{U}}_c=\mathbf{0}$. In 2-D space, $U_{NC}$ can be formulated as

$$\begin{array}{cc}\hfill {\mathit{U}}_{\mathrm{NC}}& =\left[\begin{array}{ccc}{v}_x& v_y& v_{\varphi }\end{array}\right]\hfill \\ \hfill v_x& ={\left[\begin{array}{ccccc}1& 0& 1& 0& \cdots \end{array}\right]}^{\top }\hfill \\ \hfill v_y& ={\left[\begin{array}{ccccc}0& 1& 0& 1& \cdots \end{array}\right]}^{\top }\hfill \\ \hfill v_{\varphi }& ={\left[\begin{array}{ccccc}-(y_1-\overline{y})& (x_1-\overline{x})& -(y_2-\overline{y})& (x_2-\overline{x})& \cdots \end{array}\right]}^{\top }\hfill \end{array}$$

(37)

In which $x_1$ and $y_1$ are the two components for the position state $x_1$. $\overline{x}$ and $\overline{y}$ are x- and y- components for the centroid of the UAVs. Once ${\mathit{U}}_{\mathrm{NC}}$ is constructed, ${\mathit{U}}_c$ can be calculated with the Gram–Schmidt process.

Firstly, we evaluate the effect of visual information with closure loop for the scenario depicted in Figure 4. The CRLBs with and without visual features are calculated.

The result shown in Figure 11 indicates that the position state error is greatly reduced when visual features are available. With loop closure, UAVs are able to determine the relative pose errors with earlier states, which makes the error lesser.

The constraint CRLB in this scenario is then evaluated. ${\mathit{C}}_c$ is calculated for each time epoch, denoted as ${\mathit{C}}_c\left(t\right)$. After that, the square root of $\left({\mathit{C}}_c\left(t\right)\right)$, $\mathrm{rcCRLB}$, is calculated as shown in Figure 12.

We can see that, when visual information is available, the constraint CRLB is also reduced, which signals the improvement in the formation accuracy. Unlike rCRLB, which diverges without visual information, rcCRLB retains a relatively small value. This is because UAVs can use the ranging information to maintain a stable relative formation.

Another scenario denoted as Scenario 3 for non-loopclosure is also constructed, in which the UAV trajectories are shown, as in Figure 13, whose length along the x-axis is more than 2 km.

With a long trajectory, UAVs are unable to see previous visual features older than a certain time, which prevents loop closure with a long period.

As Figure 14 shows, the rCRLB for this scenario diverges both with and without visual features. This is because the absolute localization of the UAV is unobservable with visual–inertial–range measurement. When no loopclosure is present, the UAV cannot reduce the localization uncertainty by the visual feature constraint either. Nevertheless, visual information greatly mitigates the divergence of the position error. With visual information, rCRLB is 63.7 m at $t=200$, whereas it becomes $518.2\mathrm{m}$ when visual information is neglected.

Figure 15 shows that, even without loop closure, visual features can still provide rich and effective information for formation. The average rcCRLB is $2.35\mathrm{m}$ without visual information, while it reduces to $1.07\mathrm{m}$ when visual information is added. It is worth noting that, at end of the time epoch, the rcCRLBs for both situations start to decrease. That is due to the fact that the relative position configuration of UAVs changes, which increases the total information of range measurements.

5.3. The Effect of the Range Measurement for CL: The Scenario of Large Baseline Cooperative Flying at High Altitudes

Cooperative flight at high altitudes is a valuable application as it enables the UAV swarm to efficiently cover a large area simultaneously. However, the conventional method of VIO for single UAV positioning at high altitudes faces significant challenges. This is primarily due to its reliance on accurate visual feature triangulation. The accurate estimation of a larger scene depth requires a wider range of motion to achieve a significant triangulation angle, which is crucial for a precise VIO system [37]. However, relying solely on the IMU for estimating this extended range results in excessive noise accumulation, leading to compromised positioning performance. In light of this, our following experiment demonstrates that the addition of range measurements between UAVs can significantly improve the positioning performance, and shows how the proposed metric helps us to analyze and optimize the measurement strategy for CL in the UAV swarm.

We construct a scenario in which a UAV swarm is flying at high altitude, as shown in Figure 16. This swarm consists of five UAVs flying at an altitude of approximately 250 m. Each pair of UAVs is spaced approximately 30 m apart.

To simulate the poor ego-motion estimation of high-altitude flying, we set the noise of the pose-graph measurement rather large, i.e., ${\kappa }_{i,j}^m=50$ and ${\sigma }_{i,j}=5m$ for all $i,j,m$, and the FOV of the camera is set to ${20}^{\circ }$. ${\sigma }_{\mathit{u}}$ and ${\sigma }_z$ are set to $1\mathrm{pixel}$ and $0.05\mathrm{m}$, respectively. Other parameters remain the same as above. Then, we evaluate the position performance using the CRLB metric, with and without the range measurements.

The rCRLBs of position and rotation states for UAV 1 are shown in Figure 17 and Figure 18, respectively. Figure 17 and Figure 18 show that, with the assistant or range measurements, the average rCLRB of position state for UAV 1 decreases by $34.5\%$ (from $23.1\mathrm{m}$ to $15.1\mathrm{m}$), while the average rCLRB of rotation state decreases by $34.0\%$ (from $0.134$ to $0.0883$). This proves that the range measurement can greatly improve the estimation accuracy when the other localization information is deficient.

6. Conclusions

In this paper, we first build the FIM of the visual–inertial–range cooperative localization in the product manifold ${\mathbb{R}}^n\times \mathrm{SO}\left(n\right)$. Then, some important properties of the FIM are analyzed, such as sparsity and harmonic structure, which are essential for practical application to analyze the performance of cooperative localization. Through a series of numerical simulations, we prove the effectiveness of this performance analysis tool and conclude that visual features can greatly improve CL performance, both in terms of absolute and relative localization accuracy. This shows that our performance analysis tool can be used in measurement selection for resource-constraint CL, which will be the subject of our future work.

While this paper focuses on the fundamental performance of CL with environment visual features for UAV swarms, it can be easily modified to be applicable to CL utilizing other environment information, such as LiDAR and event camera data.