A Single-Anchor Cooperative Positioning Method Based on Optimized Inertial Measurement for UAVs

Abstract

Benefiting from its structural simplicity and low cost, the inertial/ranging integrated navigation system is widely utilized in multi-agent applications, particularly in unmanned aerial vehicles (UAVs). As the deployment of UAVs in complex environments becomes more prevalent, accurate positioning in sparse observation scenarios has become increasingly important. In satellite-denied environments with few anchors, traditional filtering methods for positioning suffer from poor effectiveness due to the lack of constraints. This article proposes a method to enhance positioning accuracy in such environments by optimizing the inertial outputs of each UAV. The optimization process is based on the range measurements between the UAVs and a single anchor. By solving the optimization function derived using Bayesian theory, the optimized inertial outputs of the UAVs can be obtained. These optimized inertial data are then used in place of the original measurements for position estimation in the filter, resulting in improved performance. Simulation and real-world experiments validate that the proposed method can enhance UAVs’ positioning accuracy in single-anchor environments, surpassing the performance of a single optimizer or filter. Furthermore, the positions estimated by cooperative agents demonstrate higher accuracy than those estimated by individual agents, as more ranging measurements are incorporated.

1. Introduction

In recent years, multi-agent systems have found increasing applications in various fields such as military, agriculture, and commerce, especially for multi-UAV systems. One critical requirement in these systems is accurate positioning. The ranging-based positioning method has emerged as a common solution, thanks to the availability of cost-effective and high-efficiency range sensors. Among the range sensors commonly used in such systems, ultra-wideband (UWB) modules [1] have gained significant popularity. These modules have been extensively utilized in UAVs due to their high-ranging accuracy, anti-multipath capabilities, and low power consumption [2].

The agents equipped with UWB modules can receive range measurements from fixed anchors which have predetermined locations. These range measurements from the anchors play a crucial role in providing geometric constraints and approximate position estimations for the agents. This methodology is akin to the three-ball intersection positioning technique used for satellites. It proves particularly useful in environments where a Global Navigation Satellite System (GNSS) is unavailable or unreliable, such as indoor spaces like rooms [3], underground coal mines [4], underwater environments [5,6], and other similar settings. In many research studies on ranging-based positioning systems, the principles of graph optimization [7,8,9] or Kalman filtering [10,11] are commonly employed to track the position of the agent. In some cases, a sliding window approach [7] is also incorporated to enhance trajectory smoothness. These techniques enable accurate and reliable position tracking in multi-agent systems utilizing range measurements. However, relying on a single ranging source is typically inadequate for achieving precise positioning due to the inherent limitations of ranging, such as susceptibility to sudden changes or interruptions, as well as the low frequency of measurements.

Fusing the inertial sensor and the range sensor is a prevalent approach to enhancing positioning accuracy. The high-frequency inertial measurements include angular velocities and specific forces. The inertial calculation process can transform the measurements in the body frame to the position in the navigation frame. Due to the accumulation of inertial errors, the positioning accuracy continuously decreases over time, and the range measurements can provide constraints and eliminate the influence of errors. In contrast to utilizing range measurements alone, the integration of inertial sensors enables more precise estimation of the motion dynamics model of the system [12]. By incorporating inertial calculations, the integration process establishes a correlation between the motion states of both preceding and succeeding moments in time, and it allows for a more comprehensive prediction of the navigation state [13].

The positioning method that integrates UWB and Inertial Measurement Unit (IMU) shares significant similarities with simple UWB positioning techniques, primarily involving optimization and filtering methodologies. The optimization methods primarily rely on the graph optimization model [14]. The sliding window technique [15] is often employed to estimate relative positions at selected key points by combining distance measurements with acceleration estimates. The filtering methods are mainly based on the Kalman filter [16], which linearizes the nonlinear ranging to predict and update the navigation state of the agent. The current research on nonlinear filtering integrating UWB and IMU mainly focuses on Extended Kalman Filter (EKF) [12,17], Unscented Kalman Filter (UKF) [18,19], and Invariant Extended Kalman Filter (IEKF) [20]. These combined approach leverages the advantages of both range and inertial measurements to enhance the accuracy and robustness of the positioning system.

In a multi-agent system, agents equipped with IMUs can receive range measurements from not only the anchors but also other agents. This is a common pattern of cooperative positioning. The additional range measurements can enhance the positioning accuracy of cooperative agents [21,22,23], compared to the single-agent positioning method.

Whether it is single-agent positioning or multi-agent cooperative positioning, the majority of inertial-ranging positioning methods are built upon multi-anchor networks. These methods rely on the presence of open space and stable communication channels to facilitate ranging [24]. In practical application environments, there may not be enough anchors or the communication is sometimes restricted, and it is not always possible to receive enough signals from multiple anchors at the same time. Therefore, it is important to explore improved positioning methods in sparse anchor environments, where single anchor positioning [25] is the most fundamental case.

The traditional single-anchor positioning methods build virtual anchors for positioning [26,27]. Some methods also involve additional measurements such as velocities [28] and angles [29]. Recently, a geometry-based single-anchor positioning method introduced in [30] estimated the position only by inertial and range measurements. Another research proposed in [31] improved the accuracy of velocity estimation without the same sensors. Our group has also presented an optimization-based positioning method for single-anchor positioning [32]. Different from the other methods, we aim at the cooperation of the agents. The proposed method involves the range measurements between agents in the process of building constraints, which centrally improve the positioning accuracy of the agents, compared to the positioning of individual agents.

The main contributions of this paper include the following:

1.

We propose a cooperative positioning method in single-anchor environments based on range and inertial measurements. At the moments of range observation, an optimizer provides the optimized inertial data to a filter, and the filter outputs the position using the prior navigation state of the agent.

2.

An optimization method for inertial measurements in a multi-agent system is presented. The optimal estimation model is derived from Bayesian theory, and the optimal inertial measurements are obtained at the extreme points, whose existence is further discussed.

3.

The performance of the single-anchor cooperative positioning method is presented in a simulated multi-UAV environment and a real-world indoor environment. The impact of cooperation on improving the positioning accuracy is also discussed.

The rest of this paper is organized as follows. The preliminaries and sensor models are presented in Section 2. Section 3 introduces the process of cooperative positioning method, where we focus on the derivation of the optimization algorithm. Section 4 and Section 5 evaluate the simulation and real-world experimental results, respectively.

2. Preliminaries and Sensor Models

The sensors involved in the multi-agent system consist of inertial sensors and range sensors. Before the optimization, the error distribution of the sensor outputs need to be determined first.

2.1. Inertial Sensor Model

The composition of inertial errors is complex, and there are various approximate models for sensors with different accuracies. According to IEEE standards for inertial devices in [33], the main errors that may exist in inertial sensors include random walk, quantization noise, correlated (Markov) random drift, bias instability, and ramp instability. This classification of errors is detailed based on the characteristics of noise, but there is a significant difference in the magnitude of these errors. The classification of inertial errors based on static and dynamic components introduced in [34] is widely accepted for high-accuracy sensors, but it is not available for low-accuracy and medium-accuracy sensors.

Referring to the classification methods in [33,34], the classification of inertial errors in this paper is given in Figure 1.

As shown in Figure 1, the inertial errors can be divided into bias, installation error, and random noise.

• The bias is a constant error exhibited by all accelerometers and gyroscopes. The bias comprises the fixed bias and the run-to-run bias: the former is the residual fixed bias remaining after sensor calibration, and the latter is the run-to-run variation of each instrument bias. The fixed bias is constant throughout an IMU operating period, while the run-to-run bias changes each turn-on time.
• The installation error is caused by the deviations between actual axes and ideal axes, which contains the scale factor error and the cross-coupling error. The scale factor error is the departure of the input–output gradient of the instrument from unity following unit conversion by the IMU. The cross-coupling error arises from the misalignment of the sensitive axes of the inertial sensors with respect to the orthogonal axes of the body frame due to manufacturing limitations. Different from the bias, the installation error is independent from run to run, and it can be estimated by calibration in advance.
• The random noise exhibited in the inertial sensors has several sources, such as electromagnetic interference, temperature variation, and mechanical vibration. The bias instability and temperature drift both belong to random noise. The spectrum of accelerometer and gyroscope noise for frequencies below 1 Hz is approximately white, so the standard deviation of the average specific force and angular velocity noise varies in inverse proportion to the square root of the averaging time. Inertial sensor noise is thus usually quoted in terms of the root power spectral density (PSD). The customary units [34] are $\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$ for accelerometer random noise, where

  $$1\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}=9.80665\times {10}^{-6}\mathrm{m}·{\mathrm{s}}^{-1.5},$$

  (1)

  and ${}^{\circ }/\sqrt{h}$ for gyroscope random noise, where

  $$1^{\circ }/\sqrt{h}=2.909\times {10}^{-4}\mathrm{rad}·{\mathrm{s}}^{-0.5},$$

  (2)
  The standard deviations of the random noise samples are obtained by multiplying the corresponding root PSDs by the root of the sampling rate or dividing them by the root of the sampling interval. The white random noise cannot be calibrated and compensated as there is no correlation between past and future values.
  A commonly used method for quantifying random noise is Allan variance [35]. The Allan variance is a method of representing the root means square (RMS) random-drift error as a function of averaging time. By performing a simple operation on the entire length of static data, a characteristic curve is obtained, whose inspection facilitates the determination of different types and magnitude of error terms existing in the inertial sensors.

In our work, the inertial random noise is identified as Gaussian white noise, as the inertial sensors in the multi-agent system have low accuracy or medium accuracy. The standard deviations of angular velocity and specific force are independently and identically distributed in three orthogonal directions of body frame, as the accelerometers and gyroscopes are approximately independent. Let ${\sigma }_g$ denote the standard deviation of angular rate and ${\sigma }_a$ denote the standard deviation of specific force; they can be estimated by Allan variance before experiments.

In conclusion, the output of inertial sensor in our work can be modeled as follows:

The bias is denoted by ${\mathit{b}}_g$ for the gyroscope and ${\mathit{b}}_a$ for the accelerometer, and the random noise is denoted by ${\mathit{w}}_g$ for the gyroscope and ${\mathit{w}}_a$ for the accelerometer. The installation error can be obtained in the calibration and eliminated before the experiments. Therefore, the inertial random errors follow the Gaussian distribution with mean zero, as given by

$$\left\{\begin{array}{c}{\mathit{w}}_g\sim \mathcal{N}\left(\mathbf{0},{\Sigma }_g\right)\hfill \\ {\mathit{w}}_a\sim \mathcal{N}\left(\mathbf{0},{\Sigma }_a\right)\hfill \end{array}\right.,$$

(3)

where ${\Sigma }_g$ and ${\Sigma }_a$ are the diagonal matrix of the variances ${\sigma }_g^2$ and ${\sigma }_a^2$. The errors of IMU data can be modeled using a Gaussian distribution as follows:

$$\left\{\begin{array}{c}\delta {\mathbf{\omega }}_{ib}^b\sim \mathcal{N}\left({\mathit{b}}_g,{\Sigma }_g\right)\hfill \\ \delta {\mathit{f}}^b\sim \mathcal{N}\left({\mathit{b}}_a,{\Sigma }_a\right)\hfill \end{array}\right.,$$

(4)

in which $\delta {\mathbf{\omega }}_{ib}^b$ denotes the angular velocity error and $\delta {\mathit{f}}^b$ denotes the specific force error.

Consequently, the output of a inertial sensor can be expressed by the following:

$$\left\{\begin{array}{c}{\tilde{\mathbf{\omega }}}_{ib}^b={\mathbf{\omega }}_{ib}^b+\delta {\mathbf{\omega }}_{ib}^b={\mathbf{\omega }}_{ib}^b+{\mathit{b}}_g+{\mathit{w}}_g\hfill \\ {\tilde{\mathit{f}}}^b={\mathit{f}}^b+\delta {\mathit{f}}^b={\mathit{f}}^b+{\mathit{b}}_a+{\mathit{w}}_a\hfill \end{array}\right.,$$

(5)

where ${\tilde{\mathbf{\omega }}}_{ib}^b$ and ${\tilde{\mathit{f}}}^b$ are measurements, and ${\mathbf{\omega }}_{ib}^b$ and ${\mathit{f}}^b$ are the unbiased truth-value of angular velocity and specific force.

In the multi-agent system composed of N agents with inertial sensors, the inertial outputs can be represented in the form of sets. Let ${\mathbb{W}}_N$ denote the set of the unbiased angular velocity ${\mathbf{\omega }}_{ib}^b$, ${\mathbb{F}}_N$ denote the set of the unbiased specific force ${\mathit{f}}^b$, ${\tilde{\mathbb{W}}}_N$ denote the set of the measured angular velocity ${\tilde{\mathbf{\omega }}}_{ib}^b$, and ${\tilde{\mathbb{F}}}_N$ denote the set of the measured specific force ${\tilde{\mathit{f}}}^b$, where the subscript N represents the number of the agents. These sets are given by

$${\mathbb{W}}_N=\left({\mathbf{\omega }}_{ib,1}^b,{\mathbf{\omega }}_{ib,2}^b,\dots ,{\mathbf{\omega }}_{ib,N}^b\right),$$

(6)

$${\mathbb{F}}_N=\left({\mathit{f}}_1^b,{\mathit{f}}_2^b,\dots ,{\mathit{f}}_N^b\right),$$

(7)

$${\tilde{\mathbb{W}}}_N=\left({\tilde{\mathbf{\omega }}}_{ib,1}^b,{\tilde{\mathbf{\omega }}}_{ib,2}^b,\dots ,{\tilde{\mathbf{\omega }}}_{ib,N}^b\right),$$

(8)

$${\tilde{\mathbb{F}}}_N=\left({\tilde{\mathit{f}}}_1^b,{\tilde{\mathit{f}}}_2^b,\dots ,{\tilde{\mathit{f}}}_N^b\right),$$

(9)

where the numbers 1 to N in the subscripts indicate which agent the corresponding output comes from.

2.2. Range Sensor Model

Similar to the expression of inertial measurement in (5), the range measurement $\tilde{R}$ is given by

$$\tilde{R}=R+\delta R,$$

(10)

where R denotes the real range and $\delta R$ denotes the ranging error. The ranging error follows the Gaussian distribution as

$$\delta R\sim \mathcal{N}\left(0,{\sigma }_R^2\right),$$

(11)

where ${\sigma }_R$ denotes the ranging standard deviation.

There are two types of ranges in the multi-agent system: one is the range between an agent and a fixed anchor; the other is the range between two different agents. These two types of ranging can be distinguished by subscripts. ${\tilde{R}}_1$ denotes the measured ranging between agent 1 and the single anchor. ${\tilde{R}}_{12}$ denotes the measured ranging between agent 1 and agent 2, which is received and measured by agent 1.

A lot of range sensors including UWB utilize the technology of time difference of arrival (TDOA). According to the principle of TDOA [36], the range measurements of two nodes participating in transmission and reception simultaneously are different, due to the slight difference in round-trip times. This principle indicates that ${\tilde{R}}_{12}$ is different from ${\tilde{R}}_{21}$. So, there are $N^2$ range measurements in total every measurement time, with N range measurements between agents and the single anchor, and $N^2-N$ range measurements between different agents, as given by

$${\tilde{\mathbb{R}}}_N=\left({\tilde{R}}_1,{\tilde{R}}_2,\dots ,{\tilde{R}}_N,{\tilde{R}}_{12},{\tilde{R}}_{21},\dots ,{\tilde{R}}_{N-1,N},{\tilde{R}}_{N,N-1}\right),$$

(12)

where N in the subscripts indicates the number of the agents.

The sensors equipped on each agent can receive the measurements, as the UWB module and MEMS IMU show in Figure 2. Due to the lack of UAVs, we replaced them with unmanned ground vehicles (UGVs) for demonstration. The measurements of angular velocity, specific force, and range update the sets in (8), (9), and (11), which form the inputs for subsequent positioning estimation process.

3. Inertial Optimization Algorithm

3.1. Overview

The method of optimizing inertial data using inertial and range measurements is discussed in this section. The optimization process is shown in Figure 3. Firstly, we find the relationship between the inertial measurement and the range measurement, which helps to fuse the sensor outputs. Secondly, the optimization model is designed based on Bayesian theory, and the optimization equation is derived. Thirdly, we solve the optimization function, and the inertial data at the extreme point are the optimal solution. Fourthly, we discuss some issues on the extreme point to confirm that the calculated stationary point is exactly the point to be solved.

3.2. Range Calculated Using Inertial Output

The fusion of inertial and range sensors is based on the consistent representation of outputs. So, the first step of optimization is using the inertial outputs to represent the ranges, which is based on the inertial calculation process [31]. The inertial calculation process indicates the update of the agent’s attitude, velocity, and position, using the angular velocities and the specific forces output by the inertial sensors.

The inertial sensors in the inertial navigation system (INS) include three orthogonal gyroscopes and three orthogonal accelerometers. In this paper, the coordinate direction in navigation frame n is defined as East–North–Up, while the direction in body frame b is Right–Ahead–Up. The INS calculated the attitude matrix (or direction cosine matrix, DCM) ${\mathit{C}}_b^n$, velocity ${\mathit{v}}^n$, and position ${\mathit{p}}^n$ of the carrier using the angular velocity ${\mathbf{\omega }}_{ib}^b$ and specific force ${\mathit{f}}^b$ output by the inertial sensors. Specifically, ${\mathit{C}}_b^n$ denotes the attitude matrix from body frame to navigation frame, ${\mathit{v}}^n$ and ${\mathit{p}}^n$ are in the navigation frame, ${\mathbf{\omega }}_{ib}^b$ and ${\mathit{f}}^b$ are in the body frame, while $ib$ indicates the rotation direction from the Earth-centered inertial frame i to the body frame. The inertial outputs ${\mathbf{\omega }}_{ib}^b$ and ${\mathit{f}}^b$ here are written as a truth value, but they are actually measurements with errors.

In a update interval, the INS updates the attitude, velocity, and position in sequence. The velocity in the navigation frame is defined as

$${\mathit{v}}^n={\left[v_E,v_N,v_U\right]}^T,$$

(13)

where $v_E$, $v_N$, and $v_U$ denote the eastward, northward, and upward projections, respectively. The position is defined as

$${\mathit{p}}^n={\left[L,\lambda ,h\right]}^T,$$

(14)

where L denotes latitude, $\lambda$ denotes longitude, and h denotes the height above the geoidal surface.

The inertial update process includes calculations related to the rotation of the Earth. Suppose that ${\omega }_{ie}$ is the rotational angular rate of the Earth; then, ${\mathbf{\omega }}_{ie}^n$ is the rotational angular velocity of the Earth frame with respect to the inertial frame and ${\mathbf{\omega }}_{en}^n$ is the rotational angular velocity of the navigation frame with respect to the Earth frame, which can be computed as follows:

$${\mathbf{\omega }}_{ie}^n={\left[0,{\omega }_{ie}cosL,{\omega }_{ie}sinL\right]}^T,$$

(15)

$${\mathbf{\omega }}_{en}^n={\left[-\frac{v_N}{R_M+h},\frac{v_E}{R_N+h},\frac{v_{E}tanL}{R_N+h}\right]}^T,$$

(16)

where $R_M$ and $R_N$ are, respectively, the meridian radius of curvature and the transverse radius of curvature of the WGS-84 reference ellipsoid.

The inertial calculation process follows the sequence of attitude update, velocity update, and position update:

• Attitude Update Process: In the navigation frame, the differential equation [13] of attitude matrix has the following form:

  $${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n\left[{\mathbf{\omega }}_{nb}^b\times \right],$$

  (17)

  where $\left[{\mathbf{\omega }}_{nb}^b\times \right]$ is the antisymmetric matrix of body angular velocity with respect to the navigation frame. The differential equation can be transformed to a function of ${\mathbf{\omega }}_{ib}^b$ as

  $${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n\left[{\mathbf{\omega }}_{ib}^b\times \right]-{\mathit{C}}_b^n\left[{\mathbf{\omega }}_{in}^b\times \right]={\mathit{C}}_b^n\left[{\mathbf{\omega }}_{ib}^b\times \right]-\left[{\mathbf{\omega }}_{in}^n\times \right]{\mathit{C}}_b^n,$$

  (18)

  where ${\mathbf{\omega }}_{in}^n$ is calculated as ${\mathbf{\omega }}_{in}^n={\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n$. Solving the differential equation in (17), the attitude matrix at time $t_k$ can be derived by the attitude matrix at time $t_{k-1}$ as

  $${\mathit{C}}_b^n\left(t_k\right)={\mathit{C}}_b^n\left(t_{k-1}\right)exp{\int }_{t_{k-1}}^{t_k}\left[{\mathbf{\omega }}_{nb}^b\times \right]dt.$$

  (19)
  Considering the small rotation angle in the interval of integration, the attitude matrix in (14) can be approximated by the Taylor expansion as

  $$\begin{array}{c}{\mathit{C}}_b^n\left(t_k\right)\approx {\mathit{C}}_b^n\left(t_{k-1}\right)\left\{\mathit{I}+\left[{\mathbf{\omega }}_{nb}^b\left(t_k\right)\times \right]\right\}\hfill \\ ={\mathit{C}}_b^n\left(t_{k-1}\right)+{\mathit{C}}_b^n\left(t_{k-1}\right)\left[{\mathbf{\omega }}_{ib}^b\left(t_k\right)\times \right]-\left[{\mathbf{\omega }}_{in}^n\left(t_k\right)\times \right]{\mathit{C}}_b^n\left(t_{k-1}\right).\hfill \end{array}$$

  (20)
• Velocity Update Process: In the navigation frame, the specific force is given by

  $${\mathit{f}}^n={\mathit{C}}_b^n{\mathit{f}}^b,$$

  (21)

  and the differential equation [13] of velocity has the following form:

  $${\dot{\mathit{v}}}^n={\mathit{f}}^n-\left(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n\right)\times {\mathit{v}}^n+{\mathit{g}}^n,$$

  (22)

  where ${\mathit{g}}^n$ is the gravity vector, which is considered to be a constant vector when moving in a small area.
  Solving the differential equation in (22), the velocity at time $t_k$ can be derived by the velocity at time $t_{k-1}$ as

  $$\begin{array}{c}{\mathit{v}}^n\left(t_k\right)={\mathit{v}}^n\left(t_{k-1}\right)+{\dot{\mathit{v}}}^n\left(t_{k-1}\right)\Delta t\hfill \\ ={\mathit{v}}^n\left(t_{k-1}\right)+{\mathit{f}}^n\left(t_k\right)\Delta t-\left(2{\mathbf{\omega }}_{ie}^n\left(t_k\right)+{\mathbf{\omega }}_{en}^n\left(t_k\right)\right)\times {\mathit{v}}^n\left(t_{k-1}\right)\Delta t+{\mathit{g}}^n\Delta t,\hfill \end{array}$$

  (23)

  where $\Delta t$ denotes the time duration of the update interval $\left[t_{k-1},t_k\right]$. For low-precision inertial navigation system, the equation can be approximated as

  $${\mathit{v}}^n\left(t_k\right)\approx {\mathit{v}}^n\left(t_{k-1}\right)+{\mathit{f}}^n\left(t_k\right)\Delta t+{\mathit{g}}^n\Delta t.$$

  (24)
• Position Update Process: In the navigation frame, the differential equation [37] of position has the following form:

  $${\dot{\mathit{p}}}^n={\mathit{R}}_c{\mathit{v}}^n,$$

  (25)

  where ${\mathit{R}}_c$ is the local curvature matrix as given by

  $${\mathit{R}}_c=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{1}{\left(R_N+h\right)cosL}& 0& 0\\ 0& 0& 1\end{array}\right],$$

  (26)
  Since L has little change in an update interval and h is much smaller than $R_M$ and $R_N$, ${\mathit{R}}_c$ can be considered to be a constant matrix. Solving the differential equation in (R18), the position at time $t_k$ can be derived by the position at time $t_{k-1}$ as

  $${\mathit{p}}^n\left(t_k\right)={\mathit{p}}^n\left(t_{k-1}\right)+{\mathit{R}}_c{\mathit{v}}^n\left(t_k\right)\Delta t.$$

  (27)
  Substituting (24) into (27) yields

  $$\begin{array}{c}{\mathit{p}}^n\left(t_k\right)={\mathit{p}}^n\left(t_{k-1}\right)+{\mathit{R}}_c{\mathit{v}}^n\left(t_{k-1}\right)\Delta t+{\mathit{R}}_c{\mathit{f}}^n\left(t_k\right)\Delta t^2+{\mathit{R}}_c{\mathit{g}}^n\Delta t^2\hfill \\ =\mathit{C}+{\mathit{R}}_c{\mathit{f}}^n\left(t_k\right)\Delta t^2,\hfill \end{array}$$

  (28)

  where $\mathit{C}$ denotes the constant term as

  $$\mathit{C}={\mathit{p}}^n\left(t_{k-1}\right)+{\mathit{R}}_c{\mathit{v}}^n\left(t_{k-1}\right)\Delta t+{\mathit{R}}_c{\mathit{g}}^n\Delta t^2.$$

  (29)
  Substituting (20) and (21) into (28) yields

  $$\begin{array}{c}{\mathit{p}}^n\left(t_k\right)=\mathit{C}+{\mathit{R}}_c{\mathit{C}}_b^n\left(t_k\right){\mathit{f}}^b\left(t_k\right)\Delta t^2\hfill \\ =\mathit{C}+{\mathit{R}}_c{\mathit{C}}_b^n\left(t_{k-1}\right){\mathit{f}}^b\left(t_k\right)\Delta t^2+{\mathit{R}}_c{\mathit{C}}_b^n\left(t_{k-1}\right)\left[{\mathbf{\omega }}_{ib}^b\left(t_k\right)\times \right]{\mathit{f}}^b\left(t_k\right)\Delta t^3\hfill \\ -{\mathit{R}}_c\left[{\mathbf{\omega }}_{in}^n\left(t_k\right)\times \right]{\mathit{C}}_b^n\left(t_{k-1}\right){\mathit{f}}^b\left(t_k\right)\Delta t^3;\hfill \end{array}$$

  (30)

  then, the updated position ${\mathit{p}}^n\left(t_k\right)$ can be modeled as a function of the angular velocity ${\mathbf{\omega }}_{ib}^b\left(t_k\right)$ and the specific force ${\mathit{f}}^b\left(t_k\right)$.

Suppose that there are node A and node B, which can be fixed anchors or moving agents. The range between node A and node B at time $t_k$ is given by the L2 norm:

$$||{\mathit{p}}_A^n\left(t_k\right)-{\mathit{p}}_B^n\left(t_k\right)||,$$

(31)

where the subscripts A and B indicate the node of A and B. Normally, ${\mathit{p}}_A^n$ and ${\mathit{p}}_B^n$ are indicated by (14) with latitude, longitude, and height. For the range calculated using inertial sensors, ${\mathit{p}}_A^n$ and ${\mathit{p}}_B^n$ can be directly calculated using (30).

3.3. Optimization Model for Inertial Measurements

In this subsection, we introduce the centralized optimization method of multi-agent inertial data. As the outputs of inertial sensors are approximated to the Gaussian distribution in Section 2.2, the estimated inertial data can be expressed in the form of conditional probability, which is given by

$$P({\mathbb{W}}_N,{\mathbb{F}}_N|{\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N),$$

(32)

where ${\mathbb{W}}_N$ and ${\mathbb{F}}_N$ indicate the sets of estimated inertial data.

When the conditional probability in (32) takes the maximum value, the corresponding estimated inertial data in sets ${\mathbb{W}}_N$ and ${\mathbb{F}}_N$ can be considered to be the optimal estimation. In this way, the optimization model can be built using a max-value function, as given by

$$\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)=\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}P({\mathbb{W}}_N,{\mathbb{F}}_N|{\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N).$$

(33)

According to the Bayesian formula, the model in (33) can be written in the form of prior information as

$$\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)=\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}\left[\frac{P({\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N|{\mathbb{W}}_N,{\mathbb{F}}_N)P\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{P\left({\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N\right)}\right].$$

(34)

The measurements in sets ${\tilde{\mathbb{W}}}_N$, ${\tilde{\mathbb{F}}}_N$, and ${\tilde{\mathbb{R}}}_N$ are unchanged in an observation time interval, so the prior probability $P({\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N)$ can be regarded as a constant. Then, the optimization model can be written as

$$\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)=\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}\left[P({\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N|{\mathbb{W}}_N,{\mathbb{F}}_N)P\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)\right].$$

(35)

Since the inertial measurements follow the Gaussian distribution, the prior probability $P\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$ can be regard as a constant, and the model is further simplified to the maximum likelihood estimation (MLE) as

$$\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)=\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}P({\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N|{\mathbb{W}}_N,{\mathbb{F}}_N).$$

(36)

The sensors involved are independent from each other, so each measurement in sets ${\tilde{\mathbb{W}}}_N$, ${\tilde{\mathbb{F}}}_N$, and ${\tilde{\mathbb{R}}}_N$ is only related to its corresponding optimization variable, and the conditional probability in (36) can be written as

$$\begin{array}{c}P({\tilde{\mathbb{W}}}_N,{\tilde{\mathbb{F}}}_N,{\tilde{\mathbb{R}}}_N|{\mathbb{W}}_N,{\mathbb{F}}_N)=P\left({\tilde{\mathbb{W}}}_N|{\mathbb{W}}_N\right)P\left({\tilde{\mathbb{F}}}_N|{\mathbb{F}}_N\right)P\left({\tilde{\mathbb{R}}}_N|{\mathbb{W}}_N,{\mathbb{F}}_N\right)\hfill \\ =P\left({\tilde{\mathbf{\omega }}}_{ib,1}^b|{\mathbf{\omega }}_{ib,1}^b\right)\cdots P\left({\tilde{\mathbf{\omega }}}_{ib,N}^b|{\mathbf{\omega }}_{ib,N}^b\right)P\left({\tilde{\mathit{f}}}_1^b|{\mathit{f}}_1^b\right)\cdots P\left({\tilde{\mathit{f}}}_N^b|{\mathit{f}}_N^b\right)\hfill \\ P\left({\tilde{R}}_1|{\mathbf{\omega }}_{ib,1}^b,{\mathit{f}}_1^b\right)P\left({\tilde{R}}_2|{\mathbf{\omega }}_{ib,2}^b,{\mathit{f}}_2^b\right)\cdots P\left({\tilde{R}}_N|{\mathbf{\omega }}_{ib,N}^b,{\mathit{f}}_N^b\right)P\left({\tilde{R}}_{12}|{\mathbf{\omega }}_{ib,1}^b,{\mathit{f}}_1^b,{\mathbf{\omega }}_{ib,2}^b,{\mathit{f}}_2^b\right)\hfill \\ P\left({\tilde{R}}_{21}|{\mathbf{\omega }}_{ib,1}^b,{\mathit{f}}_1^b,{\mathbf{\omega }}_{ib,2}^b,{\mathit{f}}_2^b\right)\cdots P\left({\tilde{R}}_{N-1,N}|{\mathbf{\omega }}_{ib,N}^b,{\mathit{f}}_N^b,{\mathbf{\omega }}_{ib,N-1}^b,{\mathit{f}}_{N-1}^b\right)P\left({\tilde{R}}_{N,N-1}|{\mathbf{\omega }}_{ib,N}^b,{\mathit{f}}_N^b,{\mathbf{\omega }}_{ib,N-1}^b,{\mathit{f}}_{N-1}^b\right),\hfill \end{array}$$

(37)

where the multiplied conditional probabilities can be divided into four categories, as shown in

Table 1. The number of conditional probabilities multiplied in Equation (37). There are four categories of conditional probabilities, including $P\left({\tilde{\mathbf{\omega }}}_{ib,A}^b|{\mathbf{\omega }}_{ib,A}^b\right)$ for the angular velocity of agent A, $P\left({\tilde{\mathit{f}}}_A^b|{\mathit{f}}_A^b\right)$ for the specific force of agent A, $P\left({\tilde{R}}_A|{\mathbf{\omega }}_{ib,A}^b,{\mathit{f}}_A^b\right)$ for the range between agent A and the single anchor, and $P\left({\tilde{R}}_{AB}|{\mathbf{\omega }}_{ib,A}^b,{\mathit{f}}_A^b,{\mathbf{\omega }}_{ib,B}^b,{\mathit{f}}_B^b\right)$ for the range between agent A and agent B (measured by A).

Conditional Probability	$\mathit{P}\left({\tilde{\mathit{\omega }}}_{\mathit{i}\mathit{b},\mathit{A}}^{\mathit{b}}|{\mathit{\omega }}_{\mathit{i}\mathit{b},\mathit{A}}^{\mathit{b}}\right)$	$\mathit{P}\left({\tilde{\mathit{f}}}_{\mathit{A}}^{\mathit{b}}|{\mathit{f}}_{\mathit{A}}^{\mathit{b}}\right)$	$\mathit{P}\left({\tilde{\mathit{R}}}_{\mathit{A}}|{\mathit{\omega }}_{\mathit{i}\mathit{b},\mathit{A}}^{\mathit{b}},{\mathit{f}}_{\mathit{A}}^{\mathit{b}}\right)$	$\mathit{P}\left({\tilde{\mathit{R}}}_{\mathit{A}\mathit{B}}|{\mathit{\omega }}_{\mathit{i}\mathit{b},\mathit{A}}^{\mathit{b}},{\mathit{f}}_{\mathit{A}}^{\mathit{b}},{\mathit{\omega }}_{\mathit{i}\mathit{b},\mathit{B}}^{\mathit{b}},{\mathit{f}}_{\mathit{B}}^{\mathit{b}}\right)$
Number	N	N	N	$N^2-N$

.

On the basis of the Gaussian distribution models in (4), (5), (10), and (11), these conditional probabilities can be expressed by Gaussian probability density functions as

$$\begin{array}{c}P\left({\tilde{\mathbf{\omega }}}_{ib,A}^b|{\mathbf{\omega }}_{ib,A}^b\right)=\frac{1}{{\left(2\pi \right)}^{\frac{3}{2}}{\left|{\Sigma }_g\right|}^{\frac{1}{2}}}exp\left[-\frac{1}{2}{\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)}^T{\sum }_g^{-1}\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)\right]\hfill \\ =\frac{1}{{\left(2\pi \right)}^{\frac{3}{2}}{\left|{\Sigma }_g\right|}^{\frac{1}{2}}}exp\left[-\frac{1}{2{\sigma }_g^2}{\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)}^2\right],\hfill \end{array}$$

(38)

$$\begin{array}{c}P\left({\tilde{\mathit{f}}}_A^b|{\mathit{f}}_A^b\right)=\frac{1}{{\left(2\pi \right)}^{\frac{3}{2}}{\left|{\Sigma }_a\right|}^{\frac{1}{2}}}exp\left[-\frac{1}{2}{\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)}^T{\sum }_a^{-1}\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)\right]\hfill \\ =\frac{1}{{\left(2\pi \right)}^{\frac{3}{2}}{\left|{\Sigma }_a\right|}^{\frac{1}{2}}}exp\left[-\frac{1}{2{\sigma }_a^2}{\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)}^2\right]\hfill \end{array}$$

(39)

$$P\left({\tilde{R}}_A|{\mathbf{\omega }}_{ib,A}^b,{\mathit{f}}_A^b\right)=\frac{1}{{\left(2\pi \right)}^{\frac{1}{2}}{\sigma }_R}exp\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_A-R_A\right)}^2\right]=\frac{1}{{\left(2\pi \right)}^{\frac{1}{2}}{\sigma }_R}exp\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}^2\right],$$

(40)

$$P\left({\tilde{R}}_{AB}|{\mathbf{\omega }}_{ib,A}^b,{\mathit{f}}_A^b,{\mathbf{\omega }}_{ib,B}^b,{\mathit{f}}_B^b\right)=\frac{1}{{\left(2\pi \right)}^{\frac{1}{2}}{\sigma }_R}exp\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_{AB}-R_{AB}\right)}^2\right]=\frac{1}{{\left(2\pi \right)}^{\frac{1}{2}}{\sigma }_R}exp\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_{AB}-∥{\mathit{p}}_A^n-{\mathit{p}}_B^n∥\right)}^2\right],$$

(41)

where ${\mathit{p}}_0^n$ denotes the position of the single anchor in the navigation frame.

Substituting (37)–(41) into (36) and taking the logarithm, the optimization model can be written as

$$\begin{array}{c}\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)=\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}{\displaystyle \prod _{A=1}^N}P\left({\tilde{\mathbf{\omega }}}_{ib,A}^b|{\mathbf{\omega }}_{ib,A}^b\right)·{\displaystyle \prod _{A=1}^N}P\left({\tilde{\mathit{f}}}_A^b|{\mathit{f}}_A^b\right)\hfill \\ ·{\displaystyle \prod _{A=1}^N}P\left({\tilde{R}}_A|{\mathbf{\omega }}_{ib,A}^b,{\mathit{f}}_A^b\right)·{\displaystyle \prod _{A=1,B=1,A\ne B}^{A=N,B=N}}P\left({\tilde{R}}_{AB}|{\mathbf{\omega }}_{ib,A}^b,{\mathit{f}}_A^b,{\mathbf{\omega }}_{ib,B}^b,{\mathit{f}}_B^b\right)\hfill \\ =\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}{\displaystyle \prod _{A=1}^N}exp\left[-\frac{1}{2{\sigma }_g^2}{\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)}^2\right]·{\displaystyle \prod _{A=1}^N}exp\left[-\frac{1}{2{\sigma }_a^2}{\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)}^2\right]\hfill \\ ·{\displaystyle \prod _{A=1}^N}exp\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}^2\right]·{\displaystyle \prod _{A=1,B=1,A\ne B}^{A=N,B=N}}exp\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_{AB}-∥{\mathit{p}}_A^n-{\mathit{p}}_B^n∥\right)}^2\right]\hfill \\ =\underset{{\mathbb{W}}_N,{\mathbb{F}}_N}{argmax}{\displaystyle \sum _{A=1}^N}\left[-\frac{1}{2{\sigma }_g^2}{\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)}^2\right]+{\displaystyle \sum _{A=1}^N}\left[-\frac{1}{2{\sigma }_a^2}{\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)}^2\right]\hfill \\ +{\displaystyle \sum _{A=1}^N}\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}^2\right]+{\displaystyle \sum _{A=1,B=1,A\ne B}^{A=N,B=N}}\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_{AB}-∥{\mathit{p}}_A^n-{\mathit{p}}_B^n∥\right)}^2\right]\hfill \end{array}$$

(42)

Therefore, the optimal estimation of inertial measurements ${\mathbb{W}}_N$ and ${\mathbb{F}}_N$ is transformed into solving the extreme points of the function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$.

3.4. Solution of Optimal Inertial Measurements

Solving the extreme points or the stationary points of function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$ is the same as solving the following equations of first-order partial derivative:

$$\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathbf{\omega }}_{ib,1}^b}=\mathbf{0},...,\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathbf{\omega }}_{ib,N}^b}=\mathbf{0},$$

(43)

$$\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathit{f}}_1^b}=\mathbf{0},...,\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathit{f}}_N^b}=\mathbf{0},$$

(44)

Suppose $A=1,2,...,N$, and $B=1,2,...,N$, every angular velocity ${\mathbf{\omega }}_{ib,A}^b$ and specific force ${\mathit{f}}_A^b$ are $3\times 1$ column vectors, and every $\mathbf{0}$ is a $1\times 3$ row vector. The function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$ is a scalar according to (42), so there are $6N$ nonlinear equations in total. To simplify the derivative calculation, we divide the function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$ into four subfunctions:

$$F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)=F_1+F_2+F_3+F_4,$$

(45)

where

$$F_1=\sum _{A=1}^N\left[-\frac{1}{2{\sigma }_g^2}{\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)}^2\right],$$

(46)

$$F_2=\sum _{A=1}^N\left[-\frac{1}{2{\sigma }_a^2}{\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)}^2\right],$$

(47)

$$F_3=\sum _{A=1}^N\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}^2\right],$$

(48)

$$F_4=\sum _{A=1,B=1,A\ne B}^{A=N,B=N}\left[-\frac{1}{2{\sigma }_R^2}{\left({\tilde{R}}_{AB}-∥{\mathit{p}}_A^n-{\mathit{p}}_B^n∥\right)}^2\right].$$

(49)

Then, the function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$ can be split into four functions for derivation. For the agent A, partial derivatives of the function versus ${\mathbf{\omega }}_{ib,A}^b$ are given by

$$\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{\partial F_1}{\partial {\mathbf{\omega }}_{ib,A}^b}+\frac{\partial F_2}{\partial {\mathbf{\omega }}_{ib,A}^b}+\frac{\partial F_3}{\partial {\mathbf{\omega }}_{ib,A}^b}+\frac{\partial F_4}{\partial {\mathbf{\omega }}_{ib,A}^b},$$

(50)

$$\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathit{f}}_A^b}=\frac{\partial F_1}{\partial f_A^b}+\frac{\partial F_2}{\partial {\mathit{f}}_A^b}+\frac{\partial F_3}{\partial {\mathit{f}}_A^b}+\frac{\partial F_4}{\partial {\mathit{f}}_A^b}.$$

(51)

The functions $F_1$ and $F_2$ are only related to the angular velocity ${\mathbf{\omega }}_{ib,A}^b$ or the specific force ${\mathit{f}}_A^b$ of the corresponding agent A, so the derivatives of $F_1$ and $F_2$ can be directly given by

$$\frac{\partial \left(F_1+F_2\right)}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{\partial F_1}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{1}{{\sigma }_g^2}\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right),$$

(52)

$$\frac{\partial \left(F_1+F_2\right)}{\partial {\mathit{f}}_A^b}=\frac{\partial F_2}{\partial {\mathit{f}}_A^b}=\frac{1}{{\sigma }_a^2}\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right),$$

(53)

The derivative of function $F_3$ is related to both the angular velocity ${\mathbf{\omega }}_{ib,A}^b$ and the specific force ${\mathit{f}}_A^b$ of the corresponding agent A. So, the key of the derivative calculation of $F_3$ is the expression of ${\mathbf{\omega }}_{ib,A}^b$ and ${\mathit{f}}_k^b$ regarding ${\mathit{p}}_A^n$, which is derived in Section 3.2 and shown in (30). Taking the partial derivative of ${\mathit{p}}_A^n$ versus ${\mathbf{\omega }}_{ib,A}^b$ and ${\mathit{f}}_A^b$ using the matrix derivative rules in [38], we have

$$\frac{\partial {\left({\mathit{p}}_A^n\right)}^T}{\partial {\mathbf{\omega }}_{ib,A}^b}=\left[{\mathit{f}}_A^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_c^T\Delta t^3,$$

(54)

$$\frac{\partial {\left({\mathit{p}}_A^n\right)}^T}{\partial {\mathit{f}}_A^b}={\mathit{C}}_n^b{\mathit{R}}_C^T\Delta t^2-\left[{\mathbf{\omega }}_{ib,A}^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_C^T\Delta t^3+{\mathit{C}}_n^b{\mathit{R}}_C^T\Delta t^3,$$

(55)

where the parameters are in the time interval $\left[t_{k-1},t_k\right]$.

According to (48), the L2 norm $∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥$ in function $F3$ can be simplified as

$$∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥=\sqrt{{\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right)}^T\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right)}=\sqrt{{\left({\mathit{p}}_A^n\right)}^T{\mathit{p}}_A^n-2{\left({\mathit{p}}_A^n\right)}^T{\mathit{p}}_0^n+{\left({\mathit{p}}_0^n\right)}^T{\mathit{p}}_0^n}.$$

(56)

Therefore, the partial derivative of $F_3$ versus ${\mathbf{\omega }}_{ib,A}^b$ is calculated as

$$\frac{\partial F_3}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2}·\frac{\partial ∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}·\frac{\partial {\left({\mathit{p}}_A^n\right)}^T}{\partial {\mathbf{\omega }}_{ib,A}^b}\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right),$$

(57)

and the partial derivative of $F_3$ versus ${\mathit{f}}_k^b$ is calculated as

$$\frac{\partial F_3}{\partial {\mathit{f}}_A^b}=\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2}·\frac{\partial ∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}{\partial {\mathit{f}}_A^b}=\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}·\frac{\partial {\left({\mathit{p}}_A^n\right)}^T}{\partial {\mathit{f}}_A^b}\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right),$$

(58)

Substituting (54) into (57) and (55) into (58), we obtain the partial derivatives as

$$\frac{\partial F_3}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}·\left[{\mathit{f}}_A^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_c^T\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right)\Delta t^3,$$

(59)

$$\frac{\partial F_3}{\partial {\mathit{f}}_A^b}=\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}·\left\{{\mathit{C}}_n^b{\mathit{R}}_C^T\frac{1}{\Delta t}-\left[{\mathbf{\omega }}_{ib,A}^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_C^T+{\mathit{C}}_n^b{\mathit{R}}_C^T\right\}\left({\mathit{p}}_A^n-{\mathit{p}}_0^n.\right)\Delta t^3.$$

(60)

The derivative of function $F_4$ is related to all the angular velocities and the specific forces of the N agents, which includes $N-1$ terms for each agent, and $F_4$ can be rewritten as

$$F_4=-\frac{1}{2{\sigma }_R^2}\sum _{A=1}^N\sum _{j=1,j\ne A}^N\left[{\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)}^2+{\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)}^2\right].$$

(61)

According to the derivation of $F_3$, the partial derivative of $F_4$ versus ${\mathbf{\omega }}_{ib,A}^b$ and ${\mathit{f}}_A^b$ is calculated as

$$\begin{array}{c}\frac{\partial F_4}{\partial {\mathbf{\omega }}_{ib,A}^b}={\displaystyle \sum _{j=1,j\ne A}^N}\left[\frac{\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)}{{\sigma }_R^2}·\frac{\partial ∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}{\partial {\mathbf{\omega }}_{ib,A}^b}+\frac{\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)}{{\sigma }_R^2}·\frac{\partial ∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}{\partial {\mathbf{\omega }}_{ib,A}^b}\right]\hfill \\ ={\displaystyle \sum _{j=1,j\ne A}^N}\frac{\left[\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)+\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)\right]}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}·\frac{\partial {\left({\mathit{p}}_A^n\right)}^T}{\partial {\mathbf{\omega }}_{ib,A}^b}\left({\mathit{p}}_A^n-{\mathit{p}}_j^n\right)\hfill \\ ={\displaystyle \sum _{j=1,j\ne A}^N}\frac{\left[\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)+\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)\right]}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}·\left[{\mathit{f}}_A^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_c^T\left({\mathit{p}}_A^n-{\mathit{p}}_j^n\right)\Delta t^3,\hfill \end{array}$$

(62)

$$\begin{array}{c}\frac{\partial F_4}{\partial {\mathit{f}}_A^b}={\displaystyle \sum _{j=1,j\ne A}^N}\left[\frac{\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)}{{\sigma }_R^2}·\frac{\partial ∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}{\partial {\mathit{f}}_A^b}+\frac{\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)}{{\sigma }_R^2}·\frac{\partial ∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}{\partial {\mathit{f}}_A^b}\right]\hfill \\ ={\displaystyle \sum _{j=1,j\ne A}^N}\frac{\left[\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)+\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)\right]}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}·\frac{\partial {\left({\mathit{p}}_A^n\right)}^T}{\partial {\mathit{f}}_A^b}\left({\mathit{p}}_A^n-{\mathit{p}}_j^n\right)\hfill \\ ={\displaystyle \sum _{j=1,j\ne A}^N}\frac{\left[\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)+\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)\right]}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}\left\{{\mathit{C}}_n^b{\mathit{R}}_C^T\frac{1}{\Delta t}-\left[{\mathbf{\omega }}_{ib,A}^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_C^T+{\mathit{C}}_n^b{\mathit{R}}_C^T\right\}\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right)\Delta t^3.\hfill \end{array}$$

(63)

Substituting (52), (59), and (62) into (50) and (53), (60), and (63) into (51), we obtain the partial derivatives of function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$ to agent A versus ${\mathbf{\omega }}_{ib,A}^b$ and ${\mathit{f}}_A^b$, as given by

$$\begin{array}{c}\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathbf{\omega }}_{ib,A}^b}=\frac{1}{{\sigma }_g^2}\left({\tilde{\mathbf{\omega }}}_{ib,A}^b-{\mathbf{\omega }}_{ib,A}^b-{\mathit{b}}_g\right)+\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}·\left[{\mathit{f}}_A^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_c^T\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right)\Delta t^3\hfill \\ +{\displaystyle \sum _{j=1,j\ne A}^N}\frac{\left[\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)+\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)\right]}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}·\left[{\mathit{f}}_A^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_c^T\left({\mathit{p}}_A^n-{\mathit{p}}_j^n\right)\Delta t^3,\hfill \end{array}$$

(64)

$$\begin{array}{c}\frac{\partial F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)}{\partial {\mathit{f}}_A^b}=\frac{1}{{\sigma }_a^2}\left({\tilde{\mathit{f}}}_A^b-{\mathit{f}}_A^b-{\mathit{b}}_a\right)+\frac{\left({\tilde{R}}_A-∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥\right)}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_0^n∥}·\left\{{\mathit{C}}_n^b{\mathit{R}}_C^T\frac{1}{\Delta t}-\left[{\mathbf{\omega }}_{ib,A}^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_C^T+{\mathit{C}}_n^b{\mathit{R}}_C^T\right\}\left({\mathit{p}}_A^n-{\mathit{p}}_0^n\right)\Delta t^3\hfill \\ +{\displaystyle \sum _{j=1,j\ne A}^N}\frac{\left[\left({\tilde{R}}_{Aj}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)+\left({\tilde{R}}_{jA}-∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥\right)\right]}{{\sigma }_R^2∥{\mathit{p}}_A^n-{\mathit{p}}_j^n∥}\left\{{\mathit{C}}_n^b{\mathit{R}}_C^T\frac{1}{\Delta t}-\left[{\mathbf{\omega }}_{ib,A}^b\times \right]{\mathit{C}}_n^b{\mathit{R}}_C^T+{\mathit{C}}_n^b{\mathit{R}}_C^T\right\}\left({\mathit{p}}_A^n-{\mathit{p}}_j^n\right)\Delta t^3.\hfill \end{array}$$

(65)

Substituting (64) into (43) and (65) into (44), the $6N$ nonlinear equations are expressed by the inertial outputs in sets ${\mathbb{W}}_N$ and ${\mathbb{F}}_N$; then, the estimated inertial outputs can be obtained by solving the nonlinear equations.

3.5. Discussion on Extreme Points

The optimal estimation of inertial data is solved at the extreme points of function $F\left({\mathbb{W}}_N,{\mathbb{F}}_N\right)$, as shown in (43) and (44). This method is based on the assumption that the stationary points solved by the first-order partial derivative is exactly the extreme points.

When solving for a stationary point of a function in practical applications, it is typically assumed to be either a maximum or minimum point. However, this assumption is not rigorous. Therefore, several issues about the above method of obtaining the stagnation point by solving the nonlinear equation need to be discussed:

• Does the system of nonlinear equations have a solution?
• Is the solution of the system of nonlinear equations unique?
• Does the obtained stationary point correspond to the extreme point?
• Is the extreme point the maximum point?

Issue 1 can be demonstrated with the method of solving the nonlinear equations system in (43) and (44). Because it is a nonlinear least square problem, it can be solved by iteration using optimization methods such as gradient descent. In this way, an initial value for the optimization variable needs to set before the optimization, which can be set as the original IMU data ${\tilde{\mathbf{\omega }}}_{ib}^b$ and ${\tilde{\mathit{f}}}^b$. If there are no ranging data or the optimal solution in this time interval, no iteration will be performed. That is to say, there is undoubtedly a solution to the equation, and the solution may be the original IMU data or the optimization variable after iteration.

The other issues are common to many extreme value problems, which can be explained by the concept of exponential family [39]. The exponential family distribution of x, which indicates the IMU data ${\tilde{\mathbf{\omega }}}_{ib}^b$ and ${\tilde{\mathit{f}}}^b$, with the prior parameter $\theta \in {\mathbb{R}}^d$ is defined as follows:

$$p\left(x|\theta \right)=h\left(x\right)exp\left[\eta {\left(\theta \right)}^{T}T\left(x\right)-A\left(\theta \right)\right],$$

(66)

where $h\left(x\right)$, $\eta \left(\theta \right)$, $T\left(x\right)$ and $A\left(\theta \right)$ are all known functions to a certain distribution. The Gaussian distribution with mean value $\mu$ and variance ${\sigma }^2$ can be proved to belong to the exponential distribution family:

$$\begin{array}{c}p\left(x|\mu ,\sigma \right)=\frac{1}{\sqrt{2\pi }\sigma }exp\left(-\frac{{\left(x-\mu \right)}^2}{2{\sigma }^2}\right)\hfill \\ =\frac{1}{\sqrt{2\pi }}exp\left(-\frac{1}{2{\sigma }^2}{x}^2+\frac{\mu }{{\sigma }^2}x-\frac{{\mu }^2}{2{\sigma }^2}-log\sigma \right),\hfill \end{array}$$

(67)

where $h\left(x\right)=\frac{1}{\sqrt{2\pi }\sigma }$, $\eta \left(\mu ,\sigma \right)={\left[-\frac{1}{2{\sigma }^2},\frac{\mu }{{\sigma }^2}\right]}^T$, $T\left(x\right)={\left[x^2,x\right]}^T$ and $A\left(\mu ,\sigma \right)=\frac{{\mu }^2}{2{\sigma }^2}+log\sigma$. And, it is similar to the Gaussian probability density functions in (38)–(41), which enable the Gaussian distribution model discussed above to belong to the exponential distribution family.

The key to determining the extreme point is to discuss the concavity of the log partition function $A\left(\theta \right)$. The Holder inequality and the Hessian matrix is used in [39], both of which prove that $A\left(\theta \right)$ is a concave function.

The maximum likelihood problem is defined as an estimation of $\theta$ to solve the maximum of

$$L\left(\theta |X\right)={log}_{p\left(x|\theta \right)}\left(X\right)=\sum _{i=1}^{n}p\left(x_i|\theta \right).$$

(68)

with a given data set $X=\left\{x_1,x_2,\dots ,x_n\right\}$ and a distribution family to be fitted, $p\left(x|\theta \right)$. If $p\left(x|\theta \right)$ belongs to exponential distribution family, the likelihood function can be written as follows:

$$L\left(\theta |X\right)=logh\left(X\right)+\eta {\left(\theta \right)}^T\sum _{i=1}^n{x}_i-nA\left(\theta \right).$$

(69)

where $h\left(X\right)={\displaystyle \prod _{i=1}^n}h\left(x_i\right)$ is a constant. $A\left(\theta \right)$ is a concave function about $\theta$; thus, $-A\left(\theta \right)$ is a convex function about $\theta$. ${\left(\theta \right)}^T\sum _{i=1}^n{x}_i$ is a linear function of $\theta$. So, their sum $L\left(\theta |X\right)$ is a convex function about $\theta$. That is to say, if there is a solution when the derivative equals zero, it must be the only one, and the corresponding function value is the maximum. In conclusion, Issues 2–4 are explained by the exponential distribution family, and the stationary point is calculated with the nonlinear equations in (43) and (44).

Therefore, the stationary points solved by (43) and (44) are exactly the extreme points, and the optimal estimation of the inertial measurements can be obtained at these extreme points.

4. Single-Anchor Cooperative Positioning Method

Based on the inertial optimization algorithm, we design a cooperative positioning method for multi-agent system in single-anchor environments. The framework of the single-anchor cooperative positioning method is depicted in Figure 4, and we present the design ideas of the method below.

Different from the common filtering method for inertial-ranging positioning, we add an optimizer to the inertial sensors at the ranging time, which replaces the inertial measurement ${\tilde{\mathbb{W}}}_N$ and ${\tilde{\mathbb{F}}}_N$ with the optimal estimation ${\mathbb{W}}_N$ and ${\mathbb{F}}_N$.

After the optimization, the attitude, velocity, and position can be updated using the inertial calculation, and the range calculated using the inertial data can be computed as shown in (31). However, the updated position does not differ significantly from the position calculated using the original inertial data. This can be attributed to the fact that estimated inertial data lack prior information for the optimization process, as the inertial output is independently and identically distributed within each time interval, and the ranging rate is lower than the inertial rate.

In order to further improve the positioning accuracy and to make full use of the measurements, a filter is employed after the optimizer to use the prior position for navigation. The range measurement ${\tilde{\mathbb{R}}}_N$ is also acquired in the filter to be the observation. The EKF is selected considering its high efficiency and convenience for solving nonlinear problems, and the filtering process is formulated below.

For a centralized multi-agent nonlinear system, the discretized model’s time interval $\left[t_{k-1},t_k\right]$ is given by the following:

$$\left\{\begin{array}{c}{\mathit{x}}_k={\mathit{f}}_{k-1}{\mathit{x}}_{k-1}+{\mathit{w}}_{k-1}\hfill \\ {\mathit{z}}_k={\mathit{H}}_k{\mathit{x}}_k+{\mathit{v}}_k\hfill \end{array}\right.,$$

(70)

where ${\mathit{x}}_k$ denotes the navigation state error vector, which includes the attitude ${\left[\theta ,\gamma ,\phi \right]}^T$, velocity ${\left[v_E,v_N,v_U\right]}^T$, and position ${\left[L,\lambda ,h\right]}^T$ of all the agents in navigation frame, and the error state ${\mathit{x}}_{k,A}$ for agent A can be given by the following:

$${\mathit{x}}_{k,A}={\left[\delta {\theta }_{k,A},\delta {\gamma }_{k,A},\delta {\phi }_{k,A},\delta {v_E}_{k,A},\delta {v_N}_{k,A},\delta {v_U}_{k,A},\delta L_{k,A},\delta {\lambda }_{k,A},\delta h_{k,A}\right]}^T,$$

(71)

${\mathit{w}}_{k-1}\sim N\left(\mathbf{0},{\mathit{Q}}_{k-1}\right)$ is the Gaussian process noise. ${\mathit{Q}}_k$ is the diagonal matrix of process noise cross-covariance matrices, and the process noise cross-covariance matrix for agent A is given by the following:

$${\mathit{Q}}_{k,A}=cov\left[{\mathit{w}}_{k,A}{\mathit{w}}_{k,A}^T\right]=\left[\begin{array}{ccc}{\Sigma }_g^2& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\Sigma }_a^2& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right].$$

(72)

$\mathit{z}$ is the measurement vector, which is the numerical difference between the range calculated using the inertial data and the range measured by the range sensors, as given by $\mathbb{R}{}_N-\tilde{\mathbb{R}}{}_N$ or

$${\mathit{z}}_k=\left[\begin{array}{c}{R}_1-{\tilde{R}}_1\\ R_2-{\tilde{R}}_2\\ ⋮\\ R_N-{\tilde{R}}_N\\ R_{12}-{\tilde{R}}_{12}\\ R_{21}-{\tilde{R}}_{21}\\ ⋮\\ R_{N-1,N}-{\tilde{R}}_{N-1,N}\\ R_{N,N-1}-{\tilde{R}}_{N,N-1}\end{array}\right]=\mathit{H}\delta x+\mathbf{\nu }.$$

(73)

${\mathbf{\nu }}_k\sim N\left(0,R_k\right)$ is the Gaussian measurement noise. $R_k={\sigma }_R^2$ is the observation noise cross-covariance matrix, and the dimension here is $1\times 1$ for a single-beacon measurement. $\mathit{H}$ is the measurement matrix. $\mathit{f}$ is the state-transition matrix of all agents, and the state-transition matrix for agent A is given by the following:

$$\mathit{f}=\left[\begin{array}{ccc}{\mathit{M}}_{aa}& {\mathit{M}}_{av}& {\mathit{M}}_{ap}\\ {\mathit{M}}_{va}& {\mathit{M}}_{vv}& {\mathit{M}}_{vp}\\ {\mathit{M}}_{pa}& {\mathit{M}}_{pv}& {\mathit{M}}_{pp}\end{array}\right],$$

(74)

where the block matrices in (74) are calculated using the attitude, velocity, and position updated by the inertial calculation process, as shown in Appendix A.

The prediction and correction steps of EKF are given by the following:

$${\widehat{\mathit{x}}}_{k|k-1}={\mathit{f}}_{k-1}{\widehat{\mathit{x}}}_{k-1|k-1},$$

(75)

$${\mathit{p}}_{k|k-1}={\mathit{f}}_{k-1}{\mathit{p}}_{k-1|k-1}{\mathit{f}}_{k-1}^T+{\mathit{Q}}_k,$$

(76)

$${\mathit{K}}_k=\frac{{\mathit{p}}_{k|k-1}{\mathit{H}}_k^T}{{\mathit{H}}_k{\mathit{p}}_{k|k-1}{\mathit{H}}_k^T+{\mathit{R}}_k},$$

(77)

$${\widehat{\mathit{x}}}_{k|k}={\widehat{\mathit{x}}}_{k|k-1}+{\mathit{K}}_k\left({\mathit{z}}_k-{\mathit{H}}_k{\widehat{\mathit{x}}}_{k|k-1}\right),$$

(78)

$${\mathit{p}}_{k|k}=\left(\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right){\mathit{p}}_{k|k-1},$$

(79)

where $\mathit{p}$ denotes the cross-covariance matrix and $\mathit{I}$ denotes the identity matrix.

The outputs of the filter are the updated attitude error, velocity error, and position error, which are added to the input of the filter and form the updated attitude, velocity, and position. Further, the whole process of the single-anchor cooperative positioning method is summarized as Algorithm 1.

Algorithm 1 Single-anchor cooperative positioning method using inertial measurements

1: Input: The sensor characteristics with ${\mathit{b}}_g$, ${\sigma }_g$ for gyroscope, ${\mathit{b}}_a$, ${\sigma }_a$ for accelerometer, and ${\sigma }_R$ for range sensor; the prior navigation state of time $t_{k-1}$. 2: loop 3:    if there are inertial measurements ${\tilde{\mathbb{W}}}_N$, ${\tilde{\mathbb{F}}}_N$ in $t_k$ then 4:      if there are range measurements $\tilde{\mathbb{R}}{}_N$ in $t_k$ then 5:         Optimization: Estimate the optimal inertial data in sets ${\tilde{\mathbb{W}}}_N$ and ${\tilde{\mathbb{F}}}_N$; 6:         Filtering: Update the position using the measurements and the prior information. (Various filtering methods are optional, the EKF is selected in this paper for its convenience.) 7:      else 8:         Update the attitude, velocity and position by inertial calculation process; 9:      end if 10:   end if 11: end loop 12: Output: The updated position in $t_k$.

5. Simulation Experiment

To demonstrate the effectiveness of the proposed single-anchor cooperative positioning method, the simulation experiment is carried out. We build a four-agent single-anchor simulation environment and analyze the difference on positioning accuracy between the proposed method, the single filtering, and the single optimization.

5.1. Model of Simulation Environment

A simulated scenario of four UAVs and a single anchor is built in this paper. The horizontal and three-dimensional projections of the scenario are shown in Figure 5. Each UAV starts moving simultaneously from rest in the same horizontal plane, with a vertical acceleration of 2.5 m/s${}^2$ or a horizontal acceleration of 2 m/s${}^2$. When moving at a constant speed, the velocity in the vertical direction is 5 m/s and the velocity in the horizontal direction is 2 m/s.

To facilitate the display of the moving trajectories of each agent, the navigation frame was converted to a local coordinate frame with the midpoint of the trajectory as the origin point $[0,0,0]$. The coordinates of the anchor and the starting points of agents in the local coordinate frame are shown in

Table 2. Coordinates of the anchor and the starting points of the agents in the local coordinate frame.

Point	Coordinate
Anchor	$[200,200,5]$
Starting point of Agent 1	$[100,150,0]$
Starting point of Agent 2	$[-50,100,0]$
Starting point of Agent 3	$[-100,-150,0]$
Starting point of Agent 4	$[50,-100,0]$

with the unit in meter.

The four moving agents are equipped with inertial sensors and range sensors. The parameters of the simulated sensors are shown in

Table 3. Parameters used in the simulation experiment.

Parameter	Value
Gyroscope constant bias $b_g$	0.2 ${}^{\circ }/\mathrm{h}$
Gyroscope angular random walk ${\sigma }_g$	0.02 ${}^{\circ }/\sqrt{h}$
Accelerometer constant bias $b_a$	2 mg
Accelerometer velocity random walk ${\sigma }_a$	0.2 $\mathrm{mg}/\sqrt{\mathrm{Hz}}$
Inertial sensor frequency	0.01 s
Ranging standard deviation ${\sigma }_R$	0.1 m
Range sensor frequency	0.1 s

, where the sensor outputs follow the Gaussian distribution discussed in Section 2.2. The inertial sensors on four agents have the same parameters, and the range sensors on the anchor and four agents also have the same parameters.

The simulated inertial data are output by the trajectory generator, as shown in Figure 6.

5.2. Simulation Results and Analysis

The single-anchor cooperative positioning method is a combination of optimization and filtering processes, and is presented in Section 4 and shown in Figure 4. Compared to the proposed method, both pure filtering and pure optimization have obvious divergence on the trajectories, which are shown in Figure 7. As the altitude direction of inertial calculation is divergent, we also conducted comparisons on whether there are height constraints.

As shown in Figure 7a,b, the positioning results of pure optimization have significant divergence compared to real trajectories. The reason is that the estimated inertial data cannot provide prior information, as discussed in Section 4. Due to the severe divergence of pure optimization itself, the difference in whether there are high constraints is not significant.

As shown in Figure 7c,d, the positioning results of pure filtering also deviate from the real trajectories. If there is no height constraint, as only one anchor is used for ranging, the agents move near the circle with the anchor as the center. When there are height constraints, the agents still move near the arc, with the anchor as the center, but the divergence is not as pronounced compared to when there is no height constraint. Moreover, the constraint effect caused by the range measurements between nodes is evident with the height constraints.

If there is no cooperation among the agents, which means there is no communication of the ranging $\left({\tilde{R}}_{12},{\tilde{R}}_{21},\dots ,{\tilde{R}}_{N-1,N},{\tilde{R}}_{N,N-1}\right)$ between the agents, the trajectories of the agents solved using the single-anchor positioning method is shown in Figure 8.

This positioning result in Figure 8 is equivalent to each agent being individually positioned relative to the anchor using single-anchor positioning. The observation only includes the range measurements between four agents and the anchor, and the measurement vector in the filter is simplified to

$$\mathit{z}=\left[\begin{array}{c}{R}_1-{\tilde{R}}_1\\ R_2-{\tilde{R}}_2\\ R_3-{\tilde{R}}_3\\ R_4-{\tilde{R}}_4\end{array}\right]=\mathit{H}\delta x+\mathbf{\nu }.$$

(80)

There is no height constraint in the single-anchor positioning method, and it is obvious that when relying solely on range measurements from a single anchor, the trajectory diverges significantly in the vertical direction. Regardless, the trajectories in Figure 8 are much closer to the real trajectories than those solved by pure optimization or pure filtering.

If we add the range measurements $\left({\tilde{R}}_{12},{\tilde{R}}_{21},\dots ,{\tilde{R}}_{N-1,N},{\tilde{R}}_{N,N-1}\right)$ between the agents to form the cooperative system, the results of the single-anchor cooperative positioning method is shown in Figure 9. The measurement vector here is the vector in (73).

The positioning results of two single-anchor positioning methods are presented above: one only utilizes single anchor observations, while the other integrates observations between agents for cooperation. The components of positioning errors between the estimated position and the real position in three directions are shown in Figure 10. The directions are East, North, and Up in the navigation frame. It is obvious that the cooperation of the agents improves the positioning accuracy.

The positioning results can be quantitatively analyzed using root mean square error (RMSE), as given by

$$RMSE=\sqrt{\frac{1}{n}\sum _{i=1}^n{\left({\widehat{p}}_i-p_i\right)}^2},$$

(81)

where n denotes the number of time intervals, ${\widehat{p}}_i$ denotes the estimated position, and $p_i$ denotes the real position. The RMSE of the positioning results are shown in

Table 4. The RMSE of the positioning results of two single-anchor positioning methods in simulation experiment. For each agent and each method, there are the total RMSE and its components in the East, North, and Up directions, with the unit in meter.

Agent & Method	East	North	Up	Total
Single Agent, Agent 1	5.5288	17.8081	39.6870	43.8492
Cooperative, Agent 1	1.6037	1.7913	9.2595	9.5665
Single Agent, Agent 2	18.0673	20.4627	47.8726	55.1084
Cooperative, Agent 2	3.5570	8.0210	14.0858	16.5952
Single Agent, Agent 3	9.1513	11.6996	92.2450	93.4332
Cooperative, Agent 3	5.2348	5.3550	22.1043	23.3384
Single Agent, Agent 4	14.7703	11.5248	37.7707	42.1617
Cooperative, Agent 4	5.2812	5.9927	17.0927	18.8671

. The table indicates that the positioning error in each direction of each agent decreases with the introduction of cooperation, and this is reflected in the overall decrease in total error.

Therefore, the simulation experiment shows that the single-anchor cooperative positioning method proposed in this paper has higher positioning accuracy compared to individual optimization, individual filtering, or the non-cooperative “optimization + filtering” method.

6. Real-World Experiment

To further validate the effectiveness of the single-anchor cooperative positioning method, we conducted a real-world experiment using three agents, which are shown in Figure 2. Due to the limitation of the equipment, we replace the UAVs with UGVs, but the equipped sensors are also applicable to UAVs.

6.1. Real-World Experimental Environment

The experiment was conducted indoors to create a GNSS-denied environment, and the satellite imagery and sketch map are shown in Figure 11.

The UGVs are equipped with IMUs and UWBs, and the parameters of the sensors are given in

Table 5. Parameters used in the real-world experiment.

Parameter	Value
Gyroscope constant bias $b_g$	1 ${}^{\circ }/\mathrm{h}$
Gyroscope angular random walk ${\sigma }_g$	0.1 ${}^{\circ }/\sqrt{h}$
Accelerometer constant bias $b_a$	5 mg
Accelerometer velocity random walk ${\sigma }_a$	0.5 $\mathrm{mg}/\sqrt{\mathrm{Hz}}$
Inertial sensor frequency	0.005 s
Ranging standard deviation ${\sigma }_R$	0.1 m
Range sensor frequency	0.02 s

. The vehicles started moving from stationary, with the starting points shown in Figure 11b, and the agents were aligned while stationary for accurate initial states.

6.2. Real-World Results and Analysis

The three UGVs moving along the corridor maintain communication with each other, as shown in Figure 12a. Due to the rejection of satellite signals, the odometer and other anchors were used for integrated navigation to construct the reference trajectory of the agents, and the reference trajectory of Agent 1 is shown in Figure 12b. The trajectories solved by pure filtering are shown in Figure 12c, which have significant divergence from the simulation experiment.

Similar to the simulation experiment, we estimate the position using two single-anchor positioning methods, and the components of positioning errors between the estimated position and the real position are shown in Figure 13. Additionally, the RMSE of the positioning results are shown in

Table 6. The RMSE of the positioning results of two single-anchor positioning methods in real-world experiment. For each agent and each method, there are the total RMSE and its components in the East, North, and Up directions, with the unit in meter.

Agent & Method	East	North	Up	Total
Single Agent, Agent 1	2.4677	4.7936	3.1790	6.2589
Cooperative, Agent 1	2.4672	4.9743	1.3240	5.7082
Single Agent, Agent 2	2.4240	4.7466	1.7510	6.4677
Cooperative, Agent 2	2.2601	4.9688	3.6641	5.7326
Single Agent, Agent 3	2.7217	5.6337	3.9202	7.3833
Cooperative, Agent 3	3.1588	6.8185	1.0772	7.5915

.

Although the correction effect of the eastward position and northward position is not as evident as the cooperative method in the simulation experiment, the vertical positioning error noticeably reduces when the agents are taken into consideration. Meanwhile, compared to the method without cooperation, the positioning error of the cooperative method when stationary is more stable.

The unexpected performance of the cooperative method may due to the error in the integrated navigation of the reference trajectories. Nevertheless, the positions estimated using the proposed single-anchor positioning method are much more accurate than those estimated by pure filtering.

7. Conclusions

This paper’s novel contribution is proposing a single-anchor cooperative positioning method for a multi-UAV system. The inertial measurements from the agents are optimized by the range measurements between the UAVs and the anchor. The process of Bayesian estimation is derived in detail with the model of sensors and the principle of inertial calculation. Simulation and real-world experiments are carried out, and the effectiveness of the method is proved. Although the estimated position cannot be completely consistent with the real one, it is enough in the case of insufficient sensor measurements. More experiments are required in future work to certify that the method is effective for more extensive ranges and agents.