A Novel Ranging and IMU-Based Method for Relative Positioning of Two-MAV Formation in GNSS-Denied Environments

Abstract

Global Navigation Satellite Systems (GNSS) with weak anti-jamming capability are vulnerable to intentional or unintentional interference, resulting in difficulty providing continuous, reliable, and accurate positioning information in complex environments. Especially in GNSS-denied environments, relying solely on the onboard Inertial Measurement Unit (IMU) of the Micro Aerial Vehicles (MAVs) for positioning is not practical. In this paper, we propose a novel cooperative relative positioning method for MAVs in GNSS-denied scenarios. Specifically, the system model framework is first constructed, and then the Extended Kalman Filter (EKF) algorithm, which is introduced for its ability to handle nonlinear systems, is employed to fuse inter-vehicle ranging and onboard IMU information, achieving joint position estimation of the MAVs. The proposed method mainly addresses the problem of error accumulation in the IMU and exhibits high accuracy and robustness. Additionally, the method is capable of achieving relative positioning without requiring an accurate reference anchor. The system observability conditions are theoretically derived, which means the system positioning accuracy can be guaranteed when the system satisfies the observability conditions. The results further demonstrate the validity of the system observability conditions and investigate the impact of varying ranging errors on the positioning accuracy and stability. The proposed method achieves a positioning accuracy of approximately 0.55 m, which is about 3.89 times higher than that of an existing positioning method.

1. Introduction

Micro Aerial Vehicles (MAVs), which refer to small Unmanned Aerial Vehicles (UAVs), have gained popularity in aerial robotics due to their small size, agility, and versatility. It is possible for MAVs to perform a wide range of tasks, including surveillance, inspection, mapping, and environmental monitoring. However, achieving coordinated missions for MAV formations is a challenging task, as it requires precise and reliable navigation and positioning capabilities. Furthermore, such missions involve complex maneuvers and high synchronization between multiple MAVs, and any errors or inaccuracies in navigation or positioning can result in mission failure or even accidents. Therefore, developing robust and efficient navigation and positioning methods is crucial for enabling MAVs to perform coordinated missions effectively and safely.

Urban or forest environments are renowned for their complexity and heterogeneity, which pose significant challenges to wireless communication systems due to issues such as multipath propagation and shadowing [1]. In this context, Hermosilla et al. [2] proposed the use of street-based urban metrics descriptions to quantify various spatial patterns of urban types constructed at different periods, and provided eight different types of urban environments (including urban, residential, and industrial areas) based on their structural characteristics, such as building height distribution or vegetation coverage. These differences may impact wireless signal propagation in different ways. For example, historic urban areas with lower building heights and narrower streets may experience more severe multipath effects, while emerging industrial areas with taller buildings and wider streets may face more severe signal blockage issues. As a result, there are numerous challenges in receiving Global Navigation Satellite System (GNSS) signals in urban or forest environments due to the weak anti-interference ability of GNSS [3]. Despite some related studies [4,5,6] having reduced the error in GNSS-based positioning systems through multi-information fusion technology, intentional interference may render GNSS unusable in certain specialized fields, such as military applications. Therefore, achieving relative positioning of MAVs in GNSS-denied scenarios has become a potentially fruitful area of research.

The common methods for achieving relative positioning in GNSS-denied environments can be classified into the following three categories: visual positioning [7,8,9,10,11,12,13,14,15,16,17], inertial navigation positioning [18,19,20,21,22], and radio positioning [23,24]. The development status of each category of positioning technology is systematically discussed below, and the advantages and limitations of each technology are analyzed, providing good inspiration for the study of relative positioning methods for MAVs in GNSS-denied environments.

The visual localization methods can be broadly categorized into map-based localization [7,8,9,10,11] and map-free localization [12,13,14,15,16,17], depending on whether prior visual maps are utilized. (Note that visual refers to visual information, which is typically collected using sensors such as cameras or laser scanners. Visual maps can be understood as maps constructed based on visual information and used for visual localization and navigation.) In the map-based localization, a pre-constructed visual map was used to aid in localization, and the steps involved in map construction and updating, image retrieval, feature point extraction and matching, and precise localization have been studied extensively [7,8,9,10]. The main focus of [7] was the construction of 3D maps, while reference [8] addressed map quality issues by removing outliers and tracking lanes. Map matching problems were mainly addressed in [9,10,11]. In contrast, map-free visual localization methods do not rely on prior visual maps and instead estimate the pose of the object and surrounding environment. This category can be further divided into Visual Simultaneous Localization and Mapping (VSLAM) [12,13,14,15] and Structure from Motion (SFM) [16,17]. The VSLAM is designed for real-time processing, making it well-suited for applications such as robotics and autonomous vehicles, while SFM prioritizes accuracy and is more appropriate for offline processing applications such as digital reconstruction of scenes [16]. Specific techniques within these categories have also been put forward. For example, an efficient distributed particle filter (EDPF) was proposed in [12] to address the difficulty of sampling high-dimensional state spaces in range-only SLAM. Reference [13] proposed a weight-optimized particle filter-based algorithm for monocular visual SLAM, which aimed to improve the slow environmental interference repair speed of traditional filtering SLAM algorithms. Three-level parallel optimization was adopted in [14], including the direct method, feature-based method, and pose graph optimization. The method in [15] involves two stages: the first stage implements a local SLAM process based on filtering techniques, while the second stage utilizes optimization-based techniques for constructing and maintaining a consistent global map of the environment, which includes addressing the loop closure problem. To estimate the state of a MAV, the main filtering technique employed is the Extended Kalman Filter (EKF). In terms of optimization techniques, three methods are used: a local bundle adjustment technique to minimize the total reprojection error, the minimization of the Perspective-n-Point (PnP) problem, and graph optimization to correct the global map. In [16], 3D point clouds of close-range images were generated using SFM technology. Homologous features between different images were found to determine the shooting direction and position of each image. Finally, reference [17] used optimization to enhance the convergence rate of SFM by retrieving maximum internal similarity between images. The chosen images and query results were used to reconstruct a three-dimensional scene and estimate relative camera positions with SFM.

The inertial navigation positioning is an autonomous method that provides accurate short-term navigation and positioning without relying on external information or emitting radiation. However, the general positioning system is prone to an error accumulation due to the second-order integration operation [18,19,20,21,22]. Various studies have been conducted to address this issue. For instance, a 3D trajectory planning method based on Particle Swarm Optimization-A star (PSO-A*) algorithm was proposed in [18] to solve the yaw problem of intelligent aircraft. In [19], the nonlinear error model was considered, and internal measurement information was utilized to correct the nonlinear error of inertial navigation. The zero-velocity detection algorithm was adopted to compensate for the accumulated error of pedestrian inertial navigation [20,21]. Additionally, reference [22] employed Convolutional Neural Networks (CNN) to reduce the error of MEMS IMU sensors.

There are various wireless positioning methods based on radio technology, mainly including infrared positioning [23], Ultra Wide Band (UWB) positioning [24], and radio frequency identification (RFID) positioning [25]. In [23], the angle of an object equipped with a positioning device was measured using an infrared laser beam emitted by a lighthouse base station, and the position of the object was calculated. An indoor positioning system based on improved adaptive Kalman filter (IAKF) was proposed in [24] that calculated the distance between the tag and the base stations using the time it took for UWB signals to travel, and then applied a triangulation algorithm to acquire the position of the tag. Similar to [24], reference [25] differs mainly in the method of distance measurement, using RFID technology to calculate the distance. In addition, Ref. [25] mentions that RFID positioning tags can be classified into two types, active and passive, depending on their power requirements. Passive tags mainly involve backscattering communication, and therefore do not require direct power supply, but they have limited communication range compared to active tags. Therefore, RFID technology is mostly used for indoor product tracking. Furthermore, the design, installation, and maintenance of RF navigation systems can be expensive and complex, which may pose challenges for deploying them in certain applications, such as small unmanned aerial vehicles.

In addition, multi-technology fusion is an important means to improve the positioning performance. An important trend in VSLAM is to integrate visual sensor data with other sensor data [26,27]. A method was proposed in [26] to integrate Inertial Measurement Unit (IMU) and dynamic VSLAM, which avoided the static assumption of common SLAM algorithms and solved the problems of fast vehicle motion and insufficient light. This method exhibited higher robustness compared with pure visual dynamic SLAM systems. In [27], a SLAM autonomous positioning algorithm combining magnetometer, IMU, and monocular camera was proposed to address the initialization instability and drift problem in the visual-inertial SLAM (VI-SLAM) algorithm. In wireless positioning, UWB technology is a potentially productive area of research [28,29,30] in multi-information fusion positioning due to its advantages such as strong penetration ability, low power consumption, small impact of multipath effects, and high positioning accuracy [31,32,33]. In [28], UWB ranging was used in an indoor positioning scenario, and the position was jointly estimated by fusing the ranging and IMU information through cooperative positioning, which reduced the position drift of Inertial Navigation System (INS). A relative positioning method based on trilateration was proposed for the multi-mobile user mutual positioning scenario [29], where the UWB ranging and IMU information were fused and integrated into a probabilistic framework for cooperative positioning fault recovery. In [30], the pedestrian navigation was realized based on IMU and UWB ranging, and a zero-velocity detection algorithm and single anchor point reference were used. Due to the inherent error drift of IMU, relying solely on IMU for inertial navigation positioning is uncommon. Typically, the fusion of multi-sensor information is required to reduce the positioning errors [26,27,28,29,30,31,32,33,34,35].

Authors have two observations on the existing research on positioning methods.

• In the visual-based positioning research, the positioning accuracy is weakened in low-light environments and line-of-sight limitations lead to poor system robustness. These factors pose significant challenges for MAVs to efficiently perform collaborative tasks in diverse environments.
• In the research on IMU-based relative positioning, the multi-sensor fusion is mostly employed to reduce the inertial drift. Solely relying on IMU for dead reckoning cannot achieve long-term high-precision positioning [18,19,20,21,22]. When fusing with radio information, there are restrictions on MAV cooperative formation tasks, such as the existence of zero-velocity detection [31,33] and fixed reference anchor [33]. In addition, there is a situation where two MAVs cannot be positioned [32].

Motivated by the above facts, this paper investigates a relative positioning method of a two-MAV cooperative formation fusing inter-vehicle distance and IMU information in GNSS-denied environments, which does not require the use of other auxiliary devices such as odometers, except for the IMU and the devices used for ranging. The main contributions of this paper are summarized as follows:

• A novel method for relative positioning in GNSS-denied scenarios is proposed based on ranging and IMU information. The method utilizes the EKF algorithm to jointly estimate the relative positions of MAVs, providing continuous, precise, and reliable information for the formation without being affected by the error accumulation problem of IMU. Additionally, the method is capable of achieving relative positioning for an arbitrary number of nodes without requiring an accurate reference anchor. This innovative approach offers notable advantages over existing methods and has great potential for applications in various fields of aerial robotics.
• Theoretical derivations of the system observability conditions are presented, along with the specific expressions. The conditions indicate that the system positioning accuracy and reliability can be guaranteed when the flight trajectory of the MAV formation satisfies the observability conditions. Failure to satisfy these conditions may result in decreased positioning accuracy and reliability, as well as a possibility of divergence.
• Monte Carlo simulations were conducted, where the correctness of the system observability conditions was verified and the effects of different ranging errors on the positioning accuracy and reliability were investigated. Moreover, the positioning error was reduced by approximately 3.89 times compared to an existing positioning method [32].

The rest of this paper is organized as follows. Section 2 describes the system model. Section 3 presents a detailed description of the relative positioning method. Section 4 de-rives the system observability conditions and provides the specific expressions. Section 5 presents simulation results, followed by Section 6 which concludes this paper.

2. System Model

In this paper, we consider a system model for relative localization of two cooperative MAVs in GNSS-denied environments, as shown in Figure 1. The definitions of the reference coordinate systems are explained as follows. The global coordinate system is the Earth-fixed North-East-Down (NED) coordinate system, denoted as n, and assumed to be an inertial frame. The origin of the body-fixed horizontal coordinate system (denoted as $h_i$, i = 1, 2) for MAV i (i = 1, 2) is located at its center of gravity, with x-y plane and z-axis paralleling those of n, respectively, meaning that $h_i$ is obtained by rotating the n around its z-axis by a yaw angle $\phi$.

It is worth noting that this paper does not use the typical body-fixed coordinate system (denoted as $b_i$, i = 1, 2) for the MAV i, which is represented by Euler angles with respect to n. According to the 321-rotation sequence, the corresponding Euler angles are yaw angle $\phi$, pitch angle $\theta$, and roll angle $\gamma$, respectively. The reason for using $h_i$ instead of $b_i$ is to simplify the kinematic relationships and minimize the impact of unnecessary factors, such as near-hovering states with small roll and pitch angles.

In the system model, MAV i can measure its own state variables, which include acceleration, angular velocity, and velocity in $h_i$. Furthermore, by means of wireless communication, MAV i can also obtain velocity and distance information from the other one in the system framework, where two MAVs utilize inter-vehicle distance information to enhance the accuracy and robustness of relative positioning in the system framework.

The relative motion of two MAVs is described in h. Let ${\mathit{p}}_i^n$ denote the position vector of MAV i in n, where the subscript i refers to the MAV number and the superscript n indicates the vector is projected onto n. The projection of the relative position of MAV k (k = 1, 2, but $k\ne i$) with respect to MAV i in $h_i$ can be expressed as:

$$p_{ik}^{h_i}=C_n^{h_i}\left(p_k^n-p_i^n\right)$$

(1)

where $C_n^{h_i}$ is the coordinate transformation matrix from n to $h_i$ (see Equation (2)). It is only dependent on the yaw angle ${\phi }_i$ of MAV i, since x-y plane and z-axis of $h_i$ are parallel to those of n, respectively.

$$C_n^{h_i}=\left[\begin{array}{ccc}\cos {\phi }_i& -\sin {\phi }_i& 0\\ \sin {\phi }_i& \cos {\phi }_i& 0\\ 0& 0& 1\end{array}\right]$$

(2)

It can be inferred from Equations (1) and (2) that the z-component of the relative position $p_{ik}^{h_i}$ corresponds to the difference in height (which can be measured by a barometric altimeter) between MAV i and MAV k in n. Therefore, the system model can be simplified from three-dimensional space to a two-dimensional plane in the horizontal direction.

Some of the parameters and symbols used in this section are shown in

Table 1. Descriptions of parameters or symbols in Section 2.

Parameters or Symbols	Descriptions
n	Earth-fixed North-East-Down (NED) coordinate system
h	Body-fixed horizontal coordinate system
b	Body-fixed coordinate system
$\phi$	Yaw angle
$\theta$	Pitch angle
$\gamma$	Roll angle
${‖·‖}_2$	2-norm
$p_i^n\in {ℝ}^3$	Position vector of MAV i in n
$p_{ik}^{h_i}\in {ℝ}^3$	$\mathrm{Projection}\mathrm{of}\mathrm{the}\mathrm{relative}\mathrm{position}\mathrm{of}\mathrm{MAV}k\mathrm{with}\mathrm{respect}\mathrm{to}\mathrm{MAV}i\mathrm{in}{h}_i$
$C_n^{h_i}\in {ℝ}^{3\times 3}$	$\mathrm{Coordinate}\mathrm{transformation}\mathrm{matrix}\mathrm{from}n\mathrm{to}{h}_i$

.

3. A Ranging and IMU-Based Relative Positioning Method

We propose a relative positioning method that integrates the inter-vehicle distance with on-board IMU information and employs the EKF algorithm for optimal position estimation. The acquired data is transformed into the corresponding framework to establish the state differential equation. Based on the measurement information, the state of the MAVs is updated. The system observability analysis is conducted before and after the EKF algorithm’s prediction and update process to ensure the system positioning accuracy. Specifically, in Section 3.1, the state differential equation between two MAVs is simply derived, and the observation model and the overall system process are given in Section 3.2 and Section 3.3, respectively.

3.1. State Differential Model

The nonlinear state vector system for the localization model is defined as:

$$\left\{\begin{array}{l}\dot{\mathit{x}}=f\left(x,u\right)\\ y=h\left(x\right)\end{array}\right.$$

(3)

where the vectors $x\in {ℝ}^n$, $u\in {ℝ}^m$, $y\in {ℝ}^l$ are the state vector, input vector, and output vector of the system, respectively. $f\left(x,u\right)$ is the system state differential vector function containing the vector parameters x and u, and $h\left(x\right)$ is the observation equation related to the state vector x.

According to the discussion in Section 2, and considering the relative positioning model of two MAVs, let $p=R\left({\mathit{p}}_2-{\mathit{p}}_1\right)\in {ℝ}^2$, which represents the projection of the relative position of MAV 2 with respect to MAV 1 in $h_1$, where R is the two-dimensional case of Equation (2). Specifically, it is equal to:

$$R=\left[\begin{array}{cc}\cos \left(\Delta \phi \right)& -\sin \left(\Delta \phi \right)\\ \sin \left(\Delta \phi \right)& \cos \left(\Delta \phi \right)\end{array}\right]$$

(4)

where $\Delta \phi ={\phi }_2-{\phi }_1$ is the yaw angle difference between MAV 1 and MAV 2.

Moreover, the other variables in the nonlinear system are defined below. The yaw rate difference is denoted by $\Delta \dot{\phi }=r_2-r_1$ (where $r_i$ is the yaw rate of MAV i, i = 1, 2). The projections of velocity and acceleration of MAV i in $h_i$ are denoted as $v_i\in {ℝ}^2$ and $a_i\in {ℝ}^2$, respectively.

It should be noted that the yaw rate $r_i$ and acceleration $a_i$ mentioned earlier refer to the horizontal plane components in $h_i$ and cannot be equated simply with the actual measured values ${\omega }_i\in {ℝ}^3$ and $s_i\in {ℝ}^3$ of the gyroscope and accelerometer. In particular, the latter must undergo a conversion process, which can be expressed as:

$$r_i=\frac{\sin {\gamma }_i}{\cos {\theta }_i}\cdot {\omega }_{iy}+\frac{\cos {\gamma }_i}{\cos {\theta }_i}\cdot {\omega }_{iz},i=1,2$$

(5)

$$a_i=\left[\begin{array}{ccc}\cos {\theta }_i& \sin {\gamma }_i\sin {\theta }_i& \cos {\gamma }_i\sin {\theta }_i\\ 0& \cos {\gamma }_i& -\sin {\gamma }_i\end{array}\right]\cdot s_i,i=1,2$$

(6)

where ${\omega }_{iy}$ and ${\omega }_{iz}$ represent the y-axis component (true pitch rate) and z-axis component (true yaw rate) of ${\omega }_i$, respectively. ${\gamma }_i$ and ${\theta }_i$ are the roll angle and pitch angle of MAV i, respectively. Proofs of Equations (5) and (6) are provided in Appendix A for reference.

Let the state and input vector be $x={\left[p^T,\Delta \phi ,v_1^T,v_2^T\right]}^T\in {ℝ}^7$ and $u={\left[r_1,r_2,a_1^T,a_2^T\right]}^T\in {ℝ}^6$, respectively. The state differential equation of the system is simply derived as follows.

The derivative of the relative position p with respect to time t is (Note that in Equation (7), the coordinate transformation matrix R does not act on $v_1$. The reason is that, in the scenario of mutual positioning between two MAVs, when calculating the relative position of MAV 2 with respect to MAV 1 in $h_1$, there is no need to transform the velocity of MAV 1 in its own coordinate system $h_1$ by left-multiplying R):

$$\begin{array}{ll}\frac{dp}{dt}& =\frac{dR\left(p_2-p_1\right)}{dt}\\ & =\frac{dR}{dt}\left(p_2-p_1\right)+R\frac{d\left(p_2-p_1\right)}{dt}\\ & =-r_i^{\times }R\left(p_2-p_1\right)+R{v}_2-v_1\\ & =-r_1^{\times }p+R{v}_2-v_1\end{array}$$

(7)

where the antisymmetric matrix $r_i^{\times }$ of cross product in two-dimensional case is equal to:

$$r_i^{\times }=\left[\begin{array}{cc}0& -r_i\\ r_i& 0\end{array}\right],i=1,2$$

(8)

$v_i$ is obtained by coordinate transformation of $v_i^n\in {ℝ}^2$ in n, which is equal to $v_i=R{v}_i^n$. Taking the derivative of the velocity $v_i$ with respect to time t:

$$\begin{array}{cc}\hfill \frac{d{v}_i}{dt}& =\frac{dR{v}_i^n}{dt}\hfill \\ & =\frac{dR}{dt}{v}_i^n+R\frac{d{v}_i^n}{dt}\hfill \\ & =-r_i^{\times }R{v}_i^n+a_i\hfill \\ & =-r_i^{\times }{v}_i+a_i\hfill \end{array}$$

(9)

According to Equations (7) and (9), and the definition of the yaw rate difference $\Delta \dot{\phi }$, the state differential equation in Equation (3) can be written as:

$$\dot{x}=f\left(x,u\right)=\left[\begin{array}{c}-r_1^{\times }p+R{v}_2-v_1\\ r_2-r_1\\ -r_1^{\times }{v}_1+a_1\\ -r_2^{\times }{v}_2+a_2\end{array}\right]\in {ℝ}^7$$

(10)

3.2. Observation Model

The observations involved in the positioning model are the distance information ${‖\mathit{p}‖}_2$ between two MAVs and the velocity $v_i$ of MAV i (where i = 1, 2). Converting the distance information to ${‖\mathit{p}‖}_2^2/2$ can simplify the system observability theoretical analysis in Section 4 without affecting the analysis results [36]. However, the distance observation is still selected as ${‖\mathit{p}‖}_2$ in the experimental verification. The corresponding observation model is expressed as:

$$y=h\left(x\right)=\left[\begin{array}{c}{‖p‖}_2^2/2\\ v_1\\ v_2\end{array}\right]\in {ℝ}^5$$

(11)

3.3. Overall System Process

The system process as a whole is depicted in Figure 2. In the system framework illustrated in Figure 1, the MAVs acquire their motion states through onboard sensors, encompassing acceleration, angular velocity, and velocity, which are transformed into $h_1$ via transformation Blocks T1, T2, and T3, respectively.

Subsequently, a preliminary observability analysis is conducted to determine whether the observability conditions presented in Equations (30)–(32) of Section 4 are satisfied. Provided that the conditions are met, the positioning accuracy is adequately guaranteed. Otherwise, the system fails to localize and awaits the next sampling interval to retry localization.

One important aspect of the system process is that the state differential equation is constructed based on the MAVs’ states. In addition, the real-time distance measurement between two MAVs and the target MAV’s velocity are obtained through radio communication, with the measurement equation constructed accordingly. The EKF algorithm is then used to estimate the optimal relative position of the target MAV.

A complete system observability analysis is performed based on the optimal position estimation and the MAVs’ states, as shown in Equation (29). The result is expected to be similar to that of the preliminary observability analysis, with the only difference being the expression of the conditions.

Some of the parameters and symbols used in this section are shown in

Table 2. Descriptions of parameters or symbols in Section 3.

Parameters or Symbols	Descriptions
$\mathit{x}\in {ℝ}^7$	State vector
$\mathit{u}\in {ℝ}^6$	Input vector
$\mathit{y}\in {ℝ}^5$	Output vector
$\dot{\mathit{x}}=f\left(\cdot \right)$	State differential vector function
$h\left(\cdot \right)$	Observation function
$p\in {ℝ}^2$	$\mathrm{Projection}\mathrm{of}\mathrm{the}\mathrm{relative}\mathrm{position}\mathrm{of}\mathrm{MAV}2\mathrm{with}\mathrm{respect}\mathrm{to}\mathrm{MAV}1\mathrm{in}{h}_1$
$R\in {ℝ}^{2\times 2}$	Two-dimensional case of $C_n^{h_i}$
$\Delta \phi$	Yaw angle difference between MAV 1 and MAV 2
$\Delta \dot{\phi }$	Yaw rate difference between MAV 1 and MAV 2
$r_i$	Yaw rate of MAV i
$v_i\in {ℝ}^2$	$\mathrm{Projection}\mathrm{of}\mathrm{velocity}\mathrm{of}\mathrm{MAV}i\mathrm{in}{h}_i$
$a_i\in {ℝ}^2$	$\mathrm{Projection}\mathrm{of}\mathrm{acceleration}\mathrm{in}{h}_i$
${\omega }_i\in {ℝ}^3$	Gyroscope measurement
$s_i\in {ℝ}^3$	Accelerometer measurement
$r_i^{\times }\in {ℝ}^{2\times 2}$	Antisymmetric matrix of cross product
$v_i^n\in {ℝ}^2$	Velocity of MAV i in n

.

4. System Observability Analysis

Considering the nonlinear state vector system shown in Equation (3), the system observability is analyzed by means of Lie derivative. The multiple Lie derivatives are defined as follows:

$$L_f^{0}h=h$$

(12)

$$L_f^{i}h=L_f\left(L_f^{i-1}h\right)=J\left(L_f^{i-1}h\right)\cdot f,i\in {\mathbb{N}}^{\ast }$$

(13)

where $J\left(L_f^{i}h\right)$ represents the Jacobi matrix of $L_f^{i}h$. Furthermore, the observability matrix of the nonlinear system is:

$$H=\left[\begin{array}{c}J\left(L_f^{0}h\right)\\ J\left(L_f^{1}h\right)\\ ⋮\\ J\left(L_f^{i}h\right)\end{array}\right],i\in {\mathbb{N}}^{\ast }$$

(14)

When the observable matrix H has full rank, the nonlinear system is considered locally weakly observable [37].

The first term of the observability matrix H is equal to:

$$J\left(L_f^{0}h\right)=J\left(h\right)=\left[\begin{array}{cccc}{p}^T& 0& 0_{1\times 2}& 0_{1\times 2}\\ 0_{2\times 2}& 0_{2\times 1}& E_2& 0_{2\times 2}\\ 0_{2\times 2}& 0_{2\times 1}& 0_{2\times 2}& E_2\end{array}\right]=\left[\begin{array}{ccc}{p}^T& 0& 0_{1\times 4}\\ 0_{4\times 2}& 0_{4\times 1}& E_4\end{array}\right]$$

(15)

Since the cross term of the last four rows and columns in the first term of the observability matrix H is the unit matrix, the rank of H cannot be increased by its corresponding observation functions. Therefore, only the distance observation corresponding to the first row in Equation (11), denoted as $h_1\left(x\right)={‖p‖}_2^2/2$, needs to be considered.

The first-order Lie derivative corresponding to the distance observation $h_1\left(x\right)$ is equal to:

$$L_f^1{h}_1=J\left(L_f^0{h}_1\right)\cdot f=p^T\left(-r_1^{\times }p+R{v}_2-v_1\right)$$

(16)

Calculating the Jacobi matrix of the first-order Lie derivative, the second term of the observability matrix H can be obtained:

$$J\left(L_f^1{h}_1\right)={\left[\begin{array}{c}\left(R{v}_2-v_1\right)\\ p^T\frac{\partial R}{\partial \Delta \phi }{v}_2\\ -p\\ R^{T}p\end{array}\right]}^T$$

(17)

Given that the full rank of the observability matrix H is seven and the first term $J\left(L_f^{0}h\right)$ has rank five, calculation of the second-order Lie derivative is necessary. The second-order Lie derivative is given by:

$$\begin{array}{ll}{L}_f^2{h}_1& =J\left(L_f^1{h}_1\right)\cdot f\\ & ={\left(R{v}_2-v_1\right)}^T\left(-r_1^{\times }\mathit{p}+R{v}_2-v_1\right)+p^T\frac{\partial R}{\partial \Delta \phi }{v}_2\left(r_2-r_1\right)\\ & -p^T\left(-{\mathit{r}}_1^{\times }{v}_1+a_1\right)+p^{T}R\left(-{\mathit{r}}_2^{\times }{v}_2+a_2\right)\\ & =v_2^T{v}_2-2v_1^{T}R{v}_2+v_1^T{v}_1+p^T\frac{\partial R}{\partial \Delta \phi }{v}_2{r}_2-p^T{a}_1+p^{T}R\left(-{\mathit{r}}_2^{\times }{v}_2+a_2\right)\end{array}$$

(18)

where the simplification is achieved using Equations (19) and (20). The result shows that the yaw rate $r_1$ of MAV 1 is completely cancelled out.

$$R^{T}R=E_2$$

(19)

$$R^T{r}_1^{\times }=-\frac{\partial R^T}{\partial \Delta \phi }{r}_1$$

(20)

The third term of the observability matrix H is the Jacobi matrix of the second-order Lie derivative (see Equation (18)), which can be expressed as:

$$J\left(L_f^2{h}_1\right)=\left[\begin{array}{cccc}\frac{\partial L_f^2{h}_1}{\partial p}& \frac{\partial L_f^2{h}_1}{\partial \Delta \phi }& \frac{\partial L_f^2{h}_1}{\partial v_1}& \frac{\partial L_f^2{h}_1}{\partial v_2}\end{array}\right]$$

(21)

where the specific expressions of each term are derived simply as follows:

$$\begin{array}{ll}\frac{\partial L_f^2{h}_1}{\partial \mathit{p}}& =v_2^T\left(\frac{\partial R^T}{\partial \Delta \phi }{r}_2-r_2^{\times T}{R}^T\right)-a_1^T+a_2^T{R}^T\\ & =-a_1^T+a_2^T{R}^T\end{array}$$

(22)

$$\begin{array}{ll}\frac{\partial L_f^2{h}_1}{\partial \Delta \phi }& =-2v_2^T\frac{\partial R^T}{\partial \Delta \phi }{v}_1+v_2^T\left(\frac{{\partial }^2{R}^T}{\partial \Delta {\phi }^2}{r}_2-r_2^{\times T}\frac{\partial R^T}{\partial \Delta \phi }\right)p+a_2^T\frac{\partial R^T}{\partial \Delta \phi }p\\ & =-2v_2^T\frac{\partial R^T}{\partial \Delta \phi }{v}_1+a_2^T\frac{\partial R^T}{\partial \Delta \phi }p\end{array}$$

(23)

$$\frac{\partial L_f^2{h}_1}{\partial v_1}=-2v_2^T{R}^T+2v_1^T$$

(24)

$$\begin{array}{ll}\frac{\partial L_f^2{h}_1}{\partial v_2}& =2v_2^T-2v_1^{T}R+p^T\left(\frac{\partial R}{\partial \Delta \phi }{r}_2-R{r}_2^{\times }\right)\\ & =2v_2^T-2v_1^{T}R\end{array}$$

(25)

It can be seen from the above simplified results that the yaw rate $r_2$ of MAV 2 is completely offset. In the event that the combination matrix A consisting of the Jacobi matrixes of the zero-order, first-order, and second-order Lie derivatives corresponding to the distance observation $h_1\left(x\right)$ has full rank, the rank of observability matrix H is guaranteed to have full rank, indicating that the nonlinear system is observable.

$$A=\left[\begin{array}{cc}{p}^T& 0\\ v_2^T{R}^T-v_1^T& p^T\frac{\partial R}{\partial \Delta \phi }{v}_2\\ -a_1^T+a_2^T{R}^T& -2v_1^T\frac{\partial R}{\partial \Delta \phi }{v}_2+p^T\frac{\partial R}{\partial \Delta \phi }{a}_2\end{array}\right]\in {ℝ}^{3\times 3}$$

(26)

In light of the above discussion, the observable system needs to satisfy the following condition:

$$\left|A\right|\ne 0$$

(27)

Multiplying each element in the third column of determinant $\left|A\right|$ with the corresponding algebraic cofactor, adding and expanding to calculate, we can obtain the following expression:

$$\begin{array}{ll}\left|A\right|& =-p^T\frac{\partial R}{\partial \Delta \phi }{v}_2\cdot \left(-a_1^T+a_2^T{R}^T\right)R_{\Delta \phi =\pi /2}p\\ & +\left(-2v_1^T\frac{\partial R}{\partial \Delta \phi }{v}_2+p^T\frac{\partial R}{\partial \Delta \phi }{a}_2\right)\cdot \left(v_2^T{R}^T-v_1^T\right)R_{\Delta \phi =\pi /2}p\\ & =\left(p^T\frac{\partial R}{\partial \Delta \phi }\left(v_2{a}_1^T-a_2{v}_1^T\right)-2v_1^T\frac{\partial R}{\partial \Delta \phi }\left(v_2{v}_2^T{R}^T-v_2{v}_1^T\right)\right)R_{\Delta \phi =\pi /2}p\end{array}$$

(28)

Considering the properties of matrix $R_{\Delta \phi =\pi /2}$, when $p\ne 0$ and Equation (28) satisfies the following inequality (where m is an arbitrary constant), Equation (27) holds, and the nonlinear system is observable.

$$p^T\frac{\partial R}{\partial \Delta \phi }\left(v_2{a}_1^T-a_2{v}_1^T\right)-2v_1^T\frac{\partial R}{\partial \Delta \phi }\left(v_2{v}_2^T{R}^T-v_2{v}_1^T\right)\ne m{p}^T$$

(29)

While the inequality (29) is not intuitive in revealing the motion constraints of MAVs, it can be used to extract some more obvious conditions (as seen in Equations (30)–(32)), which greatly aid our comprehension of Equation (29).

$$p\ne 0$$

(30)

$$v_i\ne 0or{a}_i\ne 0,i=1,2$$

(31)

$$v_1\ne nR{v}_{2}or\left(a_1\ne 0or{a}_2\ne 0\right)$$

(32)

Equation (30) serves as a prerequisite for Equation (29), which means that the relative position between two MAVs cannot be equal to zero. Equation (31) indicates that the MAVs cannot remain stationary. Both the conditions are obvious. Equation (32) shows that two MAVs cannot fly in parallel unless at least one of the MAVs has a non-zero acceleration, where n is an arbitrary constant.

According to the system observability analysis in this section, we derived the motion conditions that must be satisfied by MAVs (see Equations (29)–(32)), which means if the relative motion between two MAVs in the system violates the observability conditions, the positioning accuracy and reliability of MAVs cannot be guaranteed. In such a case, by actively intervening in the relative motion of MAVs to satisfy the observability conditions, MAVs in the system can still achieve mutual positioning.

Some of the parameters and symbols used in this section are shown in

Table 3. Descriptions of parameters or symbols in Section 4.

Parameters or Symbols	Descriptions
$L_f^{i}h$	The i-th order Lie derivative of h with respect to f
$J\left(\cdot \right)$	Jacobi matrix
H	Observability matrix
$E_i$	The i-th order identity matrix
$\partial$	Partial derivative
$A\in {ℝ}^{3\times 3}$	Combination matrix
$\left|\cdot \right|$	Determinant

.

5. Results

In this section, we employ computer simulations to demonstrate the performance of the proposed relative positioning method. Section 5 is structured as follows: Section 5.1 describes the experimental setup, followed by Section 5.2 which verifies the system observability conditions. In Section 5.3, we investigate the impact of different ranging errors on the positioning accuracy and stability. Finally, an error comparison experiment with an existing method is conducted in Section 5.4.

5.1. Experimental Setup

Assuming two MAVs, A and B, in the system framework depicted in Figure 1, we use the proposed relative positioning method to calculate the position of MAV B relative to MAV A. The specific parameter settings for the simulation experiment are listed in

Table 4. Parameter settings.

Parameters/(Unit)	Values
Filtering step/(s)	0.1
Total simulation duration/(s)	1000
Constant gyroscope drift/(rad/s)	0.05 deg/3600
$\mathrm{Constant}\mathrm{accelerometer}\mathrm{bias}/(\mathrm{m}/{\mathrm{s}}^2$)	0.1 mg
Barometric altimeter error/(m)	1
Ranging error/(m)	2
Velocity error/(m/s)	0.1
Noise matrix	$diag\left(0,0,{10}^{-8},{10}^{-6},{10}^{-6},{10}^{-6},{10}^{-6}\right)$
Initial error	${\left[2,2,2\mathrm{deg},0.1,0.1,0.1,0.1\right]}^T$

, where $\mathrm{deg}=\pi /180$ and $mg=9.8\times {10}^{-3}$.

Explanations for certain simulation parameters in Table 4 are provided below. The gyroscope and accelerometer parameters are typical electrical parameters of commercial IMU inertial sensors (such as MPU6050) commonly available on the market [38]. The error of the barometric altimeter was referenced from [39], where an ultrahigh resolution pressure sensor based on percolative metal nanoparticle arrays was designed. The sensor has an ultra-high resolution of 0.5 Pa and high sensitivity of $0.13{\mathrm{kPa}}^{-1}$, and the pressure range and sensitivity can be adjusted by changing the thickness of the PET membrane, which extends the working pressure range to 40 kPa. In actual altitude tests, the altitude measurement sensitivity was calculated as $-0.00025{\mathrm{m}}^{-1}$ according to the slope of the response curve. RMS noise analysis shows that the accuracy of the sensor can be as low as 1 m. As a high-precision barometric sensor, the sensor can be used for high-resolution barometric altimeters (Note that it is possible to have different pressure at the same height but at different horizontal locations. However, in this work, we mainly focus on the relative positioning of two MAVs, where two MAVs are generally not distant from each other. Thus, it is reasonable to assume that the pressure difference of two MAVs only comes from the altitude level) [39]. The ranging error in this paper is referenced from [40], which proposed an improved through-the-wall (TTW) NLOS ranging method using UWB technology to achieve ranging errors of 0–2 m in NLOS environments. In addition, the noise used in the simulation is close to actual noise, and an adaptive adjustment method is employed for the noise matrix of the system to better adapt to the changes in noise characteristics and maintain good filtering performance.

In accordance with the system observability analysis presented in Section 4, the flight trajectory of the two MAVs needs to satisfy Equation (29) to guarantee the system positioning accuracy and reliability. A trajectory satisfying the requirements of Equation (29), called Trajectory 1, can be defined as follows: the straight-line AB rotates around the geometric center O in the horizontal plane, while O undergoes sinusoidal motion. The MAV height is measured using a barometric altimeter. The motion parameters are listed in

Table 5. Motion parameter settings.

Parameters/(Unit)	Values
Rotational angular velocity of AB/(rad/s)	0.03
Sinusoidal angular velocity of O/(rad/s)	0.03
Linear velocity of O/(m/s)	20
Initial angle of MAV A/(rad)	$\pi /3$
Initial angle of MAV B/(rad)	$\pi$

. Setting the rotational angular velocity of AB and sinusoidal angular velocity of O to zero in Table 5, results in a straight-line flight of the MAVs in parallel, which is denoted as Trajectory 2.

5.2. Verification of the System Observability Conditions

Section 5.2 aims to validate the system observability conditions derived in Section 4 and evaluate the effectiveness of the proposed relative positioning method.

In Section 5.1 of the experimental setup, two trajectories for the MAVs are defined: Trajectory 1 and Trajectory 2. Trajectory 1 satisfies the system observability condition shown in Equation (29), while Trajectory 2 fails to comply with it. More specifically, Trajectory 2 violates the observability sub-condition shown in Equation (32).

Figure 3 shows the filtered error results for the two trajectories, while the corresponding relative motion trajectories are presented in Figure 4. The average filtering errors for x and y axes of Trajectory 1 are approximately 1.22 m and 0.57 m, respectively, with the errors gradually converging to zero over time. In contrast, Trajectory 2 has significantly larger average filtering errors of 6.23 m and 3.86 m for x and y axes, respectively, exceeding those of Trajectory 1 by factors of 5.1 and 6.77. Furthermore, the errors of Trajectory 2 tend to diverge over time. These results indicate that system observability conditions are critical for ensuring the higher positioning accuracy and reliability, whereas failing to meet it, the system accuracy and reliability cannot be guaranteed.

For the purpose of avoiding experimental randomness, Monte Carlo simulation experiments were conducted to investigate the error statistical characteristics of Trajectory 1 and Trajectory 2. Figure 5 presents the results of 100 Monte Carlo simulation experiments for error statistics. The upper and lower dashed lines of the error curves represent the +3σ and −3σ boundaries, respectively, while the solid lines represent the error mean. The boundary values represent the positioning accuracy, and the boundary range represents the positioning stability. The specific data for Figure 5 is listed in

Table 6. Mean statistical data from 100 Monte Carlo simulation tests.

	Parameters	$+3\mathit{\sigma }\mathbf{Boundary}$	Mean	$-3\mathit{\sigma }\mathbf{Boundary}$
Trajectory 1	x	1.62 m	−0.04 m	−1.69 m
	y	2.07 m	0.02 m	−2.03 m
Trajectory 2	x	8.47 m	2.44 m	−2.59 m
	y	4.19 m	1.39 m	−3.41 m
$\frac{\mathrm{Trajectory}2}{\mathrm{Trajectory}1}$	x	5.23	/	1.53
	y	2.02	/	1.18

. It can be concluded from Figure 5 and Table 6 that the positioning accuracy and stability of Trajectory 2 are worse than those of Trajectory 1, and the positioning error of Trajectory 2 exhibits a diverging trend.

The simulation experiments in Section 5.2 demonstrate that the system has smaller positioning errors when it satisfies the observability conditions, versus larger errors with a divergent trend when it fails to meet the conditions. The result further confirms the validity of the system observability theoretical analysis in Section 4.

5.3. The Influence of Different Ranging Errors on the Positioning Accuracy and Stability

In this section, we investigate the impact of different ranging errors on the positioning accuracy and stability of the proposed relative localization method, which integrates ranging and IMU information. The ranging errors for Trajectory 1 are set to four levels: 1 m, 2 m, 4 m, and 8 m. The results of a single experiment’s positioning errors are shown in Figure 6. Since Trajectory 1 satisfies the system observability conditions, the corresponding positioning accuracy is within 2 m after approximately 100 s, despite the different ranging errors, and the system remains stable.

Subsequently, 100 Monte Carlo simulations are conducted, and the simulation results are presented in Figure 7. It can be visually observed that the larger the ranging error, the worse the system’s positioning accuracy and stability. However, increasing ranging errors did not cause the positioning accuracy and stability of the proposed system to deteriorate significantly, meaning the system did not diverge, but only increased the error. Specifically, the average boundary value increased from about 2 m to around 3 m as ranging errors increased from 1 m to 8 m. These experimental results indicate that different ranging errors have a limited impact on the system’s positioning accuracy and stability.

5.4. Error Comparison with an Existing Positioning Method

In Section 5.4, we compare the performance of the proposed method with an existing method introduced in [32].

To demonstrate the rationality of the parameter settings in the comparative experiment, we offer the following explanations: The UWB ranging error in [32] was set to 2 m, and the IMU used was the 9-DOF MPU-9150, with assumed displacement errors of 0.2 m and orientation errors of 0.1 rad. The rationality of the parameters has been previously confirmed in [32]. Therefore, we adopt the ranging error of 2 m from [32]. In addition, the IMU data used in this paper is from MPU-6050, whose data parameters are essentially the same as those of MPU-9150. Based on these considerations, we conclude that our parameter settings are appropriate for the purposes of our simulation.

The experimental results of the two methods are shown in Figure 8. It can be observed that the average positioning error of the proposed method in this paper is approximately 0.55 m, while that of the method in [32] is about 2.14 m, which is approximately 3.89 times the error of the proposed method. In contrast, the relative positioning method proposed in this paper, which fuses ranging and IMU information, achieves better positioning accuracy.

At the initial startup phase of the positioning system, EKF may have uncertainty about the initial state, that is, the initial estimate of the system state may not be accurate enough, which may lead to large errors. As time goes on, since the system has started to operate, through measurement updates and filtering, the state estimate of the system will gradually become more accurate, and the error will gradually decrease until it stabilizes.

6. Conclusions

In this paper, we have presented a novel cooperative relative positioning method for MAVs in GNSS-denied scenarios. Within the framework of the system model, we have employed the EKF algorithm to fuse ranging and IMU information, addressing the problem of IMU error drift and enabling high-precision and robust MAV positioning. In addition, our proposed method has the following several distinctive features: it eliminates the requirement for precise reference anchors, expands the application scope of the system, and solves the problem of limited number of nodes in MAV formation. Furthermore, we have theoretically derived the system observability conditions and provided specific expressions that guarantee the system positioning accuracy when the system satisfies the observability conditions. We have verified the correctness of the observability theoretical analysis through simulations and investigated the influence of different ranging errors on the positioning accuracy and stability. Finally, we have demonstrated the superiority of the proposed method through simulation comparisons with an existing method.

We have verified the proposed method through simulation experiments. To ensure that the experiments are as close to reality as possible, the system parameters considered in this paper are taken from the official datasheet of the sensor and previous research [32,38,39,40]. However, there is still a certain gap between simulation and reality, mainly in the following aspects:

• The presence of obstacles, wind conditions, and other environmental factors may affect the performance of MAVs in practical applications.
• Simulated noise cannot fully reflect the noise experienced by MAVs in actual applications.
• There may be a certain difference in air pressure at the same altitude but different horizontal positions due to the influence of factors such as atmospheric pressure, temperature, and humidity, which may result in some spatial variations. In addition, the scale of pressure gradient varies across different regions. As a result, the barometric altimeter readings may be affected and may introduce some measurement bias.
• In practical applications, the airflow generated by the movement of MAVs may have an impact on the flight stability and control of MAVs.

These factors are difficult to model accurately in simulation experiments. Therefore, actual verification will direct our research in the future.