Cooperative Localization of Multi-UAVs via Dynamic Nonparametric Belief Propagation under GPS Signal Loss Condition

Abstract

Self-localization is critical for many unmanned aerial vehicles (UAVs) tasks such as formation flight, path planning, and activity coordination. Traditionally, UAV can locate itself using GPS combined with some inertial sensors. However, due to the complex flight environment or failure of the GPS receiver, the UAV may lose its GPS signal and fail to locate itself, resulting in devastating consequence. In this paper, we will consider the problem of cooperative localization among multiple UAVs, in which the UAVs with failure of GPS receiver can help each other to locate themselves through mutual information exchanged based on the relative distance measurements. Specifically, we propose a dynamic Nonparametric Belief Propagation (dNBP) algorithm to calculate the posterior distribution of UAV's position conditioned on all observations made in the whole UAVs group. The dNBP is a natural combination of NBP with particle filtering, suitable for treating with the nonlinear model and highly non-Gaussian distributions arising in our application. Furthermore, dNBP provides the basis for distributed algorithm in which messages are exchanges between neighboring UAVs. Thus, the computational burden is distributed across UAVs. Simulations in Matlab environment show the effectiveness of our method.

1. Introduction

Unmanned aerial vehicles (UAVs) have attracted significant interest for a wide range of applications. Among these applications, active cooperation of several low-cost small-size UAVs has important advantages. The basic requirement of many flight tasks of UAVs group, such as the formation flight [1, 2], coordinated rendezvous [3], coordinated path planning [4], and task coordination [5], is the correct localization of each member in the group. Traditionally, the navigation or localization method of UAV is based on integration of inertial navigation system (INS), global positioning system, and so forth. In the case of flying in complicated environment such as buildings, foliage and hilly terrain, or errors of GPS receiver, UAV may lose its GPS signal, leading to catastrophic effects during the flight mission.

Here, we classify UAVs in the group into two types: fault UAVs, in which GPS fails to provide location information, and normal UAVs, which have available GPS measurements. We assume that each UAV in the group, fault or normal, has limited resource for computation and communication and synchronized internal clock that allows the UAVs to share a common notion of time. We also assume that each UAV has a noisy distance estimate to its neighbors (i.e., the ones within certain range) from signal metrics measurement based on direct communication. The relative range measurements can be obtained using either ultra wideband (UWB) radio technology or optical systems [6].

In a group of UAVs, the one with failure of GPS receiver can manage to localize itself using classical least square estimator [7] based on the relative distance measurement with respect to nearby normal ones. See Figure 1; for example, the three black points represent three normal UAVs (UAV0, UAV1, and UAV2) whose positions measurements are available. The red points stand for the fault UAVs (UAV3 and UAV4) whose positions are unknown. UAV3 can independently determine its own position in 2-dimensional space based on the relative distance estimates with respect to the other three normal UAVs (UAV0, UAV1, and UAV2).

Figure 1

Scheme of multi-UAVs cooperative localization.

However, due to the limitation of signal transmission, the distance measurement between UAV1 and UAV4 may be not reachable or be too noisy to be useful, as they are too far away from each other. So, UAV4 is unable to localize itself without ambiguity based on the knowledge of positions of UAV0 and UAV2. But if the distance measurement between UAV3 and UAV4 is available, UAV4 can manage to determine its position by taking into account the estimated position of UAV3. In this way, UAV4 can localize itself even when it has less than three normal neighbors. In fact, this is achieved by propagation of the position knowledge of normal UAVs in the group. In this paper, we will formulate this knowledge propagation procedure in a statistical inference frameworks and propose a dynamic Nonparametric Belief Propagation algorithm for UAV localization, which treats the uncertainties in UAVs' state and their propagations in a systematical way and integrates all available information, including INS measurements on each UAV, GPS measurements on normal UAVs, and the distance measurement between pairs of UAVs, in a distributed manner.

There are several related works about the cooperative localization of UAVs. Qu et al. [8] proposed a UAV localization method based on information synchronization for a ring communication topological structure. Shames et al. [9] considered the problems of cooperative self-localization of mobile agents and proposed a solution based on the rigid graph theory. However, in the above approach, it is assumed that both the distance and the angle measurement are available, which is not true in many cases. Qu and Zhang [10] uses relative range measurements and GPS signals of 3 nearby healthy UAVs to cooperatively locate the UAVs with failure of GPS receiver. But it is not clear how to treat the ambiguity in localization when the neighbors of fault UAV include less than 3 healthy ones. There are also some works about the cooperative localization in sensor networks via inference on graphical model [11–13]. However, in these applications, the anchor nodes are static and fixed. This is in contrast with our work where the state of normal UAVs is also uncertain and the normal member may vary with time.

In this paper, we propose a novel approach for multiple UAVs cooperative localization; the main features of our work include the following.

• (1)

We formulate the cooperative localization as a probabilistic inference problem and propose a graphical model representing the joint distribution of involved random variables in UAVs group. The model is more flexible than [11–13]. In our model it allows the movement of anchors (here refer to the normal UAVs), and the variation of anchors member over time.

(2)

We propose a dynamic Nonparametric Belief Propagation algorithm (dNBP) for inference on the graphical model. By Belief Propagation, the posterior marginal of the position of fault UAV is calculated conditioned on all information made in the whole UAVs group up to current time. This is in contrast with traditional works, such as the works in [7, 10], where the fault UAV is located using only neighboring normal ones.

(3)

The proposed dNBP is a natural combination of traditional NBP with particle filter, which applies to dynamic case. In dNBP, posterior distribution of unobserved variables is approximated by set of representative samples, which makes it suitable for ring-shaped distribution produced by relative distance measurement; see Figure 4.

(4)

In dNBP, the distributions are updated by messages passing among UAVs and upon convergence; the position estimate of UAV is given by the belief state of the corresponding variables. This provides the basis for distributed implementation of information fusion in UAVs group.

(5)

We utilize several efficient sampling techniques in dNBP to reduce the computational time. Implemented by Matlab on PC in our lab, a single filtering step on individual UAV can be finished within few seconds.

This paper is organized as follows: in Section 2, we formalize the problem and discuss the types of uncertainty which occur in the localization problem. In Section 3, we present the dNBP algorithm in detail. Section 4 shows how the dNBP works in our simulations. Finally, we present a brief conclusion and the future plan.

2. Problem Formulation

In this section, we formulate the multi-UAV cooperative localization problem in a probabilistic framework. The position of the UAV is three-dimensional. But concerning the convenience of description, we will narrow the scope of position to two-dimensional geographical coordinates. However, it can also easily be extended to three-dimension occasion. We restrict our attention to the simplified cases in which each UAV is equipped with a 2-axis accelerometer and GPS sensor and assume it can obtain noisy distance measurements with respect to its nearby UAVs. It is also straightforward to extent to the UAV with more complex sensor configurations. Suppose that we have N UAVs in our system. The state of the sth UAV at time τ consists of its position and velocity $X_{s,\tau }=(x_{s,\tau },{\dot{x}}_{s,\tau })$ .

Under the above assumptions, the state of each UAV evolves according to the following linear Markov model:

$$\begin{array}{l}{x}_{s,\tau }=x_{s,\tau -\mathrm{1}}+{\dot{x}}_{s,\tau -\mathrm{1}},\\ {\dot{x}}_{s,\tau }={\dot{x}}_{s,\tau -\mathrm{1}}+u_{s,\tau }+v_{\tau },\end{array}$$

(1)

where u is the output of accelerometer and v is a zero-mean Gaussian process modeling the imprecision of accelerometer measurement. For normal UAV, at each time τ, a GPS observation is available. The observation model is

$$\begin{array}{cc}& z_{s,\tau }^n=x_{s,\tau }^n+w_{\tau },\end{array}$$

(2)

where w is also a Gaussian noise. Here, we use the superscript n to denote the normal UAV. Note that the set of normal UAVs may change with time due to the varying flight condition.

Moreover, we assume that with some probability $P_{\mathrm{0}}$ , which is a function of the relative distance, each UAV has some noisy measurement from its neighbors $t\in \Gamma (s)$

$$\begin{array}{cc}& \begin{array}{c}{d}_{st,\tau }=\parallel x_{s,\tau }-x_{t,\tau }\parallel +v_{st,\tau }\end{array}\hspace{1em}{v}_{st,\tau }~N\left(\mathrm{0},{\sigma }_v^{\mathrm{2}}\right),\end{array}$$

(3)

where $v_{st,\tau }$ is the distance measurement noise. Concerning the limitation of transmission technology, it is reasonable to assume that the probability of detecting nearby UAVs falls off with distance. Following [14], we model the decline of the probability as follows:

$$\begin{array}{cc}& \begin{array}{c}{P}_{\mathrm{0}}(x_{s,\tau },x_{t,\tau })=\exp \left\{-\frac{\mathrm{1}}{\mathrm{2}}{\parallel x_{s,\tau }-x_{t,\tau }\parallel }^{\mathrm{2}}/R^{\mathrm{2}}\right\}\end{array}.\end{array}$$

(4)

Here, R is a parameter specifying the range of the transmitter used for distance estimation.

We use y to denote the available observations, and $y_{\tau }\triangleq \{z_{s,\tau }^n,d_{st,\tau }\}$ . Our goal is to recursively calculate the posterior marginal distribution of $x_{s,\tau }$ conditioned on all observations up to current time, that is, $p(x_{s,\tau }\mid y_{\mathrm{1}:\tau })$ . We will use dNBP algorithm to solve this problem, which we will discuss in the following sections.

3. Inference Algorithm

3.1. Graphical Model

The graphical model for inference is shown in Figure 2. Each node is associated with a random variable representing the state of a single UAV at certain time $X_{s,\tau }$ . The node with superscript n corresponds to the normal UAV. Note that the set of normal nodes varies with time. The dashed arrows between time slice model the evolution of state across time $p(x_{s,\tau }\mid X_{s,\tau -\mathrm{1}})$ which is defined by the dynamic model (1). The edges within each time slice model the relationship among UAVs. An edge presenting between two nodes means that the relative distance measurement is available and the corresponding UAVs can communicate with each other. Note that the edges within each time slice appear according to the detection probability $P_{\mathrm{0}}$ and also vary across time.

Figure 2

Graphic model for inference in cooperative localization.

The problem of inference on the graphical model, that is, computation of the posterior distribution (belief) of state variable of each UAV, can be divided into two parts. First, is how to compute the posterior marginal distribution of the state on each node conditioned on observations from a single time slice. We will use NBP to solve this problem in Section 3.2. Second, is how to incorporate the dynamic information contained in dynamic model and perform tracking, that is, recursively calculate the belief state $p(x_{s,\tau }\mid y_{\mathrm{1}:\tau })$ . We will discuss this in Section 3.3.

3.2. Intraslice Update

In this section, we omit the subscript τ in expression for clarity. During intraslice update, the only involved state variable is the position $x_s$ . Note that the positions of different UAVs within a single time slice are correlated through the relative distance measurements according to the edge pattern shown in Figure 2. The joint distribution of the position of all UAVs in the group at current time is

$$\begin{array}{cc}& p\left(x\right)\propto {\displaystyle \prod }_{s\in V}{\psi }_s\left(x_s\right){\displaystyle \prod }_{\left(s,t\right)\in E}{\psi }_{s,t}\left(x_s,x_t\right),\end{array}$$

(5)

where V and E are the set of nodes and edges, and the local potential function for each node is defined as

$$\begin{array}{cc}& {\psi }_s\left(x_s\right)=\{\begin{array}{cc}p\left(z_s\mid x_s\right),\hfill & s\mathrm{\hspace{0.17em}\hspace{0.17em}}\text{is}\mathrm{\hspace{0.17em}\hspace{0.17em}}\text{normal},\hfill \\ \mathrm{1},\hfill & s\mathrm{\hspace{0.17em}\hspace{0.17em}}\text{is}\mathrm{\hspace{0.17em}\hspace{0.17em}}\text{fault},\hfill \end{array}\end{array}$$

(6)

where $p(z_s\mid x_s)$ is obtained from the GPS observation model (2). The pairwise potential function is defined as

$$\begin{array}{cc}& {\psi }_{s,t}\left(x_s,x_t\right)=p\left(d_{st}\mid x_s,x_t\right)\end{array}$$

(7)

which is obtained from the distance measurement model (3).

We will use the NBP algorithm [13] to calculate the marginal distributions of each UAV's position from the joint distribution (5). Nonparametric Belief Propagation is an iterative inference algorithm for graphic model, which is very effective for marginal distribution computation with respect to the global joint distribution. During each iteration, NBP consists of two steps: message propagation and belief computation.

For message propagation, the tth UAV sends a message $m_{ts}^n$ to its neighbors $s\in \Gamma (t)$ , which is defined as (here, n is the index for iteration)

$$\begin{array}{cc}& m_{ts}^n\left(x_s\right)\propto {{\displaystyle \int }}_{x_t}^{}{\psi }_{s,t}\left(x_s,x_t\right){\psi }_t\left(x_t\right){\displaystyle \prod }_{u\in \Gamma \left(t\right)\setminus s}{m}_{ut}^{n-\mathrm{1}}\left(x_t\right)d{x}_t.\end{array}$$

(8)

For belief computation, each node produces an approximation $p^n(x_s\mid y)$ to the true marginal distribution $p(x_s\mid y)$ by combining the incoming messages with the local potential

$$\begin{array}{cc}& p^n\left(x_s\mid y\right)\propto {\psi }_s\left(x_s\right){\displaystyle \prod }_{t\in \Gamma \left(s\right)}{m}_{ts}^n\left(x_s\right).\end{array}$$

(9)

After convergence, the position estimate of each UAV is given by the beliefs on the corresponding node. Exact evaluation of (8)-(9) involves computations which is not analytically tractable for most continuous variables. So, we represent $m_{ts}^n$ by a mixture of M Gaussian distributions

$$\begin{array}{cc}& m_{ts}\left(x_s\right)={\displaystyle \sum }_{i=\mathrm{1}}^M{w}_s^{i}N\left(x_s;x_s^i,{\Sigma }_s\right).\end{array}$$

(10)

The covariance ${\Sigma }_s$ is chosen as ${\Sigma }_s={\sigma }_v^{\mathrm{2}}{\xi }_{M}I$ , where ${\xi }_M$ is a constant calibrated offline to the number of samples M.

The message update equations (8) are performed using stochastic approximations in two stages: first, drawing samples $x_s^{(i)}$ from $p^{n-\mathrm{1}}(x_s)$ , then using these samples to approximate each outgoing message. We summarize the procedure in Algorithm 1. As $m_{ts}^n$ is a ring-shaped distribution, in Algorithm 1, we sample the message in the polar coordinates, with uniformly distributed orientation and normal radius perturbed from the noisy interdistance reading. Note that the detection probability $P_{\mathrm{0}}$ plays its role in sample weighing in line (3).

Algorithm 1: Computation of outgoing message.

Input: M weighted samples $\left\{w_s^{(i)},x_s^{(i)}\right\}$ from $p^{n-\mathrm{1}}\left(x_s\right)$ ; Output: an approximation to $m_{st}^n\left(x_t\right)$ for each neighbor $t\in \mathrm{\Gamma }\text{}\left(s\right)$

( $\mathit{1}$ )   Draw ${\theta }_{st}^{(i)}~U\text{}\left(\mathrm{0,2}\pi \right)$ , $v_{st}^{(i)}~N\left(\mathrm{0},{\sigma }_v^{\mathrm{2}}\right)$

( $\mathit{2}$ )   Means: $x_{st}^{(i)}=x_s^{(i)}+\left(d_{st}+v_{st}^{(i)}\right)\left[\sin \left({\theta }_{st}^{(i)}\right)\text{};\cos \left({\theta }_{st}^{(i)}\right)\right]$

( $\mathit{3}$ )   Weights: $w_{st}^{(i)}=P_{\mathrm{0}}\left(x_{st}^{(i)}\right)/m_{st}^{n-\mathrm{1}}\left(x_{st}^{(i)}\right)$

( $\mathit{4}$ )   Variance: ${\mathrm{\Sigma }}_{st}={\sigma }_v^{\mathrm{2}}{\xi }_{M}I$

Since each outgoing message is a Gaussian mixture with M components, estimation of the marginal distribution (belief) (9) is more difficult. Because it is the product of several Gaussian mixtures, computing (9) exactly is exponential in the number of incoming messages. Following [15], we use a technique called mixture importance sampling to compute the belief. As shown in Algorithm 2, we denote the set of neighbors of t having observed edges to t by ${\Gamma }_t^{\mathrm{0}}$ . We create a collection of kM weighted samples ( $k>1$ ) by drawing samples from each incoming message.

Algorithm 2: Belief computation.

Input: several Gaussian mixture messages $\left\{w_{ut}^{(i)},m_{ut}^{(i)},{\mathrm{\Sigma }}_{ut}\right\}$ , $u\in {\mathrm{\Gamma }}_t^{\mathrm{0}}$ ; Output: an approximation to belief $p^n\left(x_t\right)$

( $\mathit{1}$ )   For each observed neighbor $u\in {\mathrm{\Gamma }}_t^{\mathrm{0}}$

( $\mathit{2}$ )   Draw $kM/\left|{\mathrm{\Gamma }}_t^{\mathrm{0}}\right|$ samples $\left\{x_t^{(i)}\right\}$ from message $m_{ut}^n$

( $\mathit{3}$ )   Weight by $w^{(i)}={\psi }_t\left(x_t^{(i)}\right){\prod }_{u\in {\mathrm{\Gamma }}_t}{m}_{ut}^n\left(x_t^{(i)}\right)/\text{}{{\displaystyle \sum }}_{u\in {\mathrm{\Gamma }}_t}^{}{m}_{ut}^n\left(x_t^{(i)}\right)$

( $\mathit{4}$ )   From these $kM$ locations, re-sample by weight M times to produce M equal-weight samples.

3.3. Interslice Prediction and Tracking

The dynamic information is incorporated into inference by interslice prediction. For interslice prediction, each node sends a forward-time message to its corresponding node in the next time according to the dynamic mode

$$\begin{array}{cc}& m_{s,\tau ,\tau +\mathrm{1}}\left(X_{s,\tau +\mathrm{1}}\right)={\displaystyle \int }p\left(X_{s,\tau +\mathrm{1}}\mid X_{s,\tau }\right)p\left(X_{s,\tau }\mid y_{\mathrm{1}:\tau }\right)d{X}_{s,\tau }.\end{array}$$

(11)

The time-forward message summarizes our previous knowledge about UAV's state conditioned on all history observations. Under the mixture representation of message, the calculation in (11) can be implemented using Algorithm 3. In Algorithm 3, the covariance ${\Sigma }_{s,\tau +\mathrm{1}}$ is given by the estimate ${\Sigma }_{s,\tau +\mathrm{1}}=\text{ROT}(\{X_{s,\tau +\mathrm{1}}^{(i)},w_{s,\tau +\mathrm{1}}^{(i)},\})$ , which is proportional to the weighted covariance of the observed samples

$$\begin{array}{l}{\Sigma }_{s,\tau +\mathrm{1}}=M^{\left(-\mathrm{2}/\left(\delta +\mathrm{4}\right)\right)}{\displaystyle \sum }_{i=\mathrm{1}}^M{w}_{s,\tau +\mathrm{1}}^{\left(i\right)}\left(X_{s,\tau +\mathrm{1}}^{\left(i\right)}-{\stackrel{-}{X}}_{s,\tau +\mathrm{1}}^{}\right)\\ \times {\left(X_{s,\tau +\mathrm{1}}^{\left(i\right)}-{\stackrel{-}{X}}_{s,\tau +\mathrm{1}}^{}\right)}^T,\end{array}$$

(12)

where ${\stackrel{-}{X}}_{s,\tau +\mathrm{1}}^{}={\sum }_i{w}_{s,\tau +\mathrm{1}}^{(i)}{X}_{s,\tau +\mathrm{1}}^{(i)}$ is the weighted sample mean, and $\delta =\dim (X)$ .

Algorithm 3: Outgoing messages computation for tracking.

Input: M samples $X_{s,\tau }^{(i)}=\left\{x_{s,\tau }^{(i)},{\dot{x}}_{s,\tau }^{(i)}\right\}$ from marginal $p_{s,\tau }\left(X_{s,\tau }\right)$ ; Output: forward message $m_{s,\tau ,\tau +\mathrm{1}}\left(X_{s,\tau +\mathrm{1}}\right)$

( $\mathit{1}$ )   Means: $X_{s,\tau +\mathrm{1}}^{(i)}~p\left(X_{s,\tau +\mathrm{1}}\mid X_{s,\tau }^{(i)}\right)$

( $\mathit{2}$ )   Weights: $w_{s,\tau +\mathrm{1}}^{(i)}=\mathrm{1}/M$

( $\mathit{3}$ )   Variance: ${\mathrm{\Sigma }}_{s,\tau +\mathrm{1}}=ROT\left(\left\{X_{s,\tau +\mathrm{1}}^{(i)},w_{s,\tau +\mathrm{1}}^{(i)}\right\}\right)$

( $\mathit{4}$ )   Forward message: $m_{s,\tau ,\tau +\mathrm{1}}\left(X_{s,\tau +\mathrm{1}}\right)={{\displaystyle \sum }}_{i=\mathrm{1}}^M{w}_{s,\tau +\mathrm{1}}^{(i)}N\left(X_{s,\tau +\mathrm{1}};X_{s,\tau +\mathrm{1}}^{(i)},{\mathrm{\Sigma }}_{s,\tau +\mathrm{1}}\right)$ .

Taking the forward-time messages (11) as prior distribution of UAV's state, we can combine it with the NBP update for tracking. During tracking, at time τ, each UAV integrates the time-forward message from itself at previous time $\tau -\mathrm{1}$ , the messages from its neighbors at current time τ, and its local potential to compute its belief

$$\begin{array}{cc}& p\left(X_{s,\tau }\mid y_{\mathrm{1}:\tau }\right)\propto m_{s,\tau -\mathrm{1},\tau }\left(X_{s,\tau }\right){\psi }_s\left(x_{s,\tau }\right){\displaystyle \prod }_t{m}_{ts,\tau }^n\left(x_{s,\tau }\right),\end{array}$$

(13)

where $m_{ts,\tau }^n$ are obtained from NBP described in Section 3.2. Note that the time-forward message $m_{s,\tau -\mathrm{1},\tau }$ is a distribution over $X_{s,\tau }=(x_{s,\tau },{\dot{x}}_{s,\tau })$ , while the local potential ${\psi }_s$ and the intraslice message $m_{ts,\tau }$ are distributions over position $x_{s,\tau }$ . To calculate $p(X_{s,\tau }\mid y_{\mathrm{1}:\tau })$ , we first decompose the time-forward message into marginal position distribution and the conditional distribution of velocity given position

$$\begin{array}{cc}& m_{s,\tau -\mathrm{1},\tau }\left(X_{s,\tau }\right)=m_{s,\tau -\mathrm{1}}\left({\dot{x}}_{s,\tau }\mid x_{s,\tau }\right)m_{s,\tau -\mathrm{1}}\left(x_{s,\tau }\right).\end{array}$$

(14)

Then we calculate the posterior distribution over $x_{s,\tau }$ in (13), which is a product of density, using the same technique in Algorithm 2. Finally, the resulting position estimates are transferred to velocity via the conditional distribution in (14). The remaining issue is how to perform the decomposition in (14). Note that the time-forward message is a Gaussian mixture. Because the conditional distribution of a subset of jointly Gaussian variable is Gaussian, conditional distributions of Gaussian mixture are mixture of lower dimensional Gaussian. Consider the ith component in the joint mixture with weight $w_{s,\tau }^{(i)}$ , the corresponding mean and covariance are

$$\begin{array}{cc}& {\mu }_X^{\left(i\right)}=\left[\begin{array}{c}{\mu }_x^{\left(i\right)}\\ {\mu }_{\dot{x}}^{\left(i\right)}\end{array}\right],\hspace{1em}{\Sigma }_X^{\left(i\right)}=\left[\begin{array}{cc}{\Sigma }_{x,x}^{\left(i\right)}& {\Sigma }_{x,\dot{x}}^{\left(i\right)}\\ {\Sigma }_{\dot{x},x}^{\left(i\right)}& {\Sigma }_{\dot{x},\dot{x}}^{\left(i\right)}\end{array}\right].\end{array}$$

(15)

The marginal $m_{s,\tau -\mathrm{1}}(x_{s,\tau })$ has the weight $w_{s,\tau }^{(i)}$ , mean ${\mu }_x^{(i)}$ , and covariance ${\Sigma }_{x,x}^{(i)}$ . From standard formulas [16], the conditional density $m_{s,\tau -\mathrm{1}}({\dot{x}}_{s,\tau }\mid x_{s,\tau })$ has the mean, variance, and weight as

$$\begin{array}{cc}& {\mu }_{\dot{x}|x}^{\left(i\right)}={\mu }_{\dot{x}}^{\left(i\right)}+{\Sigma }_{\dot{x},x}^{\left(i\right)}{\left({\Sigma }_{x,x}^{\left(i\right)}\right)}^{-\mathrm{1}}\left(x^{\left(i\right)}-{\mu }_x^{\left(i\right)}\right),\\ {\Sigma }_{\dot{x}|x}^{\left(i\right)}={\Sigma }_{\dot{x},\dot{x}}^{\left(i\right)}-{\Sigma }_{\dot{x},x}^{\left(i\right)}{\left({\Sigma }_{x,x}^{\left(i\right)}\right)}^{-\mathrm{1}}{\Sigma }_{x,\dot{x}}^{\left(i\right)},\\ w_{\dot{x}|x}^{\left(i\right)}\propto w_{s,\tau }^{\left(i\right)}\left(\frac{\left|{\Sigma }_{\dot{x}|x}^{\left(i\right)}\right|}{\left|{\Sigma }_X^{\left(i\right)}\right|}\right)\exp \left\{-\frac{\mathrm{1}}{\mathrm{2}}{\sigma }_{\mathrm{0}}^T{\left({\Sigma }_X^{\left(i\right)}\right)}^{-\mathrm{1}}{\sigma }_{\mathrm{0}}^{}\right\},\end{array}$$

(16)

where ${\sigma }_{\mathrm{0}}^{}$ is defined as

$$\begin{array}{cc}& {\sigma }_{\mathrm{0}}^{}=\left[\begin{array}{c}{x}_{s,\tau }^{\left(i\right)}-{\mu }_x^{\left(i\right)}\\ {\mu }_{\dot{x}|x}^{\left(i\right)}-{\mu }_{\dot{x}}^{\left(i\right)}\end{array}\right].\end{array}$$

(17)

The whole tracking algorithm is summarized in Algorithm 4. For normal UAVs, (13) is just a slightly modified particle filtering. By combining information provided by temporal dynamic and GPS observation, normal UAVs can give more reliable position information and act as anchor nodes during cooperative localization in UAVs group. The fault UAVs use (13) to combine temporal information and distance measurement, which can greatly improve their localization accuracy.

Algorithm 4: Marginal distribution computation for tracking.

Input: incoming messages $m_{ts,\tau }^{}$ , $m_{s,\tau -\mathrm{1},\tau }^{}$ ; Output: an approximation to marginal distribution $p\left(X_{s,\tau }\right)$

( $\mathit{1}$ )   Decompose the Gaussian mixture: $m_{s,\tau -\mathrm{1},\tau }(X_{s,\tau })=m_{s,\tau -\mathrm{1}}\left(x_{s,\tau }\right)m_{s,\tau -\mathrm{1}}\left({\dot{x}}_{s,\tau }\mid x_{s,\tau }\right)$

( $\mathit{2}$ )   Draw $kM/\mathrm{2}\left|\mathrm{\Gamma }(s)\right|$ samples $\left\{x_{s,\tau }^{(i)}\right\}$ from each incoming messages $m_{ts,\tau }^{}$ ;

( $\mathit{3}$ )   Draw $kM/\mathrm{2}$ samples from $\left\{x_{s,\tau }^{(i)}\right\}$ from each incoming messages $m_{s,\tau -\mathrm{1}}\left(x_{s,\tau }\right)$

( $\mathit{4}$ )   Weight each of the kM samples by

      $w_{s,\tau }^{(i)}={\psi }_s\left(x_{s,\tau }^{(i)}\right)\frac{m_{s,\tau -\mathrm{1}}\left(x_{s,\tau }^{(i)}\right){\prod }_{t\in \mathrm{\Gamma }(s)\hspace{0.17em}}{m}_{st}(x_{s,\tau }^{(i)})}{m_{s,\tau -\mathrm{1}}(x_{s,\tau }^{(i)})+{{\displaystyle \sum }}_{t\in \mathrm{\Gamma }(s)}^{}{m}_{st}(x_{s,\tau }^{(i)})}$

( $\mathit{5}$ )   Resample from these $kM$ samples, producing $kM$ equally weighted samples $\left\{x_{s,\tau }^{(i)}\right\}$

( $\mathit{6}$ )   Draw kM samples ${\dot{x}}_{s,\tau }^{(i)}~m_{s,\tau -\mathrm{1}}\left({\dot{x}}_{s,\tau }\mid {\widehat{x}}_{s,\tau }\right)$ , with ${\dot{w}}_{s,\tau }^{(i)}=\mathrm{1}/kM$ , producing kM equally weighted samles $X_{s,\tau }^{(i)}=\left\{x_{s,\tau }^{(i)},{\dot{x}}_{s,\tau }^{(i)}\right\}$

( $\mathit{7}$ )   Resample M samples from theses $kM$ samples $X_{s,\tau }^{(i)}$

3.4. Discussion

In practice, we find that, in the intraslice update, if the traditional parallel message passing schedule is used, in which every node transmits message to its neighbors in parallel with each iteration, the convergence behavior of NBP is very unsatisfactory. This is mainly because the message passing from the fault UAV to normal one, which may contain significant bias from true distribution, may corrupt the belief of the normal UAV and slow down the convergence of the algorithm. Therefore, in our implementation, as in [12], we use a modified message passing schedule in which a normal node never receive messages from fault ones, and any fault node only transmits an outgoing message if it has receive at least three incoming messages. In situation where no fault UAV has three incoming messages, the threshold drops to two messages and then finally drops to one.

It should be noticed that, in the dNBP algorithm presented above, there are only two kinds of operations: the local information processing on individual UAV and the mutual message exchanges between neighboring UAVs. So, it is a fully distributed algorithm and can scale well for large UAV group. For a single UAV, the most computational demanding portions are the sampling from Gaussian mixture and evaluating Gaussian mixture at a particular location. Both require $O(kM\left|E\right|)$ operations per timestep, where $\left|E\right|$ is the number of observed interdistance measurements. And the parameters k and M correspond to the number of samples used to represent the distributions. Note that we can further reduce the computational burden by only updating belief using GPS and inter-distance measurement every N timesteps, as the INS navigation is accurate enough for short time.

4. Results

4.1. Case Study

In this section, we consider a typical scenario to illustrate how our algorithm works for the cooperative localization of multiple UAVs.

As shown in Figure 3, the multi-UAVs team consists of 3 normal UAVs and 3 fault UAVs. $X^F$ and $X^N$ stand for the state of fault UAV and normal UAV, respectively. The solid edges between nodes represent messages passing between UAVs, the arrows represent the passing direction, and associated number of each direct edge indicates message passing order according to the schedule described in Section 3.4. For example, a directed edge from node 1 to node 4 with number 0 means that a message $m_{\mathrm{14}}^{\mathrm{0}}$ is sent from UAV1 to UAV4 at the 0th iteration of NBP at current timestep. The dashed arrows represent passing of time-forward messages (11) of individual UAVs. For clearness, we only show the arrow for UAV6.

Figure 3

Illustration of the dNBP.

Figure 4

Cooperative localization via dNBP.

Let us first consider the intraslice update in time t. Suppose at time t that UAV1, UAV2, and UAV3 are normal, and UAV4, UAV5, and UAV6 have no knowledge about their location. Thus at the iter = 0 of NBP, UAV4 receives messages from UAV1, UAV2, and UAV3. UAV5 receives messages from UAV 1 and UAV 2. UAV 6 just receives message from UAV 3. For UAV4, it can locate itself using the messages coming from the other 3 UAVs shown in Figure 4(a), where three blue ring-shaped distributions represent the messages from its neighbors (8) calculated by Algorithm 1 and the red single-modal distribution stands for its belief (position estimation) (9) calculated by Algorithm 2 concerning all the three incoming messages. From Figure 4(a), we can see that after one iteration the position of UAV4 is located successfully without ambiguity, while due to the lack of the incoming messages, the estimations of UAV5 and UAV6 represent two-modal distribution (Figure 4(b)) and ring-shaped distribution (Figure 4(c)), respectively.

At the iter = 1, UAV4 will transmit outgoing messages to UAV5 and UAV6, as shown in Figure 3. With the incoming message sent by UAV4, UAV5 is able to locate itself by combining the messages from the other two normal UAVs (UAV1 and UAV2) as shown in Figure 4(d). For UAV6, using message coming from UAV4 enables it to improve the estimation accuracy from ring-shaped distribution to multimodal distribution shown in Figure 4(e). At iter = 2, both UAV4 and UAV5 are able to send outgoing messages according to the message update schedule. Combining incoming messages from UAV3, UAV4, and UAV5, the localization of UAV6 is accomplished, with distribution changing from multimodal to single modal, as shown in Figure 4(f). In this case, all fault UAV can localize itself without ambiguity after 3 iterations of NBP.

Now, let us turn to the dynamic case. At time t, we use the linear Markov model (1) to calculate the state predictive distribution (11) of all UAVs at time $t+1$ using Algorithm 3. For normal UAVs, this distribution is combined with GPS observation in $t+1$ to improve the position accuracy, which is in fact a particle filtering process. For fault UAVs, the predictive distribution is combined with incoming messages sent from its neighbors in $t+1$ for its localization. In this way, dNBP reduces the ambiguity of fault UAV's position estimate by integrating the information provided by both temporal dynamics and distance measurements at time $t+1$ . Take UAV6 as an example. Suppose at time $t+1$ that UAV6 can only communicate with UAV3 and UAV1, as shown in Figure 3. Figure 4(h) shows the ambiguity, in which green two-modal distribution represents the position estimation of UAV6, just considering messages coming from its two neighbors. But UAV6 can be located correctly at time t, and the predictive distribution of UAV6 at $t+1$ is shown in Figure 4(g). In dNBP, we combine the information in Figures 4(g) and 4(h) using Algorithm 4, resulting the final belief of UAV6's position shown in Figure 4(i). We can see that the belief in Figure 4(i) has reduced ambiguity (from two-modal to single-modal) compared with distributions in Figure 4(g) and decreased covariance (from 29.6 to 14.6) compared with predictive distribution in Figure 4(h).

4.2. Monte Carlo Simulation

In this section, we will demonstrate the performance of our method implemented in Matlab by Monte Carlo simulations and compare it with the traditional cooperative localization method in [7], where the fault UAVs are located based on nearby normal ones only.

We consider the case in which $N=36$ UAVs are flying in 2-dimensional space during t = 1 : 100. The true trajectories of each UAV are generated independently according to the pre-specified acceleration sequence with random initial position sampled in 500 × 500 area. We model the GPS state of each UAV with a first-order Markov process, switching between being fault and normal according to the following transition matrix:

$$\begin{array}{cc}& \left[\begin{array}{cc}\mathrm{0.9}& \mathrm{0.1}\\ \mathrm{0.1}& \mathrm{0.9}\end{array}\right].\end{array}$$

(18)

For normal UAV, the GPS observations are generated according to (2) and standard derivation of Gaussian is set to 10 meters. For any pair of UAVs, we first determine whether there exists a communication link based on the detection probability (4), where the transmitter range R is typically set to 300 meters. Then the inter distance observation is produced by (3), and the standard derivation is set to 3 meters. The accelerometer noise is set to 0.5 meter/sec².

In each simulation, the true trajectories of UAVs and observations data are generated as above, and then our dNBP and traditional cooperative localization algorithm [7] are applied. The main performance criterion is the mean position estimate error, which is defined as

$$\begin{array}{l}ME\left(\tau \right)=\frac{\mathrm{1}}{N}{\displaystyle \sum }_s\parallel {\widehat{x}}_{s,\tau }-x_{s,\tau }\parallel ,\\ ME=\frac{\mathrm{1}}{NT}{\displaystyle \sum }_{\tau }{\displaystyle \sum }_s\parallel {\widehat{x}}_{s,\tau }-x_{s,\tau }\parallel .\end{array}$$

(19)

Firstly, we compare our dNBP and traditional localization method under above parameters setting. Figure 5 shows the mean position estimate error varying with time in a typical run. The upper panel depicts the mean estimate error of traditional localization algorithm and dNBP. The lower panel depicts the proportion of the fault UAVs in the total UAVs group at each time instance. It is clear in Figure 5 that our method can improve the estimate accuracy significantly, especially in the case where the number of fault UAVs is large. This can be attributed to the fact that, in our method, by using message passing, the posterior distribution of each fault UAV's position is calculated conditioned on all history observations (i.e., the GPS measurements of normal UAVs and relative distance measurements between neighboring UAVs) made in the whole UAVs group. In contrast, in traditional method, the position of fault UAV is estimated based on the states of neighboring normal ones. But when the number of normal UAVs is small, such as during a time from 40 to 60 in Figure 5, there are only 12 normal UAVs in the group of 36 UAVs, and many fault UAVs have less than 3 normal neighbors. In this case, traditional method gives poor estimate of the fault UAV's position. It can also be seen from Figure 5 that, at the initial section ( $t<5$ ), traditional method is superior to dNBP slightly. The reason is as follows. In our algorithm, the state of UAV includes its velocity, allowing incorporation of information made by accelerometer into the inference process. So, the mean error curve of dNBP is much smoother than that of traditional method, as shown in Figure 5. However, at the initial phase, the velocity is not estimated accurately as we have no direct measurement of velocity, and so does the predicted position given by the dynamic model. Consequently, the inaccurate position prediction deteriorates the posterior position estimate, resulting in a higher estimate error of dNBP than traditional method in this case. But as the accuracy of velocity increases, it is clear that, after $t>5$ , our method outperforms traditional ones in general.

Figure 5

Mean estimate error versus time.

The localization performance is influenced by many factors, including the number of normal UAVs in the group, the UAV communication range, and the noise in GPS and relative distance measurements. Next, we investigate the behavior of the localization algorithms under varying operating conditions by changing one factor and fixing others. The criterion statistics in all results below are summarized from 100 Monte Carlo simulations.

As mentioned before, the normal UAVs serve as anchors during cooperative localization. Thus, we are interested in what happens in dNBP and traditional algorithms when the number of normal UAVs changes. Figure 6(a) shows the results. The x-axis is defined as the ratio of normal UAV number to total number. In these simulations, we disable the switching of GPS state to keep the number of normal UAVs fixed. We can see from Figure 6(a) that the performance of traditional cooperative localization algorithm is heavily dependent on the number of normal UAVs. When the normal UAVs become sparse in the team, many fault UAVs cannot locate themselves due to the lack of normal neighbors, resulting in the rapid decline in localization accuracy. In contrast, in dNBP, the knowledge of normal UAVs can propagate throughout the whole networks by Belief Propagation and the fault UAV can locate itself successfully even when no normal UAV exists nearby. However, in order to keep all the position estimations of the fault UAVs correct, having at least three normal UAVs in the team is a must.

Figure 6

Performance comparison. (a) Mean estimate error versus proportion of normal UAVs. (b) Mean estimate error versus communication range. (c) Mean estimate error versus GPS noise. (d) Mean estimate error versus relative distance noise.

The transmitter range R is another important factor influencing the algorithm performance. The specific connection pattern of the UAVs group at any timestep is determined by the actual position of UAVs and their possible communication distance. We observe the behavior of dNBP and traditional algorithm when UAV's transmitter range is varying and we depict them in Figure 6(b). It is clear that, when R is small, both algorithms perform very poorly due to the lack of information for localization. On the other hand, when R is large enough, all UAVs in the group can connect to at least 3 normal ones. Thus, both algorithms show good performance. But it is noticeable that dNBP outperforms traditional algorithm significantly when R is varying between 200 and 350 meters. In fact, in dNBP, the position knowledge can be exchanged between two unconnected UAVs by a sequence of short range knowledge propagations. In Figure 6(b), when the communication range is large enough (>500 m), almost each pair of UAVs in the group can exchange information mutually. Thus, it can be ensured that there are at least three normal ones in the neighborhood of each fault UAV. In this case, the least square method can obtain the optimal estimate. So, when the communication range is larger than 500 m, as shown in Figure 6(b), traditional method performs better than dNBP.

We also compare the performance of dNBP and traditional method under varying GPS and relative distance measurement noise. The results are shown in Figures 6(c) and 6(d), where the x-axis stands for standard derivation of GPS and relative distance measurement noise, respectively. As expected, the estimate error increases with the observation noise, and dNBP is always better than traditional one.

As discussed before, in dNBP, the messages are represented by a set of samples. The more the samples used, the more the representation and hence the inference are accurate. The dNBP is a distributed algorithm in which the computational burden is distributed to individual UAVs in the group. For each UAV, the computing time is determined by the number of samples used for representing messages and the number of its neighbors. Table 1 shows the tradeoff between estimate accuracy and computational complexity. The time in the table refers to the average time consumed by individual fault UAV in a single filtering step, including an intraslice update and an interslice prediction. We can see that using a highly unoptimized Matlab code, satisfactory localization result (error of 3 meters) can be achieved within few seconds using 300 samples for message representation. By code optimization on embedded system, reduction in measurements update frequency, and constraint in communicating neighbors, the computing time can be further reduced. This provides the basis for large scale real-time application.

Table 1

Tradeoff between accuracy and complexity.

M	100	200	300	400	500	1000
Time (s)	1.9	4.5	7.8	15.9	17.6	50
$M\mathrm{E\hspace{0.17em}\hspace{0.17em}}$ (m)	12.21	6.21	3.02	2.40	2.09	1.63

5. Conclusion

In this paper, we propose a novel approach for cooperative localization of multiple UAVs using dNBP. The proposed algorithm can locate UAV with fault GPS successfully when the communication range is short and GPS observations in the UAVs group are sparse. In practice, the motions of different UAVs in the team are always correlated. In the future, we plan to incorporate this correlation into the graphical model and develop new inference algorithm for improving the localization performance further.