Time-of-Arrival and Angle-of-Arrival Measurement-Assisted 3D Inter-Unmanned Aerial Vehicle Relative Localization Under Distance-Dependent Noise Model

Abstract

This paper addresses the 3D relative localization problem for two unmanned aerial vehicles (UAVs) using a combination of time-of-arrival (TOA) and angle-of-arrival (AOA) measurements across varied flight trajectories. We commenced by examining the problem of relative attitude estimation using only time-of-arrival (TOA) measurements, taking into account a distance-dependent noise model. To address this issue, we constructed a constrained weighted least squares (CWLS) problem and applied semidefinite relaxation (SDR) techniques for its resolution. Furthermore, we extended our analysis to incorporate AOA measurements and scrutinize the Cramer–Rao Lower Bound (CRLB) to illustrate enhanced localization accuracy through TOA-AOA integration compared to TOA alone under stable trajectory conditions. Ultimately, numerical simulations substantiate the efficacy of the proposed methodologies.

1. Introduction

Research on multi-unmanned aerial vehicle (UAV) systems has garnered considerable attention in recent years due to the promising applications of these systems across a multitude of fields, including emergency communication, disaster relief, and power grid detection [1,2,3,4,5,6]. Precise relative localization, which encompasses both position and orientation, is paramount for the cooperative stability of multiple UAVs [7,8,9,10,11], facilitating effective mission execution, collision avoidance, and strategic path planning within UAV swarms.

Traditionally, absolute positioning through global localization methods, such as GPS and wireless sensor networks (WSNs), are employed to indirectly ascertain relative positions among UAVs by providing each UAV’s location within a unified reference frame. The combination of GPS with inertial navigation systems (INSs) has been widely adopted, utilizing techniques such as belief propagation (BP) [12,13,14] or factor graph optimization [15,16] to augment both accuracy and continuity, even in instances where GPS signals are attenuated.

Wireless sensor networks (WSNs) provide an alternative for GPS-denied environments, utilizing time-of-arrival (TOA), angle-of-arrival (AOA), and frequency-of-arrival (FOA) measurements to derive relative positions [17,18,19,20,21,22]. TOA-based methods depend on time delays to estimate inter-node distances, whereas AOA and FOA capitalize on angular and frequency measurements to enhance positioning accuracy. Nevertheless, WSNs necessitate fixed anchors or established infrastructure, constraining their applicability in dynamic or infrastructure-less environments.

Given these constraints, relative pose estimation among UAVs presents an appealing alternative, facilitating a more adaptable and infrastructure-independent approach to uphold stability and coordination within UAV swarms [23,24,25,26,27,28]. By leveraging inter-UAV measurements directly—without the need for external anchors—relative localization provides a resilient solution that can operate effectively in GPS-denied or infrastructure-limited environments. This approach derives positional and orientational information exclusively from inter-UAV measurements, leveraging various sensing modalities, including visual, LiDAR, and radio signal assessments.

Visual and LiDAR-based methods bestow exceptional accuracy for UAV relative localization by delivering intricate spatial information and high resolution [29,30,31]. Vision-based approaches depend on camera sensors to extract and track visual features. However, they frequently necessitate considerable computational resources and are sensitive to environmental factors such as fluctuations in lighting and occlusions [32]. Similarly, LiDAR captures precise depth information through 3D point clouds, providing robustness in various lighting conditions [33]. However, factors such as fog, dust, and rain can scatter or absorb LiDAR signals, impacting performance [34]. Furthermore, LiDAR’s computational and energy demands limit its feasibility for lightweight UAVs, making both methods challenging for real-time applications within dynamic environments.

Radio-based methods present considerable advantages for UAV relative localization, particularly in dynamic, anchor-free environments where flexibility and minimal computational demands are crucial. In an inter-UAV range measurement system, two UAVs mutually exchange distance measurements to accumulate sufficient information for relative localization. This arrangement is straightforward and practical for dynamic, anchor-free contexts, rendering it a significant area of research.

Considering these advantages, various approaches have been proposed to tackle the relative pose estimation dilemma utilizing radio-based measurements. In particular, the constraints on the rotation matrix, such as the orthonormality condition ${\mathit{R}}^T\mathit{R}=\mathit{I}$, must be carefully considered to ensure valid solutions in relative pose estimation. For instance, ref. [27] formulates a constrained weighted least squares (CWLS) problem that disregards noise terms, transforming it into a semidefinite programming (SDP) problem solved through semidefinite relaxation (SDR). Building on this foundation, ref. [21] integrates quaternions into the framework and employs quadratic constrained quadratic programming (QCQP) to enhance estimation accuracy.

Further advancing the discourse, ref. [24] incorporates inertial measurement unit (IMU) errors into the model, devising a motion-based framework to account for cumulative error effects. This methodology also utilizes SDR for the solution and is validated through empirical experiments, underscoring its robustness, while other studies, such as [35], propose deterministic schemes with global exponential stability or robust fusion approaches [36] to address challenges like non-line-of-sight (NLOS) errors in complex environments.

Although effective, TOA-only methods may struggle to achieve high localization precision in certain scenarios. Inspired by recent advancements in visual-based [29,30,31] and AOA-based [20,21] localization techniques, we integrate AOA measurements with TOA. Advances in antenna technology [37,38,39] now enable compact angle measurement arrays on UAVs, making joint TOA-AOA localization feasible. By combining distance and angular information, our approach aims to enhance the precision and adaptability of inter-UAV relative pose estimation.

Despite recent advances, the challenge of joint TOA-AOA relative localization remains unaddressed, particularly in dynamic UAV environments. The specific influence of UAV flight trajectories on AOA measurements—namely, the variations in azimuth and elevation angles along different paths—has yet to be thoroughly investigated in the context of relative localization accuracy [28]. Understanding these angular variations is vital, as they directly affect the stability and precision of inter-UAV localization in practical applications. This highlights the pressing need for research into the trajectory-dependent factors shaping TOA-AOA localization, underscoring the potential for achieving more accurate and resilient UAV operations through optimized measurement integration.

In this paper, we develop an optimal joint relative pose estimation framework that leverages TOA and AOA measurements to enhance 3D relative localization accuracy between UAVs significantly. By integrating distance and angular data, our method transcends the limitations of conventional approaches that rely solely on distance, providing a more resilient solution for UAV operations in dynamic, infrastructure-constrained environments. Utilizing TOA and AOA information within an SDP framework, we address the inherent non-convexity of the pose estimation problem, transforming it into a convex optimization task through the application of SDR. Furthermore, we conduct a comprehensive Cramer–Rao Lower Bound (CRLB) analysis to evaluate the theoretical accuracy bounds of the proposed method across diverse noise levels and trajectory scenarios.

Our main contributions are as follows:

• The CRLB under a distance-dependent noise model for UAV relative pose estimation is derived, providing a theoretical benchmark for the achievable accuracy of TOA-AOA measurements.
• A novel joint TOA-AOA formulation for relative pose estimation is developed, combining distance and angle measurements to enhance localization accuracy in anchor-free environments.
• An efficient SDR estimator is devised to handle the nonlinearity and orthogonality constraints, and its effectiveness is demonstrated through comprehensive simulations under various noise and trajectory conditions.

The remainder of this paper is organized into the following sections. Section 2 introduces the problem formulation for 3D relative pose estimation between UAVs based on TOA and AOA measurements. Section 3 presents the SDR method for addressing the relative localization problem, initially applying it to TOA-only measurements and extending it to the combined TOA-AOA model. Section 4 derives the CRLB and analyzes its implications for localization accuracy. Section 5 provides numerical simulations to evaluate the performance of the proposed methods under various noise conditions and flight trajectories. This paper is concluded in Section 6.

In accordance with standard conventions, a bold lowercase letter (a) denotes a column vector, while a bold uppercase letter (A) signifies a matrix. The symbols ${(\ast )}^{-1}$ and ${(\ast )}^T$ are used to denote matrix inversion and transpose operations, respectively. The notation $\parallel ·\parallel$ refers to the ${\mathcal{L}}_2$ norm of a vector. The expression ${\left[\mathit{a}\right]}_i$ represents the i-th element of the vector $\mathit{a}$, and ${\left[\mathit{a}\right]}_{i:j}$ denotes the sub-vector of $\mathit{a}$ that includes elements from the i-th to the j-th position. Similarly, ${\left[\mathit{A}\right]}_{i,j}$ indicates the $(i,j)$-th element of the matrix $\mathit{A}$. The symbol $\mathit{I}$ stands for the identity matrix. The operation $\mathit{A}\otimes \mathit{B}$ denotes the Kronecker product of matrices $\mathit{A}$ and $\mathit{B}$. The functions $\mathrm{tr}\left(\mathit{A}\right)$ and $\mathrm{rank}\left(\mathit{A}\right)$ compute the trace and the rank of matrix $\mathit{A}$, respectively. For a square matrix $\mathit{A}$, the operation $\mathrm{vec}\left(\mathit{A}\right)$ results in a vector formed by stacking the columns of $\mathit{A}$ one beneath the other, such that ${\left[\mathrm{vec}\left(\mathit{A}\right)\right]}_{(i-1)N+1:iN}={\left[\mathit{A}\right]}_{:,i}$ for $i=1,2,\cdots ,N$.

2. Problem Formulation

In this section, we introduce the system model of two drones that achieve relative pose estimation using inter-drone distance measurement in 3D space.

System Model

Consider two drones, UAV 1 and UAV 2, operating in a 3D environment. UAV 1 serves as the reference with its position in the global coordinate system represented as ${\mathit{p}}^1={[x^1,y^1,z^1]}^T$. UAV 2, whose position is to be determined, is defined within its local body coordinate system as ${\mathit{p}}^2={[x^2,y^2,z^2]}^T$. To map the local coordinate system of UAV 2 to the global frame, we apply a coordinate transformation consisting of a rotation vector $\mathit{\phi }\in {\mathbb{R}}^3$ and a translation vector $\mathit{t}\in {\mathbb{R}}^3$.

To convert the rotation vector $\mathit{\phi }={[{\phi }_1,{\phi }_2,{\phi }_3]}^T\in {\mathbb{R}}^3$ into a rotation matrix $\mathit{R}$, we utilize the exponential map from the Lie algebra so(3) to the Lie group SO(3) [28]. The orthogonality constraints, specifically ${\mathit{R}}^T\mathit{R}=\mathit{I}$ and $\det \left(\mathit{R}\right)=1$ arise from the structural properties of rotation matrices, ensuring that $\mathit{R}$ represents a valid rotation in three-dimensional space.

The rotation matrix $\mathit{R}$ is computed as

$$\mathit{R}=exp\left({\mathit{\phi }}^{\wedge }\right)$$

(1)

where ${\mathit{\phi }}^{\wedge }$ is the skew-symmetric matrix associated with the rotation vector $\mathit{\phi }$. The exponential of this skew-symmetric matrix yields the rotation matrix required for the coordinate transformation [28]. The skew-symmetric matrix ${\mathit{\phi }}^{\wedge }$ is defined as

$${\mathit{\phi }}^{\wedge }=\left[\begin{array}{ccc}0& -{\phi }_3& {\phi }_2\\ {\phi }_3& 0& -{\phi }_1\\ -{\phi }_2& {\phi }_1& 0\end{array}\right].$$

(2)

Thus, the position of UAV 2 in the global coordinate system ${}^g\mathit{p}^2$ can be represented as

$${}^g\mathit{p}^2={[{}^{g}x^2,{}^{g}y^2,{}^{g}z^2]}^T=\mathit{R}{\mathit{p}}^2+\mathit{t}.$$

(3)

To achieve the relative localization of UAV 2 with respect to UAV 1, we utilize both TOA and AOA measurements. It is assumed that UAV 2 can transmit a signal containing its local coordinate information ${\mathit{p}}^2={[x^2,y^2,z^2]}^T$, which UAV 1 then receives. Upon reception, UAV 1 measures the distance to UAV 2 through TOA and estimates the relative angle through AOA. This signal processing provides UAV 1 with the range, angle, and UAV 2’s local coordinate data, enabling the transformation of UAV 2’s local position into the global frame and achieving precise relative localization. As illustrated in Figure 1.

The two UAVs undergo a total of N movements, each characterized by uniform rectilinear motion with a consistent time interval, $\mathrm{\Delta }t$, between movements. This ensures constant velocities, ${\mathit{v}}_n^1$ and ${\mathit{v}}_n^2$, for UAVs 1 and 2, respectively, without any acceleration. The position updates for the UAVs are given by

$$\begin{array}{cc}\hfill {\mathit{p}}_n^1+v_n^1\mathrm{\Delta }t& ={\mathit{p}}_{n+1}^1\hfill \\ \hfill {\mathit{p}}_n^2+v_n^2\mathrm{\Delta }t& ={\mathit{p}}_{n+1}^2\hfill \end{array}$$

(4)

where ${\mathit{p}}_n^1$ and $p_n^2$ denote the positions at the n-th movement step. The velocities of both UAVs at all movement steps are compactly represented in the column vector $\mathit{V}={[{\left(v_1^1\right)}^T,{\left(v_1^2\right)}^T,\cdots ,{\left(v_N^1\right)}^T,{\left(v_N^2\right)}^T]}^T$. We assume that the vector $\mathit{V}$ is error-free, meaning that the recorded velocities are accurate and do not contain any measurement inaccuracies. This assumption holds reliably over short time intervals.

Assume that the signal transmitted from UAV 2 contains a pilot header, and the clocks of the two UAVs are perfectly synchronized. Based on correlation computations, the range measurement can be obtained, making TOA feasible. For the n-th measurement, where $n=0,1,\cdots ,N-1$, the distance between the two UAVs is given by

$$\begin{array}{cc}\hfill d_n=d_n^o+\mathrm{\Delta }{d}_n& =\parallel \mathit{R}{\mathit{p}}_n^2+\mathit{t}-{\mathit{p}}_n^1\parallel +\mathrm{\Delta }{d}_n\hfill \\ \hfill & =\parallel \mathit{R}({\mathit{p}}_0^2+\sum _i^n{v}_i^2\mathrm{\Delta }t)+\mathit{t}-({\mathit{p}}_0^1+\sum _i^n{v}_i^1\mathrm{\Delta }t)\parallel +\mathrm{\Delta }{d}_n\hfill \end{array}$$

(5)

where the actual distance between UAV 1 and 2 is denoted by $d_n^o$. The distances between UAV 1 and UAV 2 at all movement steps are compactly represented in the column vector $\mathit{d}={[d_1,d_2,\cdots ,d_N]}^T$.

The measurement noise, characterized by a normal distribution with a mean of zero and a variance of ${\sigma }_d^2$, is represented by $\mathrm{\Delta }{d}_n$. It is noteworthy to mention that the noise covariance is proportional to the distance. This is due to the fact that the ranging is based on communication signals, and as the distance increases, the signal-to-noise ratio (SNR) decreases, leading to a higher noise variance. Following the approach in [40,41], we assume that the relationship between the variance and the distance satisfies the equation

$${\sigma }_{d_n}^2=\kappa {\left({\displaystyle \frac{d_n^o}{d_{ref}}}\right)}^{\mu },$$

(6)

where $\kappa$ and $\mu$ are parameters that define the scaling and power–law relationship between the noise variance and the distance, respectively. $d_{ref}$ is the reference distance, which we assume to be 1 m, $\kappa$ is 0.01, and $\mu$ is equal to 2.

To streamline the notation, we compile the noise elements into a single vector $\mathbf{\Delta }\mathit{d}$, which is transposed as ${[\mathrm{\Delta }{d}_1,\dots ,\mathrm{\Delta }{d}_n]}^T$. According to the distance-related noise model, the covariance matrix for the noise vector $\mathbf{\Delta }\mathit{d}$ is given by

$${\Sigma }_d=\mathrm{diag}\left(\kappa {\left(d_1^o\right)}^{\mu },\dots ,\kappa {\left(d_n^o\right)}^{\mu }\right),$$

(7)

where each element on the diagonal of ${\Sigma }_d$ corresponds to the variance of the noise for the respective distance measurement $d_n^o$.

Given only ranging measurements, our task is to solve for

$$arg\underset{\mathit{R},\mathit{t}}{max}\mathrm{Prob}\left(\mathit{d}\right|{\mathit{p}}_0^1,{\mathit{p}}_0^2,\mathit{V},\kappa ,\mu ,\mathit{R},\mathit{t})$$

(8)

The ranging function is a typical nonlinear and non-convex function due to the presence of Euclidean norms in the formulation. This inherent non-convexity significantly increases the complexity of the problem, making it challenging to find a global optimum.

For angular measurements, we denote ${\mathit{u}}_n$ as the unit vector representing the true angle between UAV 2 and UAV 1, as measured by UAV 1 in the current body frame coordinate system [30,42]. This value is defined as follows:

$${\mathit{u}}_n^o={\displaystyle \frac{{\mathit{R}}^T(\mathit{R}{\mathit{p}}_n^2+\mathit{t}-{\mathit{p}}_n^1)}{d_n^o}}={\displaystyle \frac{{\mathit{R}}^T(\mathit{R}{\mathit{p}}_n^2+\mathit{t}-{\mathit{p}}_n^1)}{\parallel \mathit{R}{\mathit{p}}_n^2+\mathit{t}-{\mathit{p}}_n^1\parallel }}$$

(9)

where $d_n^o$ represents the true distance at the current time instant. The noisy measurement of the angle can be expressed as

$${\mathit{u}}_n={\mathit{Q}}_n{\mathit{u}}_n^o$$

(10)

Here, ${\mathit{Q}}_n$ is a rotation matrix that also satisfies the SO(3) group. This definition is chosen to ensure that the vector $\mathit{u}$ maintains its characteristic as a unit vector. The unit vectors for the angular measurements between UAV 1 and UAV 2 at all movement steps are compactly represented in the column vector $\mathit{u}={[{\left({\mathit{u}}_1\right)}^T,{\left({\mathit{u}}_2\right)}^T,\cdots ,{\left({\mathit{u}}_N\right)}^T]}^T$.

When the angular measurement noise is relatively small, we can approximate the rotation matrix ${\mathit{Q}}_n$ as ${\mathit{Q}}_n=\mathit{I}+\mathrm{\Delta }{\mathit{Q}}_n$, where $\mathrm{\Delta }{\mathit{Q}}_n$ is an anti-symmetric matrix [18,19,20]. The specific form of $\mathrm{\Delta }{\mathit{Q}}_n$ is given by

$$\mathrm{\Delta }{\mathit{Q}}_n=\left[\begin{array}{ccc}0& -{\gamma }_n& {\beta }_n\\ {\gamma }_n& 0& -{\alpha }_n\\ -{\beta }_n& {\alpha }_n& 0\end{array}\right]$$

(11)

Here, ${\alpha }_n$, ${\beta }_n$, and ${\gamma }_n$ represent the small angular perturbations around the respective axes. Defining $\mathrm{\Delta }\mathit{q}={[{\alpha }_n,{\beta }_n,{\gamma }_n]}^T$, we assume that they follow a normal distribution with the covariance matrix ${\Sigma }_q$ being a diagonal matrix, denoted as ${\sigma }_q^2{\mathit{I}}_3$. In particular, we set ${\sigma }_q^2=\eta {\sigma }_d^2$. This choice is made to facilitate a comparative analysis of the extent to which the performance of ranging measurements is enhanced under varying levels of angular noise.

Similar to a ranging-only scenario, when angular measurements are available, our task is to solve for

$$arg\underset{\mathit{R},\mathit{t}}{max}\mathrm{Prob}(\mathit{d},\mathit{u}|{\mathit{p}}_0^1,{\mathit{p}}_0^2,\mathit{V},\kappa ,\mu ,\mathit{R},\mathit{t})$$

(12)

3. SDR Method for Relative Localization

In this section, we first develop a TOA-based model for relative localization and then extend it to incorporate AOA measurements. Through semidefinite relaxation, we transform the problem into a solvable convex form to estimate the rotation matrix and translation vector.

3.1. SDR Method Using TOA Only

We commence our analysis with the TOA measurement model, as outlined in Equation (5). This particular issue has been investigated in prior research, as documented by [27].

By transferring $\mathrm{\Delta }{d}_n$ to the left side and squaring both sides of the equation, we arrive at the simplified expression:

$${\parallel {\mathit{p}}_n^1\parallel }^2+{\parallel {\mathit{p}}_n^2\parallel }^2-2{\left({\mathit{p}}_n^1\right)}^T(\mathit{R}{\mathit{p}}_n^2+\mathit{t})+2{\mathit{t}}^T\mathit{R}{\mathit{p}}_n^2+{\parallel \mathit{t}\parallel }^2\approx d_n^2-2d_n^o\mathrm{\Delta }{d}_n$$

(13)

The squared second-order noise contribution, $\mathrm{\Delta }{d}_n^2$, is excluded from consideration since it is considered trivial in comparison to the first-order noise term, given the premise that noise levels are minimal.

Using the matrix vectorization formula [27]

$$vec\left(\mathit{X}\mathit{Y}\mathit{Z}\right)=\left({\mathit{Z}}^T\otimes \mathit{X}\right)vec\left(\mathit{Y}\right)$$

(14)

we can rewrite Equation (13) as

$$\begin{array}{cc}\hfill & -2[{\left({\mathit{p}}_n^1\right)}^T\otimes {\left({\mathit{p}}_n^2\right)}^T]\mathrm{vec}\left(\mathit{R}\right)-2{\left({\mathit{p}}_n^1\right)}^T\mathit{t}+2{\left({\mathit{p}}_n^2\right)}^T{\mathit{R}}^T\mathit{t}+{\parallel \mathit{t}\parallel }^2\hfill \\ \hfill & \approx d_n^2-\parallel {\mathit{p}}_n^1{\parallel }^2-{\parallel {\mathit{p}}_n^2\parallel }^2-2d_n^o\mathrm{\Delta }{d}_n\hfill \end{array}$$

(15)

By defining $\mathit{x}=[vec{\left(\mathit{R}\right)}^T,{\mathit{t}}^T,{\left({\mathit{R}}^T\mathit{t}\right)}^T,{\parallel \mathit{t}\parallel }^2{]}^T\in {\mathbb{R}}^{16}$ as the unknown variable, we can rewrite Equation (15) into a more compact matrix form:

$$\mathit{A}x-\mathit{b}\approx \mathit{B}n,$$

(16)

where for $n=0,1,\cdots ,N-1$, we have

$$\begin{array}{cc}\hfill \mathit{A}& =\left[\begin{array}{cccc}⋮& ⋮& ⋮& ⋮\\ 2{\left({\mathit{p}}_n^1\right)}^T\otimes {\left({\mathit{p}}_n^2\right)}^T& 2{\left({\mathit{p}}_n^1\right)}^T& -2{\left({\mathit{p}}_n^2\right)}^T& -1\\ ⋮& ⋮& ⋮& ⋮\end{array}\right]\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill \mathit{b}& =\left[\begin{array}{c}⋮\\ -d_n^2+{\parallel {\mathit{p}}_n^1\parallel }^2+{\parallel {\mathit{p}}_n^2\parallel }^2\\ ⋮\end{array}\right]\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill n& =\mathbf{\Delta }\mathit{d}\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill \mathit{B}& =2\mathrm{diag}(d_1^o,d_2^o,\cdots ,d_n^o)\hfill \end{array}$$

(20)

Building upon Equation (16) and considering the SO(3) constraint for the rotation matrix $\mathit{R}$, we can pose the CWLS problem as follows:

$$\begin{array}{cc}\hfill {\mathit{x}}^{*}=arg\underset{\mathit{x}}{min}& {(\mathit{A}\mathit{x}-\mathit{b})}^T\mathit{W}(\mathit{A}\mathit{x}-\mathit{b})\hfill \end{array}$$

(21a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& \mathit{R}\in \mathrm{SO}\left(3\right)\hfill \end{array}$$

(21b)

The weighting matrix $\mathit{W}$ is defined as the inverse of ${\mathit{B}}^T{\Sigma }_d\mathit{B}$:

$$\mathit{W}={\left({\mathit{B}}^T{\Sigma }_d\mathit{B}\right)}^{-1}.$$

(22)

Let us begin by considering the structure of the constraint Equation (21b). The constraint $\mathit{R}\in \mathrm{SO}\left(3\right)$ implies that the rotation matrix $\mathit{R}$ must be a member of the special orthogonal group, which requires that ${\mathit{R}}^T\mathit{R}=\mathit{I}$ and $\det \left(\mathit{R}\right)=1$. For the purpose of our derivation, we initially disregard the determinant constraint.

${\mathit{R}}^T\mathit{R}=\mathit{I}$ leads to a total of six constraint equations. Additionally, considering the nonlinear terms involving ${\mathit{R}}^T\mathit{t}$ and ${\mathit{t}}^T\mathit{t}$ within the vector $\mathit{x}$, we can further augment the number of constraint equations. According to the derivation presented by [27], it is sufficient to employ a total of ten constraint equations to solve the problem. This result is substantiated by [28], which demonstrates that the constraint space formed by these ten equations is sufficiently tight.

By using the method of increasing the order, known as the higher-order lifting function of the vector $\mathit{x}$, we define

$$\mathit{X}=\left[\begin{array}{c}\mathit{x}\\ 1\end{array}\right]\left[\begin{array}{cc}{\mathit{x}}^T& 1\end{array}\right]$$

(23)

The ten constraint equations can be represented using inner products (dot products) as follows:

$$\langle {\mathit{C}}_i,\mathit{X}\rangle =q_i,i=1,\dots ,10$$

(24)

where ${\mathit{C}}_i$ is a $17\times 17$ symmetric matrix. Each ${\mathit{C}}_i$ corresponds to the construction of a different constraint equation, with $q_i$ representing the constant associated with each of the ten constraints.

We will employ a sparse matrix notation to construct the matrix ${\mathit{C}}_i$. Specifically, this construction involves identifying the row and column indices of the non-zero elements along with their corresponding values. In the case of ${\mathit{C}}_i$, its sparse representation comprises M non-zero entries, with the set of row indices denoted as $\mathit{r}={[r_1,\dots ,r_M]}^T$, the set of column indices as $\mathit{c}={[c_1,\dots ,c_M]}^T$, and the non-zero values $\theta$ at ${\mathit{C}}_i(r_m,c_m),\mathrm{for}m=1,\cdots ,M$. Consequently, ${\mathit{C}}_i$ can be succinctly expressed as ${\mathit{C}}_i=\mathrm{sparse}(\mathbf{r},\mathbf{c},\theta )$ [26].

The representations of these ten ${\mathit{C}}_i$ matrices are as follows:

$$\begin{array}{cc}\hfill {\mathit{C}}_1& =\mathrm{sparse}({[1,2,3]}^T,{[1,2,3]}^T,{\mathbf{1}}_3),q_1=1\hfill \end{array}$$

(25a)

$$\begin{array}{cc}\hfill {\mathit{C}}_2& =\mathrm{sparse}({[4,5,6]}^T,{[4,5,6]}^T,{\mathbf{1}}_3),q_2=1\hfill \end{array}$$

(25b)

$$\begin{array}{cc}\hfill {\mathit{C}}_3& =\mathrm{sparse}({[7,8,9]}^T,{[7,8,9]}^T,{\mathbf{1}}_3),q_3=1\hfill \end{array}$$

(25c)

$$\begin{array}{cc}\hfill {\mathit{C}}_4& =\mathrm{sparse}({[1,4,7]}^T,{[1,4,7]}^T,{\mathbf{1}}_3),q_4=1\hfill \end{array}$$

(25d)

$$\begin{array}{cc}\hfill {\mathit{C}}_5& =\mathrm{sparse}({[2,5,8]}^T,{[2,5,8]}^T,{\mathbf{1}}_3),q_5=1\hfill \end{array}$$

(25e)

$$\begin{array}{cc}\hfill {\mathit{C}}_6& =\mathrm{sparse}({[1,4,7]}^T,{[2,5,8]}^T,{\mathbf{1}}_3),q_6=0\hfill \end{array}$$

(25f)

$$\begin{array}{cc}\hfill {\mathit{C}}_7& =\mathrm{sparse}({[1,4,7,17]}^T,{[10,11,12,13]}^T,{[{\mathbf{1}}_3^T,-1]}^T),q_7=0\hfill \end{array}$$

(25g)

$$\begin{array}{cc}\hfill {\mathit{C}}_8& =\mathrm{sparse}({[2,5,8,17]}^T,{[10,11,12,14]}^T,{[{\mathbf{1}}_3^T,-1]}^T),q_8=0\hfill \end{array}$$

(25h)

$$\begin{array}{cc}\hfill {\mathit{C}}_9& =\mathrm{sparse}({[10,11,12,17]}^T,{[10,11,12,16]}^T,{[{\mathbf{1}}_3^T,-1]}^T),q_9=0\hfill \end{array}$$

(25i)

$$\begin{array}{cc}\hfill {\mathit{C}}_{10}& =\mathrm{sparse}({[13,14,15,17]}^T,{[13,14,15,16]}^T,{[{\mathbf{1}}_3^T,-1]}^T),q_{10}=0\hfill \end{array}$$

(25j)

The ultimate expression for Formula (21a) can be cast by setting

$$\mathit{D}=\left[\begin{array}{cc}{\mathit{A}}^T\mathit{A}\mathit{W}& \mathit{b}\mathit{W}\\ {\mathit{b}}^T\mathit{W}& {\mathit{b}}^T\mathit{b}\mathit{W}\end{array}\right]$$

(26)

which allows us to reformulate it as

$$\begin{array}{cc}\hfill {\mathit{X}}^{*}& =arg\underset{\mathit{X}}{min}\mathrm{tr}\left(\mathit{D}\mathit{X}\right)\hfill \end{array}$$

(27a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& \left(25\right),\mathit{X}(17,17)=1,\mathrm{rank}\left(\mathit{X}\right)=1,\mathit{X}\ge 0\hfill \end{array}$$

(27b)

The rank constraint $\mathrm{rank}\left(\mathbf{X}\right)=1$ makes the problem inherently non-convex. To make the problem solvable, we apply the SDR technique by relaxing this constraint and later compensating for it. By eliminating the rank constraint, the problem is converted into an SDP issue, which can be effectively tackled using the CVX toolbox. The matrix $\mathit{W}$, specified in Equation (22), remains undetermined due to its reliance on the actual values. To facilitate a feasible approach, we initialize $\mathit{W}$ as the identity matrix and proceed to solve the problem presented in Equation (27) for an initial approximation. Subsequently, this preliminary estimate is employed to refine the weighting matrix. Following this adjustment, we re-solve the problem in Equation (27) to arrive at the ultimate solution.

If the SDP solution is tight enough, then we should have $\mathrm{rank}\left({\mathit{X}}^{*}\right)=1$. We can use the Singular Value Decomposition (SVD) to obtain ${\mathit{x}}^{*}$ and extract ${\mathit{R}}_{\mathrm{SDP}}=[{\mathit{x}}_{1:3},{\mathit{x}}_{4:6},{\mathit{x}}_{7:9}]$ and ${\mathit{t}}^{*}={\mathit{x}}_{10:12}$. Since we relaxed the constraint $\det \left(\mathit{R}\right)=1$ earlier, we need to compensate for this here. After obtaining ${\mathit{R}}_{\mathrm{SDP}}$, we seek ${\mathit{R}}^{*}$ by solving

$$\begin{array}{cc}\hfill \underset{{\mathit{R}}^{*}}{\arg \min }& \parallel {\mathit{R}}^{*}-{\mathit{R}}_{\mathrm{SDP}}{\parallel }_{\mathrm{F}}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathit{R}}^{*}\in \mathrm{SO}\left(3\right),\hfill \end{array}$$

(28)

where ${\parallel ·\parallel }_{\mathrm{F}}$ represents the Frobenius norm. We can use the Procrustes algorithm [27], specifically the SVD-based method, to find the solution as follows:

$$\begin{array}{cc}\hfill {\mathit{R}}_{\mathrm{SDP}}& =\mathit{U}\Lambda {\mathit{V}}^{\mathit{T}}\hfill \end{array}$$

(29a)

$$\begin{array}{cc}\hfill {\mathit{R}}^{*}& ={\mathit{U}\mathit{V}}^{\mathit{T}}\hfill \end{array}$$

(29b)

After the optimization process, the final results that we obtain are the rotation matrix ${\mathit{R}}^{*}$ and the translation vector ${\mathit{t}}^{*}$. To convert the rotation matrix ${\mathit{R}}^{*}$ back into a rotation vector using Lie algebra, we apply the logarithmic map [28]. The rotation vector ${\mathit{\phi }}^{*}$ can be obtained from the rotation matrix ${\mathit{R}}^{*}$ using the following relationship:

$${\mathit{\phi }}^{*}=\ln {\left({\mathit{R}}^{*}\right)}^{\vee }$$

(30)

where the operation ${·}^{\vee }$ denotes the conversion of a skew-symmetric matrix back into a vector. We list the steps of SDR-TOA in Algorithm 1.

Algorithm 1 SDR-TOA.

Data: range measurement vector $\mathit{d}$, UAV speed vector $\mathit{V}$, UAV initial positions ${\mathit{p}}_0^1$ and ${\mathit{p}}_0^2$, noise related parameters $\kappa$ and $\mu$ Result: relative pose estimation ${\phi }^{*}$ and ${\mathit{t}}^{*}$   1: Compute $\mathit{A}$ and $\mathit{b}$ using (17) and (18) respectively;   2: Initialize $\mathit{W}=\mathit{I}$;   3: Compute $\mathit{D}$ using (26);   4: Construct constraints equations using (25);   5: Solve ${\mathit{x}}^{*}$ by optimizing (27);   6: Extract ${\mathit{t}}^{*}$ and ${\mathit{R}}_{SDP}$ from ${\mathit{x}}^{*}$;   7: Compute ${\mathit{\phi }}^{*}$ using (29) and (30);   8: Update $\mathit{W}$ using (22);   9: Solve ${\mathit{x}}^{*}$ by optimizing (27) with updated $\mathit{W}$; 10: Repeat 6–7;

3.2. SDR Method Using TOA and AOA Measurements

In this section, we will introduce an SDR approach for the TOA-AOA relative localization issue. First, by reformulating Equation (9), we obtain the following:

$$\begin{array}{cc}\hfill d_n\mathit{R}{\mathit{u}}_n& =(d_n^o+\mathrm{\Delta }d)\mathit{R}(\mathit{I}+\mathrm{\Delta }{\mathit{Q}}_n){\mathit{u}}_n^o\hfill \\ \hfill & =\mathit{R}{\mathit{p}}_n^2+\mathit{t}-{\mathit{p}}_n^1+d_n\mathit{R}\mathrm{\Delta }{\mathit{Q}}_n{\mathit{u}}_n+\mathrm{\Delta }{d}_n{\mathbf{Ru}}_n+\mathrm{\Delta }{d}_n\mathit{R}\mathrm{\Delta }{\mathit{Q}}_n{\mathit{u}}_n\hfill \end{array}$$

(31)

Utilizing Equation (11), we obtain the following: 

$$\begin{array}{cc}\hfill \mathrm{vec}\left(\mathrm{\Delta }{\mathit{Q}}_n\right)& =\mathit{L}\mathrm{\Delta }{\mathit{q}}_n\hfill \\ \hfill & ={\left[\begin{array}{ccccccccc}0& 0& 0& 0& 0& -1& 0& 1& \\ 0& 0& 1& 0& 0& 0& -1& 0& \\ 0& -1& 0& 1& 0& 0& 0& 0\end{array}\right]}^T\left[\begin{array}{c}\mathrm{\Delta }{\alpha }_n\\ \mathrm{\Delta }{\beta }_n\\ \mathrm{\Delta }{\gamma }_n\end{array}\right]\hfill \end{array}$$

(32)

Omitting the second-order noise term $\mathrm{\Delta }{d}_n{\mathit{RQ}}_n{u}_n$, and using Equations (14) and (32), we have, for $n=1,2,\cdots ,N$, the following:

$$[{\mathit{u}}_n^T\otimes \mathit{I}]\mathrm{vec}\left(\mathit{R}\right)-[{\left({\mathit{p}}_n^2\right)}^T\otimes \mathit{I}]\mathrm{vec}\left(\mathit{R}\right)-\mathit{t}+{\mathit{p}}_n^1=d_n[u_n^T\otimes \mathit{R}]\mathit{L}\mathrm{\Delta }{\mathit{p}}_n+\mathrm{\Delta }{d}_n{\mathit{Ru}}_n$$

(33)

By defining ${\mathit{x}}_H=[\mathrm{vec}{\left(\mathit{R}\right)}^T,{\mathit{t}}^T]\in {\mathbb{R}}^{12}$, we can construct a CWLS problem similar to Equation (16) as follows:

$${\mathit{A}}_H\mathit{x}+{\mathit{b}}_H\simeq {\mathit{B}}_H{n}_H,$$

(34)

where for $n=0,1,\cdots ,N-1$, we have

$$\begin{array}{cc}\hfill {\mathit{A}}_H& =\left[\begin{array}{cc}⋮& ⋮\\ u_n^T\otimes \mathit{I}-{\left({\mathit{p}}_n^2\right)}^T\otimes \mathit{I}& -1\\ ⋮& ⋮\end{array}\right]\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill {\mathit{b}}_H& =\left[\begin{array}{c}⋮\\ {\mathit{p}}_n^1\\ ⋮\end{array}\right]\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill {\mathit{B}}_H& =\left[\begin{array}{cc}⋮& ⋮\\ d_n[{\mathit{u}}_n^T\otimes \mathit{R}]\mathit{L}& {\mathit{Ru}}_n\\ ⋮& ⋮\end{array}\right]\hfill \end{array}$$

(37)

$$\begin{array}{cc}\hfill n_H& =\left[\begin{array}{c}⋮\\ \left[\begin{array}{c}\mathrm{\Delta }{\mathit{q}}_n\\ \mathrm{\Delta }{d}_n\end{array}\right]\\ ⋮\end{array}\right]\hfill \end{array}$$

(38)

The covariance matrix of the noise vector $n_H$, denoted as ${\Sigma }_H$, can be expressed as ${\Sigma }_H=\mathrm{blkdiag}\{{\Sigma }_{H1},\dots ,{\Sigma }_{HN}\}$. For each $n=1,\dots ,N$, the corresponding block ${\Sigma }_{Hn}$ is given by ${\Sigma }_{Hn}=\mathrm{blkdiag}\{{\Sigma }_q,{\Sigma }_d\}$, where ${\Sigma }_q$ and ${\Sigma }_d$ are the covariance matrices for the noise associated with angle measurement and distance measurement, respectively. The weighting matrix ${\mathit{W}}_H$ is defined as ${\mathit{W}}_H={\left({\mathit{B}}_H^T{\Sigma }_H{\mathit{B}}_H\right)}^{-1}.$

Similarly, define ${\mathit{D}}_H$ as

$${\mathit{D}}_H=\left[\begin{array}{cc}{\mathit{A}}_H^T{\mathit{A}}_H{\mathit{W}}_H& {\mathit{b}}_H{\mathit{W}}_H\\ {\mathit{b}}_H^T{\mathit{W}}_H& {\mathit{b}}_H^T{\mathit{b}}_H{\mathit{W}}_H\end{array}\right]$$

(39)

and let ${\mathit{X}}_H$ be defined as

$${\mathit{X}}_H=\left[\begin{array}{c}{\mathit{x}}_H\\ 1\end{array}\right]\left[\begin{array}{cc}{\mathit{x}}_H^T& 1\end{array}\right]\in {\mathbb{R}}^{13\times 13}$$

(40)

Finally, we arrive at a formula similar to (27), but it is important to note that at this point, ${\mathit{x}}_H\in {\mathbb{R}}^{12}$ contains only the linear terms of $\mathit{R}$ and $\mathit{t}$, and does not include the nonlinear terms such as ${\mathit{R}}^T\mathit{t}$ or $\parallel {\mathit{t}}^T\mathit{t}\parallel$. Consequently, the constraint equations are limited to the six equations involving the rotation matrix, which are ${\mathit{R}}^T\mathit{R}=\mathit{I}$. Thus, we can formulate the following semidefinite programming (SDP) problem:

$$\begin{array}{cc}\hfill {\mathit{X}}_H^{*}& =arg\underset{{\mathit{X}}_H}{min}\mathrm{tr}\left({\mathit{D}}_H{\mathit{X}}_H\right)\hfill \end{array}$$

(41a)

$$\begin{array}{cc}\hfill \mathrm{s}.\mathrm{t}.& \left(25a\right)-\left(25f\right),{\mathit{X}}_H(13,13)=1,\mathrm{rank}\left({\mathit{X}}_H\right)=1,{\mathit{X}}_H\ge 0\hfill \end{array}$$

(41b)

By relaxing the rank constraint, we ultimately formulate a solvable SDP problem. Similar to the TOA-only scenario, the obtained solution ${\mathit{X}}_H$ can also be decomposed using SVD to yield estimates of the rotation matrix ${\mathit{R}}_{HSDP}$ and translation vector ${\mathit{t}}_H^{*}$. Utilizing the Procrustes algorithm in conjunction with the Log map, we ultimately derive the estimated rotation vector ${\mathit{\phi }}_H^{*}$ and the translation vector ${\mathit{t}}_H^{*}$ through the combined TOA-AOA estimation process. We list the steps of SDR-TOA in Algorithm 2 below.

Algorithm 2 SDR-TOA-AOA.

Data: range measurement vector $\mathit{d}$, angular measurement vector $\mathit{u}$, UAV speed vector $\mathit{V}$, UAV initial positions ${\mathit{p}}_0^1$ and ${\mathit{p}}_0^2$, noise related parameters $\kappa$ and $\mu$ Result: relative pose estimation ${\mathit{\phi }}^{*}$ and ${\mathit{t}}^{*}$   1: Compute ${\mathit{A}}_H$ and ${\mathit{b}}_H$ using (35) and (36) respectively;   2: Initialize ${\mathit{W}}_H=\mathit{I}$;   3: Compute ${\mathit{D}}_H$ using (39);   4:bConstruct constraints equations using (25);   5: Solve ${\mathit{x}}_H^{*}$ by optimizing (27);   6: Extract ${\mathit{t}}_H^{*}$ and ${\mathit{R}}_{HSDP}$ from ${\mathit{x}}_H^{*}$;   7: Compute ${\mathit{\phi }}_H^{*}$ using (29) and (30);   8: Update ${\mathit{W}}_H$ using (37);   9: Solve ${\mathit{x}}_H^{*}$ by optimizing (41) with updated ${\mathit{W}}_H$; 10: Repeat 6–7;

4. Performance Analysis

In this section, we derive the CRLB to assess the impact of AOA measurements on localization accuracy, highlighting their effectiveness in enhancing estimation precision.

4.1. CRLB Analysis for TOA Measurements Only

In this section, we proceed to derive the CRLB for relative pose estimation when only TOA-based range measurements are available.

Firstly, we define the vector of parameters to be estimated as $\mathit{ϵ}={[{\mathit{\phi }}^T,{\mathit{t}}^T]}^T$, where $\mathit{\phi }$ and $\mathit{t}$ represent the rotation vectors and translation vectors, respectively. We can then construct the likelihood function for the range measurements as follows:

$$p\left(\mathit{d}\right|\mathit{ϵ})={\displaystyle \frac{1}{\sqrt{{\left(2\pi \right)}^{N}det{\left({\Sigma }_{\mathit{d}}\right)}^{1/2}}}}exp\left(-{\displaystyle \frac{1}{2}}{(\mathit{d}-{\mathit{d}}^o)}^T{\Sigma }_{\mathit{d}}^{-1}(\mathit{d}-{\mathit{d}}^o)\right)$$

(42)

Here, $\mathit{d}$ denotes the vector of range measurements, ${\mathit{d}}^o$ is the vector of real range measurements, ${\Sigma }_{\mathit{d}}$ is the covariance matrix of the range measurement errors, and N is the number of measurements.

When the covariance of the measurements is correlated with the variables to be estimated, we can express the CRLB and the Fisher information matrix in the following form [40,41]:

$${\mathrm{CRLB}}_d\left(\mathit{ϵ}\right)={\mathrm{F}}_d{\left(\mathit{ϵ}\right)}^{-1}={\left({\left[{\displaystyle \frac{\partial {\mathit{d}}^o}{\partial \mathit{ϵ}}}\right]}^T{\Sigma }_d^{-1}\left[{\displaystyle \frac{\partial {\mathit{d}}^o}{\partial \mathit{ϵ}}}\right]+{\displaystyle \frac{1}{2}}tr\left[{\left({\Sigma }_d^{-1}{\displaystyle \frac{\partial {\Sigma }_d}{\partial \mathit{ϵ}}}\right)}^2\right]\right)}^{-1}$$

(43)

where the i-th row of $\left[{\displaystyle \frac{\partial d^o}{\partial \mathit{ϵ}}}\right]$, denoted as ${\left[{\displaystyle \frac{\partial {\mathit{d}}^o}{\partial \mathit{ϵ}}}\right]}_{i,:}$, could be expressed as follows:

$${\left[{\displaystyle \frac{\partial d^o}{\partial \mathit{ϵ}}}\right]}_{i,:}={\left[\begin{array}{c}{\displaystyle \frac{{\left(\mathit{R}{\mathit{p}}_i^2+\mathit{t}-{\mathit{p}}_i^1\right)}^T\left({\mathit{p}}_i^{2T}\otimes {\mathit{I}}_3\right)}{\parallel \mathit{R}{\mathit{p}}_i^2+\mathit{t}-{\mathit{p}}_i^1\parallel }}\\ {\displaystyle \frac{{\left(\mathit{R}{\mathit{p}}_i^2+\mathit{t}-{\mathit{p}}_i^1\right)}^T}{\parallel \mathit{R}{\mathit{p}}_i^2+\mathit{t}-{\mathit{p}}_i^1\parallel }}\end{array}\right]}^T\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathrm{vec}\left(\mathit{R}\right)}{\partial {\phi }^T}}& {\mathbf{0}}_{9\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]$$

(44)

The term ${\displaystyle \frac{\partial {\Sigma }_d}{\partial \mathit{ϵ}}}$ can be expressed as

$${\displaystyle \frac{\partial {\Sigma }_d}{\partial \mathit{ϵ}}}=\mathrm{diag}\left\{\kappa \mu {\left(d_1^0\right)}^{\mu -1},\cdots ,\kappa \mu {\left(d_N^0\right)}^{\mu -1}\right\}·{\displaystyle \frac{\partial {\mathit{d}}^o}{\partial \mathit{ϵ}}}$$

(45)

4.2. CRLB Analysis for TOA-AOA Measurements

Next, we proceed to derive the CRLB for relative pose estimation when both TOA range measurements and AOA angular measurements are available.

Considering that ranging and angle measurements are independent of each other, the joint likelihood function under the TOA-AOA scenario can be expressed as

$$\begin{array}{cc}\hfill p(\mathit{d},\mathit{u}|\mathit{ϵ})& =p\left(\mathit{d}\right|\mathit{ϵ}\left)p\right(\mathit{u}\left|\mathit{ϵ}\right)\hfill \\ \hfill & =p\left(\mathit{d}\right|\mathit{ϵ}){\displaystyle \frac{1}{\sqrt{{\left(2\pi \right)}^{N}det{\left({\Sigma }_{\mathit{u}}\right)}^{1/2}}}}exp\left(-{\displaystyle \frac{1}{2}}{(\mathit{u}-{\mathit{u}}^o)}^T{\Sigma }_{\mathit{u}}^{-1}(\mathit{u}-{\mathit{u}}^o)\right)\hfill \end{array}$$

(46)

Utilizing Formulas (10), (11), and (32), we can express the angular measurement error in the form of an additive domain as $\mathrm{\Delta }\mathit{u}={\left[{({\mathit{u}}_1-{\mathit{u}}_1^o)}^T,\cdots ,{({\mathit{u}}_N-{\mathit{u}}_N^o)}^T\right]}^T,$ where ${\mathit{u}}_i$ denotes the measured angle and ${\mathit{u}}_i^o$ denotes the true angle for the i-th measurement. To further compute the covariance of the angular measurement error, ${\Sigma }_u$, it can be represented as follows:

$$\begin{array}{cc}\hfill {\Sigma }_u& ={\mathit{H}}_u({\mathit{I}}_N\otimes {\Sigma }_q){\mathit{H}}_u^T\hfill \\ \hfill {\mathit{H}}_u& =\mathrm{blkdiag}\left\{[{\left({\mathit{u}}_1^o\right)}^T\otimes {\mathit{I}}_3]\mathit{L},\cdots ,[{\left({\mathit{u}}_N^o\right)}^T\otimes {\mathit{I}}_3]\mathit{L}\right\}\hfill \end{array}$$

(47)

Since the noise in angle measurement is indirectly dependent of the range as ${\sigma }_q^2=\eta {\sigma }_d^2$, the CRLB and the Fisher information matrix under TOA-AOA measurements could be formulated as

$$\begin{array}{cc}\hfill {\mathrm{CRLB}}_H\left(\mathit{ϵ}\right)& ={\left({\mathrm{F}}_d\left(\mathit{ϵ}\right)+{\mathrm{F}}_a\left(\mathit{ϵ}\right)\right)}^{-1}\hfill \\ \hfill {\mathrm{F}}_a\left(\mathit{ϵ}\right)& ={\left[{\displaystyle \frac{\partial {\mathit{u}}^o}{\partial \mathit{ϵ}}}\right]}^T{\Sigma }_u^{-1}\left[{\displaystyle \frac{\partial {\mathit{u}}^o}{\partial \mathit{ϵ}}}\right]+{\displaystyle \frac{1}{2}}tr\left[{\left({\Sigma }_u^{-1}{\displaystyle \frac{\partial {\Sigma }_u}{\partial \mathit{ϵ}}}\right)}^2\right]\hfill \end{array}$$

(48)

By substituting Equation (9), we can express the term $\left[{\displaystyle \frac{\partial {\mathit{u}}^o}{\partial \mathit{ϵ}}}\right]$ as follows:

$${\displaystyle \frac{\partial {\mathit{u}}^o}{\partial \mathit{ϵ}}}=\mathrm{diag}\left\{d_1^o,\cdots ,d_N^o\right\}\left[{\displaystyle \frac{\partial {\mathit{d}}^o}{\partial \mathit{ϵ}}}\right]$$

(49)

Considering that the $\mathrm{diag}\left\{d_1^o,\cdots ,d_N^o\right\}$ is necessarily positive semidefinite, given that the FIM is a semidefinite positive matrix, the CRLB for the combined measurement case satisfies the inequality

$${CRLB}_H\left(\mathit{ϵ}\right)<error\left(\mathit{ϵ}\right)$$

(50)

indicating that the inclusion of additional AOA measurements lead to a lower bound on the variance of the estimator for $\mathit{ϵ}$, thereby enhancing estimation accuracy compared to using TOA measurements alone.

The influence of ${\Sigma }_{\mathit{d}}$ and ${\Sigma }_{\mathit{u}}$ on the CRLB exhibits distinct characteristics due to their roles in the measurement process. ${\Sigma }_{\mathit{d}}$, as the covariance matrix of range measurements, directly impacts ${\mathrm{CRLB}}_d$, which naturally reflects the uncertainty in the distance measurements. This influence is further complicated because ${\Sigma }_{\mathit{u}}$, the covariance matrix of angular measurements, is indirectly related to ${\Sigma }_{\mathit{d}}$ through a proportional factor $\eta$, representing the dependency between the variances of range and angular noise. Consequently, changes in ${\Sigma }_{\mathit{d}}$ also indirectly affect ${\Sigma }_{\mathit{u}}$, thereby influencing ${\mathrm{CRLB}}_d$. These interdependencies highlight the complex interactions between range and angular measurement uncertainties and their combined effects on the overall CRLB.

5. Numerical Experiments

We conducted simulations to evaluate the performance of the proposed methods (denoted as “SDR-TOA” and “SDR-TOA-AOA”). In these simulations, the SDR-TOA-RAW [20] is used as benchmark. The MATLAB toolbox CVX was employed to address the SDP issues, utilizing SDPT3 as the designated solver. We used the root mean squared error (RMSE) as the performance metric, defined as

$$\begin{array}{cc}\hfill \mathrm{RMSE}\left(\mathit{t}\right)& =\sqrt{{\displaystyle \frac{1}{MC}}{\displaystyle \frac{1}{L}}\sum _i^{MC}\sum _j^L\parallel {\widehat{\mathit{t}}}_{i,j}-\mathit{t}\parallel }\hfill \end{array}$$

(51a)

$$\begin{array}{cc}\hfill \mathrm{RMSE}\left(\mathit{\phi }\right)& =\sqrt{{\displaystyle \frac{1}{MC}}{\displaystyle \frac{1}{L}}\sum _i^{MC}\sum _j^L\parallel {\widehat{\mathit{\phi }}}_{i,j}-\mathit{\phi }\parallel }\hfill \end{array}$$

(51b)

where $\mathit{\phi }$ and $\mathit{t}$ denote the real value of rotation angle and translation vectors, while $\widehat{\mathit{\phi }}$ and $\widehat{\mathit{t}}$ represent the estimate of the true value during the i-th iteration of the Monte Carlo (MC) simulation for the j-th setup, with L and MC representing the number of configurations and MC runs, respectively.

5.1. Simulation Configuration

The TOA and AOA measurements were generated using models (5) and (9), respectively. We set the number of MC runs to 1000, with 4 distinct flight trajectories (configurations) within a 100 m × 100 m × 100 m 3D space. The rotation angle was fixed at $[0.1,0.2,0.3]$ rad. The four trajectories, shown in Figure 2, include parallel paths, intersecting paths, spiral paths with increasing altitude, and circular paths in opposite directions. The selected flight path scenarios were carefully designed to evaluate the practical limitations and performance of the proposed method under varying motion patterns.

Specifically, trajectories s2 and s4 were chosen to compare the differences between parallel and opposing (intersecting) flight paths, highlighting the effect of relative motion direction on localization accuracy. On the other hand, trajectories s1 and s3 were selected to investigate the impact of AOA measurements under scenarios with similar TOA conditions. In particular, s1 focuses on angle measurements concentrated in the XY-plane, while s3 emphasizes angle measurements in the Z-direction. This comparison allowed us to analyze the influence of angular diversity on the relative pose estimation results.

The red line represents the trajectory of UAV 1, while the blue line corresponds to that of UAV 2. The trajectories are designated as s1, s2, s3, and s4. The starting points of UAVs are marked with circles, while the endpoints are denoted by stars.

For each configuration, 50 groups of range and angle measurements were taken, with only the first 25 used for TOA and TOA-AOA comparisons unless otherwise specified. The simulation generated noise-free range and angle measurements, and Gaussian noise was subsequently added to model realistic sensor errors. The TOA noises followed a zero-mean Gaussian distribution with covariance $R_d={\sigma }_d^2\mathbf{I}$, while the AOA noises followed a zero-mean Gaussian distribution with covariance $R_a={\eta }^2{\sigma }_d^2\mathbf{I}$, where $\eta$ denotes the ratio between AOA and TOA noise levels. This setup ensures that the influence of measurement noise and the integration of AOA measurements can be effectively analyzed under constrained conditions.

We assumed a measurement frequency of 5 Hz for both range and angle, meaning that a range and angle measurement was completed every 0.2 s. Additionally, we assumed that the maximum speed of the UAV would not exceed 10 m/s. Pose estimation was performed after collecting the required N number of range and angle measurements.

5.2. Performance Comparison of Measurement Noise Variance

Our proposed algorithm consistently outperforms the SDR-TOA-RAW [20] benchmark across all trajectories, as seen in Figure 3. This superior performance is due to our algorithm’s comprehensive consideration of noise and distance-related models. By integrating AOA measurements alongside TOA, our approach significantly enhances the observability of the system, leading to improved estimation accuracy.

The four trajectories exhibit distinct differences in localization accuracy due to variations in relative distance and measurement stability. As shown in Figure 3g,h, trajectory s4 consistently yields the highest localization error across all noise levels, primarily because of its significant fluctuations in relative position, elevation, and azimuth angles, which reduce measurement stability and amplify errors. In contrast, trajectories s2 and s3 demonstrate better localization performance due to more stable relative distances, which enhance the quality of ranging information and improve overall accuracy, as shown in Figure 3c–f.

Among these, trajectory s2 achieves the best accuracy. By comparing the TOA-only (SDR-TOA and SDR-TOA-RAW) and TOA-AOA results in Figure 3, it is evident that the inclusion of AOA measurements significantly enhances accuracy across all trajectories. For instance, with −20 dB noise (corresponding to a 0.1 m standard deviation in range measurements), the rotational vector estimation error for s2 improves from 0.085 to 0.026, reflecting a $69\%$ improvement, as shown in Figure 3c. Similarly, for s3, the rotational vector error decreases from 0.12 to 0.031, reflecting a $74\%$ improvement, as shown in Figure 3e. The translational vector estimates are relatively stable, with s2 improving from 0.019 m to 0.017 m and s3 from 0.045 m to 0.032 m, as shown in Figure 3d,f, respectively. These findings highlight the advantage of leveraging stable relative positions in trajectories to fully exploit the benefits of AOA.

Each trajectory also displays distinct sensitivity to noise. As shown in Figure 3a,b,g,h, trajectories s1 and s4 exhibit smaller error increases as noise levels rise, indicating greater robustness to noise. In contrast, s2 and s3 show sharper error growth at higher noise levels. This suggests that while s2 and s3 substantially benefit from AOA at lower noise levels, they are more susceptible to error amplification as noise increases. Collectively, these findings underscore the importance of both stable measurement geometry and noise robustness in optimizing UAV relative localization.

5.3. Performance Comparison of TOA and AOA Measurement Noise Ratio

In this subsection, we investigate the effect of increasing the AOA noise ratio $\eta$ on localization accuracy across different trajectories. Our goal is to understand how the addition of AOA measurements impacts the performance under varying noise conditions.

It is evident that the influence of $\eta$ on TOA-AOA performance is highly trajectory-dependent. As illustrated in Figure 4, for s2 and s3, when $\eta$ exceeds 8, the TOA-AOA method performs worse than the TOA-only method. It occurs because the inaccuracies in AOA estimation at high noise levels deteriorate the overall localization accuracy. Furthermore, with regard to rotational vector estimation, Figure 4a reveals that s2 performs worse than s3 at higher $\eta$ values, although s2 still outperforms s3 in translational vector estimation.

In contrast, trajectories s1 and s4 show relatively stable localization performance as $\eta$ increases. This aligns with our findings from the first experiment regarding noise sensitivity, where s1 and s4 exhibited stronger noise robustness, albeit at the cost of lower baseline accuracy. Thus, these results confirm that while trajectories with stable measurement conditions (like s2 and s3) benefit more from AOA at lower noise levels, they are also more sensitive to increases in AOA noise, whereas s1 and s4 demonstrate better resilience to noise fluctuations but with lower overall accuracy.

While the inclusion of AOA measurements generally enhances localization accuracy, as shown in Figure 4, the improvement becomes less significant when AOA noise levels are relatively high. In such cases, the information provided by angle measurements may contribute less to the overall accuracy, making the additional computational effort for processing AOA less effective. In practical applications, this trade-off should be carefully considered. When AOA noise is substantial, it may be worth evaluating whether incorporating AOA measurements is beneficial or if a TOA-only approach could achieve comparable performance with reduced complexity.

5.4. Performance Comparison of Number of Measurements

We investigated the influence of the number of observations on relative positioning accuracy. To conduct this research, we set $\sigma$ to 0.1 m and $\eta$ to 0.3, gradually increasing the number of observations N from 25 to 50. In this process, we used CRLB as an evaluation metric. It can be seen clearly from Figure 5 that this result is consistent with the results of the previous two experiments: s2 corresponds to the best positioning effect, while s4 shows the worst positioning performance. However, it is worth noting that regardless of which flight trajectory, the localization accuracy has shown a significant improvement trend as the number of observations increase. This means that more observations can indeed bring better localization accuracy.

6. Conclusions

This study addresses the 3D relative localization problem for UAVs using a hybrid approach of TOA and AOA measurements. We first established baseline localization performance using TOA-only measurements under varied noise conditions, formulating the problem as a CWLS model to estimate the relative position and orientation of the UAVs. To manage the non-convexity and orthogonality constraints of the rotation matrix, we applied SDR to transform the model into a convex semidefinite program. Expanding our analysis to include AOA measurements, we demonstrated that integrating TOA with AOA significantly enhances localization accuracy, especially for stable flight trajectories. Through CRLB analysis and extensive numerical simulations, we validated that the proposed TOA-AOA approach outperforms TOA-only localization across diverse trajectory configurations, providing a robust solution for relative UAV localization.

The proposed algorithm can be applied to practical scenarios such as UWB-based ranging systems and Bluetooth-based AOA measurement systems. By leveraging both TOA and AOA measurements, our approach highlights the importance of UAV trajectory selection in achieving accurate relative localization. Specifically, optimizing flight trajectories can significantly enhance localization performance, which is particularly valuable for UAV swarm operations in GPS-denied environments. Additionally, the flexibility of our method allows for integration with other sensing modalities, such as IMUs, cameras, or LiDARs, enabling robust multi-sensor fusion solutions for real-world UAV applications.