3D Hybrid Localization Algorithm for Mitigating NLOS Effects in Flying Ad Hoc Networks

Abstract

Positions of unmanned aerial vehicles (UAVs) are typically obtained using the global positioning system (GPS). However, in GPS-denied or GPS-degraded environments, ad hoc networks with flying sensor nodes are used for UAV localization. In this study, we propose a novel three-dimensional (3D) localization algorithm for UAVs in flying ad hoc sensor networks. Interacting multiple model probability data association and finite impulse response filters are integrated in our hybrid localization algorithm. The non-line-of-sight condition can be overcome using the proposed algorithm, which is demonstrated through 3D localization simulations based on flying ad hoc networks.

1. Introduction

Unmanned aerial vehicles (UAVs) have been widely used for various industrial and personal purposes such as delivery of packages, spraying pesticides and aerial photography [1,2]. Recently, autonomous flight control for UAVs has been intensively studied by researchers, for which accurate position information of the UAV is essential. Positioning of UAVs can be achieved using the global positioning system (GPS). However, GPS is often not available in indoor spaces or may not provide sufficiently accurate positioning data, owing to multipath effects in urban environments or ionospheric effects in polar regions [3,4]. Thus, localization systems based on flying ad hoc networks (FANETs) have been attracting the attention of researchers, for use in GPS-denied or GPS-degraded environments [5,6,7,8].

FANET uses UAVs as aerial sensor nodes that perform wireless communication. Various kinds of wireless communication technologies can be used for FANET-based localization systems, such as time of arrival (TOA), time difference of arrival (TDOA), angle of arrival (AOA) and received signal strength (RSS). In urban environments, wireless signals may be obstructed by buildings, which can degrade the localization accuracy [9]. Such conditions where the transmitter and receiver are not in direct visual line-of-sight are called the non-line-of-sight (NLOS) conditions. In localization systems based on wireless communications, NLOS situations frequently occur and cause significant problems [9,10,11]. Thus, it is essential to develop localization algorithms that are robust against NLOS effects.

Localization algorithms estimate the coordinates of targets through wireless signal measurements. However, because the measurements are often contaminated by noise and NLOS errors, stochastic (i.e., time-domain) filtering algorithms are commonly used to mitigate the effects of noise and NLOS errors [12,13,14,15]. Stochastic filters require state-space models, which include motion and measurement models. The wireless measurements, including the TOA, are expressed in nonlinear measurement models and hence nonlinear filters such as the extended Kalman filter (EKF) and particle filter are typically used [3,16]. The constant velocity (CV) model is one of the widely used motion models [17,18] and the interacting multiple model (IMM) algorithm is appropriate for tracking rapidly maneuvering targets such as UAVs [11,19]. The probabilistic data association (PDA) algorithm is used for tracking targets in the presence of false measurements [20].

In this study, we propose a novel 3D localization algorithm based on the EKF, IMM and PDA algorithms, for mitigating NLOS effects in FANET. Existing algorithms for NLOS mitigation focus on identifying NLOS situations and are based on classification or artificial intelligence algorithms [21,22,23]. However, the proposed algorithm is aimed at reinforcing the robustness of the localization algorithm against NLOS effects. Thus, existing NLOS identification algorithms and the proposed localization algorithm can be used together. The proposed algorithm has a unique hybrid filtering framework that consists of two heterogeneous filters. The main filter is composed of EKF, IMM and PDA algorithms and is called the IMM-PDA filter (IMM-PDAF) [24,25,26]. This filter is in charge of localization in normal situations. At least four TOA measurements are required for trilateration in a 3D space, but we employ more than four anchor nodes (ANs). In addition, we select small TOA measurements at each time step, which increases the probability of avoiding the measurements contaminated by NLOS effects because NLOS conditions typically increase the TOA measurements owing to multipath effects. We call the selected small measurements the nearest neighbor (NN) measurements. Next, $C_4^n$ measurement sets are constructed by combining four measurements from n NN measurements. The constructed measurement sets are input to the IMM-PDAF. Because the IMM-PDAF is robust against false measurements, it will also be robust against the TOA measurement set contaminated by NLOS effects. However, the IMM-PDAF has an infinite impulse response (IIR) structure and accumulates localization errors over time. Thus, we exploit a stochastic finite impulse response (FIR) filter [27] as an assisting filter. The FIR filter does not accumulate errors and can be used without initialization [28,29,30,31,32,33]. The assisting FIR filter operates only when the main filter fails and it resets the main filter to enable it to recover from failure. The proposed filter is therefore referred to as a hybrid IMM-PDA/FIR filter (HIPFF). Three-dimensional (3D) localization simulations are conducted to demonstrate the performance of the proposed localization algorithm based on HIPFF. In the simulations, aerial sensor nodes are configured using eight UAVs and the 3D coordinates of a target UAV are estimated using localization algorithms. We compare the proposed algorithm with the localization algorithms based on IMM-EKF and IMM-PDAF. First, we show the effectiveness of the IMM algorithm by comparing the IMM-EKF and the ordinary EKF. Second, we show the effect of constructing the measurement set using four TOA measurements in NLOS situations. Third, the ability to recover from failure is demonstrated by comparing the HIPFF and pure IMM-PDAF. Finally, the overall performance of the HIPFF-based localization is evaluated in comparison with the localizations based on IMM-EKF and IMM-PDAF.

The main contributions of this study are summarized as follows.

• A novel 3D localization algorithm that can mitigate NLOS effects is proposed. The proposed algorithm deals with the measurements contaminated by NLOS bias errors as clutters (false measurements), whereas the existing algorithms identify and discard the NLOS measurements.
• The proposed algorithm significantly improves the localization accuracy without the risk of false alarm occurring in the NLOS identification.
• The unique hybrid filtering framework, in which the assisting FIR filter recovers the main IMM-PDAF from failures, significantly improves the reliability of the localization algorithm under NLOS conditions.

The remainder of this paper is organized as follows. Previous works to solve the NLOS problem in localization are introduced in Section 2. The FANET-based 3D localization scheme is described and the NLOS effect is explained in Section 3. Mitigation of the NLOS effects using the NN measurements is explained in Section 4, in which the localization algorithms using NN measurements, based on IMM-EKF and IMM-PDAF, are explained. The HIPFF-based 3D localization algorithm is proposed in Section 5. Simulation results that demonstrate the superior performance of the proposed algorithm are presented in Section 6. Finally, the conclusions are drawn in Section 7.

2. Related Works

NLOS situations have been serious problems in localization and various approaches to the NLOS problem have been proposed. The most common approach is identifying the ANs in the NLOS situation and discarding the measurements from them. In [34], a residual test algorithm was proposed, in which the squares of the normalized residuals are computed and a threshold test based on the chi-square distribution is carried out to identify the NLOS ANs. In [35], a method for selecting best combination of ANs to reduce localization uncertainties was proposed. In [36], a maximum likelihood (ML)-based method was proposed, in which several hypotheses for different sets of ANs are constructed and the best hypothesis is selected using the ML principle. Recently, NLOS identification based on artificial intelligence algorithms, such as the convolutional neural networks and deep learning, have been intensively studied [21,22,23]. The NLOS identification approaches can achieve significant improvement in localization accuracy when the identification is correct, but there always exists the possibility of false alarm (i.e., identifying an LOS AN as NLOS) and the consequent performance degradation. Moreover, useful information can be lost in the process of discarding NLOS measurements.

Alternative approaches to the NLOS problem have been studied in the field of robust filtering. The Huber M-estimators [13,37,38] are stochastic filters that can suppress the effect of outlier measurements and outperforms the conventional least square (LS) estimators under NLOS conditions. In [15], an H∞ filter that is tolerant of NLOS errors was proposed and applied to the mobile robot localization using wireless sensor networks. In other approaches [11,39], estimators were derived based on the assumption that the measurement noise follows a skewed t-distribution, instead of Gaussian distribution. Because the robust filtering approaches do not discard NLOS measurements, there is no possibility of false alarm. However, it is difficult to achieve significant performance improvement by using them and a priori information or assumptions about the measurement noise are required.

There are different approaches to the NLOS problems in the UAV localization. In [40], the NLOS mitigation algorithm based on the combination of TDOA and AOA measurements was proposed. In [9,41], the NLOS problem was overcome by carefully designing the location (or trajectory) of the flying ANs. In [42], a composite filtering algorithm, which integrates a disturbance observer and a skew-t variational Bayes filter, was proposed for NLOS mitigation in UAV localization.

The HIPFF-based 3D localization algorithm proposed in this study pursues both avoiding NLOS measurements and enhancing the robustness against NLOS errors. The proposed algorithm utilizes redundant ANs and NN measurements, instead of identifying NLOS ANs. This increases the possibility of avoiding NLOS measurements without the false alarm and consequent performance degradation. Moreover, the proposed algorithm utilizes the NLOS measurements as clutters, which reduces the loss of useful information. The PDA algorithm has shown excellent performance in dealing with false measurements. Thus, the proposed algorithm based on the PDA can achieve significant improvements in performance under NLOS conditions.

3. 3D Localization Using Flying Ad Hoc Network

In this section, the 3D localization scheme using FANET and the NLOS problem are explained. Figure 1 shows the geometry of a 3D localization system using FANET in which flying UAVs are in charge of aerial ANs. It is assumed that the UAV ANs can obtain their 3D coordinates by using a real-time kinematic (RTK) GPS and that they are equipped with ultra-wideband (UWB) transmitters [3,4]. The 3D coordinates of the ANs are expressed as $(x_i,y_i,z_i)$, where $i=1,2,\cdots ,N$. The mobile node is also a UAV and can receive the UWB signals transmitted from the ANs. The TOA measurements can be obtained by computing the difference between the transmission and reception times. The 3D coordinate of a mobile node is denoted by $(x,y,z)$ and the TOA measurements are

$$\begin{array}{cc}\hfill z_i& =\frac{1}{c}{d}_i+v,\hfill \\ \hfill d_i& =\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2},(i=1,2,3,\cdots ,N),\hfill \end{array}$$

(1)

where $z_i$ is the TOA obtained from the i-th AN, c is the speed of light, $d_i$ is the distance between the mobile node and the i-th AN and v is a zero-mean white Gaussian measurement noise with variance $r^2$.

Under NLOS conditions, the wireless signal cannot travel along a direct path between the transmitter and receiver. Instead, the signal travels along indirect paths through reflection, which causes bias errors in TOA measurements. The TOA measurement contaminated by the NLOS error can be expressed as

$$\begin{array}{cc}\hfill {\overline{z}}_i& =\frac{1}{c}{d}_i+v+b,\hfill \end{array}$$

(2)

where b is the bias error that follows a Gaussian distribution with mean ${\mu }_b$ and variance ${\sigma }_b^2$. In this study, we assume that the localization algorithm cannot identify a TOA measurement contaminated by NLOS error. Thus, (2) is used only for generating measurements contaminated by NLOS errors, in the simulation. When designing a localization algorithm, the pure TOA measurement Equation (1) is used.

Stochastic filters require motion and measurement models. A discrete-time motion model represents the transition of the state vector between time instances. In the CV motion model, the state vector at time step k is constructed as ${\mathbf{x}}_k={\left[x_k{y}_k{z}_k{\dot{x}}_k{\dot{y}}_k{\dot{z}}_k\right]}^T$, where $(x_k,y_k,z_k)$ and $({\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k)$ are the 3D positions and velocities, respectively. The 3D CV model can be expressed as

$$\begin{array}{cc}\hfill {\mathbf{x}}_k& =\mathbf{F}{\mathbf{x}}_{k-1}+\mathbf{G}{\mathbf{w}}_k,\hfill \\ \hfill \mathbf{F}& =\left[\begin{array}{cccccc}1& 0& 0& T& 0& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],\mathbf{G}=\left[\begin{array}{ccc}{T}^2/2& 0& 0\\ 0& T^2/2& 0\\ 0& 0& T^2/2\\ T& 0& 0\\ 0& T& 0\\ 0& 0& T\end{array}\right],\hfill \end{array}$$

(3)

where T is the sampling interval and ${\mathbf{w}}_k$ is the process noise that is assumed to be a zero-mean white Gaussian noise. The covariance of ${\mathbf{w}}_k$ is $\mathbf{Q}=q^2{\mathbf{I}}_3$ where $q^2$ is the variance and ${\mathbf{I}}_3$ is a $3\times 3$ identity matrix.

The measurement model is a representation of the relationship between the measurement and state vectors. Estimation of the 3D coordinates of a mobile node requires at least four TOA measurements. If we use N TOA measurements, where $N\ge 4$, the measurement vector ${\mathbf{z}}_k$ is constructed as ${\mathbf{z}}_k={[z_{1,k}{z}_{2,k}\cdots z_{N,k}]}^T$. Then, the TOA measurement model can be written as

$$\begin{array}{cc}\hfill {\mathbf{z}}_k& =\mathbf{h}\left({\mathbf{x}}_k\right)+{\mathbf{v}}_k,\hfill \end{array}$$

(4)

where ${\mathbf{v}}_k$ is the zero-mean white Gaussian measurement noise and its covariance is $\mathbf{R}=r^2{\mathbf{I}}_N$. $\mathbf{h}(·)$ is the vector representation of the nonlinear function in (1).

Given the motion and measurement models, the state vector ${\mathbf{x}}_k$ containing the 3D positions can be estimated using stochastic filters.

For instance, the EKF estimates the state vector at each time step k using the following equations:

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k-1}& =\mathbf{F}{\widehat{\mathbf{x}}}_{k-1|k-1},\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{k|k-1}& =\mathbf{F}{\mathbf{P}}_{k-1|k-1}{\mathbf{F}}^T+\mathbf{G}\mathbf{Q}{\mathbf{G}}^T,\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill {\mathbf{K}}_k& ={\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T{({\mathbf{H}}_k{\mathbf{P}}_{k|k-1}{\mathbf{H}}_k^T+\mathbf{R})}^{-1},\hfill \end{array}$$

(7)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k}& ={\widehat{\mathbf{x}}}_{k|k-1}+{\mathbf{K}}_k({\mathbf{z}}_k-{\mathbf{H}}_k{\widehat{\mathbf{x}}}_{k|k-1}),\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{k|k}& =({\mathbf{I}}_6-{\mathbf{K}}_k{\mathbf{H}}_k){\mathbf{P}}_{k|k-1},\hfill \end{array}$$

(9)

where ${\mathbf{P}}_{k|k-1}$ and ${\mathbf{P}}_{k|k}$ are the estimation error covariances; ${\mathbf{K}}_k$ is the Kalman gain; the subscripts $k|k-1$ and $k|k$ denote a priori and a posteriori, respectively; and ${\mathbf{H}}_k$ is the Jacobian matrix, which is defined as

$$\begin{array}{cc}\hfill {\mathbf{H}}_k& ={\left.\frac{\partial \mathbf{h}}{\partial \mathbf{x}}\right|}_{{\widehat{\mathbf{x}}}_{k|k-1}}.\hfill \end{array}$$

(10)

The current location of a mobile node can be obtained by extracting the 3D coordinates from the a posteriori estimated state vector ${\widehat{\mathbf{x}}}_{k|k}$.

4. Interacting Multiple Model Filtering and Nearest Neighbor Measurements

The CV motion model (3) assumes that a target moves with a nearly constant velocity in a sampling interval. Thus, if the target’s velocity (i.e., course and speed) changes significantly, the accuracy of localization using the CV model can be degraded because of modeling errors. The IMM filtering algorithm was developed to overcome this problem. The IMM algorithm uses multiple models and can deal with significant changes in the target’s motion. In the case of aircraft targets, an IMM algorithm that mixes the CV and coordinated turn (CT) models is typically used. The CT model is appropriate for situations wherein a fixed-wing aircraft is tracked using radars, with a long sampling interval. However, UWB-based localization systems have short sampling intervals and the motions of small UAV targets are different from those of fixed-wing aircrafts. The process noise covariance $\mathbf{Q}$ in the CV model represents the acceleration of a target. If a target moves with a nearly constant velocity, a small $\mathbf{Q}$ is suitable. A large $\mathbf{Q}$ is suitable in the case of significant maneuvering. Therefore, we employ multiple CV models with different $\mathbf{Q}$ values for the IMM algorithm as follows:

• $M_1$: CV model (3) with small $\mathbf{Q}$ for nearly constant motion.
• $M_2$: CV model (3) with large $\mathbf{Q}$ for maneuvering.

The IMM filtering runs multiple model-matched filters in parallel. Inputs to the j-th model-matched filter are

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k-1|k-1}^{\left(j\right)}& =\sum _{i=1}^2{\mu }_{k-1}^{i|j}{\widehat{\mathbf{x}}}_{k-1|k-1}^i,\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{k-1|k-1}^{\left(j\right)}& =\sum _{i=1}^2{\mu }_{k-1}^{i|j}\{{\mathbf{P}}_{k-1|k-1}^i+({\widehat{\mathbf{x}}}_{k-1|k-1}^i-{\widehat{\mathbf{x}}}_{k-1|k-1}^{\left(j\right)}){({\widehat{\mathbf{x}}}_{k-1|k-1}^i-{\widehat{\mathbf{x}}}_{k-1|k-1}^{\left(j\right)})}^T\},\hfill \end{array}$$

(12)

where the superscript i indicates the output of the i-th model-matched filter and ${\mu }_{k-1}^{i|j}$ is the mixing probability computed as

$$\begin{array}{cc}\hfill {\mu }_{k-1}^{i|j}& =\frac{{\pi }_{ij}{w}_{k-1}^i}{{\sum }_{i=1}^2{\pi }_{ij}{w}_{k-1}^i},\hfill \end{array}$$

(13)

where ${\pi }_{ij}$ is the transitional probability and $w_{k-1}^i$ is the mode probability. ${\pi }_{ij}$ and the initial value of $w_{k-1}^i$ are design parameters in IMM filtering [17].

The model-matched filtering can use various recursive filters, such as the EKF and PDAF. After model-matched filtering, the output of the model-matched filters are merged as follows:

$$\begin{array}{cc}\hfill {\widehat{\mathbf{x}}}_{k|k}& =\sum _{i=1}^2{w}_k^i{\widehat{\mathbf{x}}}_{k|k}^i,\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{k|k}& =\sum _{i=1}^2{w}_k^i\{{\mathbf{P}}_{k|k}^i+({\widehat{\mathbf{x}}}_{k|k}^i-{\widehat{\mathbf{x}}}_{k|k}){({\widehat{\mathbf{x}}}_{k|k}^i-{\widehat{\mathbf{x}}}_{k|k})}^T\},\hfill \end{array}$$

(15)

where ${\widehat{\mathbf{x}}}_{k|k}$ and ${\mathbf{P}}_{k|k}$ are the output of the IMM filter, ${\widehat{\mathbf{x}}}_{k|k}^i$ and ${\mathbf{P}}_{k|k}^i$ are the outputs of the i-th model-matched filter and $w_k^i$ is the mode probability at time k. The mode probability can be updated as

$$\begin{array}{cc}\hfill w_k^j& =\frac{{\Lambda }_k^j{\sum }_{i=1}^2{\pi }_{ij}{w}_{k-1}^i}{{\sum }_{j=1}^2{\Lambda }_k^j{\sum }_{i=1}^2{\pi }_{ij}{w}_{k-1}^i},\hfill \end{array}$$

(16)

where ${\Lambda }_k^j$ is the model conditioned likelihood, which is computed as

$$\begin{array}{cc}\hfill {\Lambda }_k^j& =\frac{1}{{\left(2\pi \right)}^{n_z/2}{\left|\mathbf{R}\right|}^{1/2}}\exp [-0.5{({\mathbf{z}}_{\mathrm{k}}-{\widehat{\mathbf{z}}}_{\mathrm{k}}^{\mathrm{j}})}^{\mathrm{T}}{({\mathbf{S}}_{\mathrm{k}}^{\mathrm{j}})}^{-1}({\mathbf{z}}_{\mathrm{k}}-{\widehat{\mathbf{z}}}_{\mathrm{k}}^{\mathrm{j}})],\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {\widehat{\mathbf{z}}}_k^j& =\mathbf{h}\left({\widehat{\mathbf{x}}}_{k|k}^j\right),\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill {\mathbf{S}}_k^j& ={\mathbf{H}}_k^j{\mathbf{P}}_{k|k}^{\left(j\right)}{\left({\mathbf{H}}_k^j\right)}^T+\mathbf{R},\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {\mathbf{H}}_k^j& ={\left.\frac{\partial \mathbf{h}}{\partial \mathbf{x}}\right|}_{{\widehat{\mathbf{x}}}_{k|k}^j},\hfill \end{array}$$

(20)

where $n_z$ is the dimension of the measurement vector.

We now propose a simple method to mitigate the NLOS effects. The idea was inspired by the fact that NLOS causes bias errors in TOA measurements. Although the bias error is unpredictable, it generally increases the TOA measurements. Thus, if we select small TOA measurements, the probability of avoiding the TOA measurements contaminated by NLOS effects increases. A small TOA means that the AN is near the mobile node and hence we call this method the NN (nearest neighbor) method. To be able to select several (at least four) small TOAs, the number of TOAs should be much larger than four. Thus, this idea requires redundant ANs. As FANET typically uses more than four ANs, the method using NN measurements is feasible. The localization algorithm based on IMM-EKF using NN measurements is summarized as Algorithm 1.

Algorithm 1: 3D localization using the IMM-EKF and NN TOA measurements

The method using NN measurements is heuristic and incomplete. Thus, we propose another systematic method for NLOS mitigation. The PDAF has the ability to track a target in the presence of false measurements and we use the ability to deal with the measurements contaminated by NLOS bias errors. The PDAF algorithm includes a measurement-validation process that discards unlikely measurements. If the statistical distance between the actual measurement ${\mathbf{z}}_k$ and predicted measurement ${\widehat{\mathbf{z}}}_k$ is larger than a threshold, the measurement is judged to be an unlikely measurement. The statistical distance can be computed as

$$\begin{array}{cc}\hfill {\mathcal{V}}_k& ={({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_k)}^T{\mathbf{S}}_k^{-1}({\mathbf{z}}_k-{\widehat{\mathbf{z}}}_k).\hfill \end{array}$$

(21)

The threshold $\gamma$ can be determined by selecting the chi-square value corresponding to $0.01$.

The input to the PDAF is the set of measurements containing true and false measurements. The PDA algorithm computes the association probabilities between each measurement and its estimated state. The true/false measurements contribute to the estimation process depending on their association probabilities. To use the PDAF for 3D localization, we propose the following method:

• Select n NN measurements from the N measurements.
• Construct the measurement vectors by selecting four TOA measurements as $\mathbf{z}={\left[z_{i_1}{z}_{i_2}{z}_{i_3}{z}_{i_4}\right]}^T$, where $i_1,i_2,i_3,i_4\in \{1,2,\cdots ,n\}$. Then, the number of combinations is $C_4^n$.
• Use the $C_4^n$ measurement vectors as inputs to the PDAF.

We use the IMM-PDAF instead of the ordinary PDAF. The 3D localization algorithm using the IMM-PDAF and NN measurements is summarized as Algorithm 2.   

Algorithm 2: 3D localization algorithm using the IMM-PDAF and NN measurements

5. 3D Localization Algorithm Based on Hybrid IMM-PDA/FIR Filter

In this section, we propose a 3D localization algorithm based on HIPFF. The localization algorithm proposed in the previous section is robust against NLOS errors. However, a weakness of the IMM-PDAF can cause localization failures. The IMM-PDAF discards unlikely measurements through the validation process. If there is no surviving measurement after the validation process, the IMM-PDAF cannot perform the measurement update; it performs only the time update. If such situations occur successively, the positions estimated by IMM-PDAF will be farther away from the true positions. This divergence phenomenon can be found in many stochastic IIR filters because they all use past information and accumulate errors over time. FIR filters have been developed to overcome the divergence problem occurring in IIR filters. FIR filters perform batch-processing using only recent finite measurements and are robust against divergence due to error accumulation. However, FIR filters are less accurate than IIR filters under normal conditions. Thus, combining IIR and FIR filters can complement both the filters.

The hybrid filtering framework combining the IMM-PDAF and an FIR filter can be summarized as follows:

• The IMM-PDAF is used as a main filter that provides estimates in normal situations.
• The FIR filter is used as an assisting filter that provides estimates when failures of the main filter are detected.
• The main filter is reset and rebooted using the output of the assisting filter.

A situation where the IMM-PDAF filter can fail is determined when all of the following conditions are met.

• The measurement vector containing all NN measurements is too far from the predicted measurements in terms of the Mahalanobis distance.
• There is no surviving measurement vector after the measurement validation process (i.e., $n_{v,k}=0$).

The Mahalanobis distance between the actual and predicted measurements is computed as

$$\begin{array}{cc}\hfill {\mathcal{D}}_k& =({\mathbf{z}}_k^{*}-{\widehat{\mathbf{z}}}_k){\mathbf{R}}^{-1}({\mathbf{z}}_k^{*}-{\widehat{\mathbf{z}}}_k),\hfill \\ \hfill {\mathbf{z}}_k^{*}& ={[z_1{z}_2\cdots z_n]}^T,\hfill \\ \hfill {\widehat{\mathbf{z}}}_k& =\mathbf{h}\left({\widehat{\mathbf{x}}}_{k|k}\right).\hfill \end{array}$$

(22)

The threshold for the Mahalanobis distance can be determined using the chi-square table. We employ the extended minimum-variance FIR (EMVF) filter as an assisting FIR filter. This is because the EMVF filter is the only nonlinear FIR filter that can provide estimation error covariance, which is essential information for resetting the main filter. The structure of HIPFF is shown in Figure 2, and the overall algorithm of 3D localization based on HIPFF is summarized as Algorithm 3.

Algorithm 3: 3D localization based on HIPFF

6. Simulation

In this section, simulation results are presented to demonstrate the performance of the proposed 3D localization algorithm.

Figure 3 shows the simulation scenario, in which the trajectory of a target UAV (mobile node) and positions of the ANs are expressed. Eight aerial ANs are located at the 3D coordinates of $(0,0,30)$, $(0,60,30)$, $(60,0,30)$, $(60,60,30)$, $(20,20,15)$, $(40,20,15)$, $(40,40,15)$ and $(20,40,15)$, all in meters. The target UAV starts at the coordinate $(0,5,20)$, moves with the speed of $15\mathrm{km}/\mathrm{h}$ and performs three types of maneuver: (1) yaw change, (2) pitch change and (3) pitch and yaw changes. The mobile node (target UAV) receives UWB signals transmitted from the eight ANs. We considered NLOS situations in which signals from an AN were obstructed for two seconds. The total simulation time was 36 s, and the sampling interval was $T=0.1$. Simulation was performed for the time steps $1\le k\le 360$ and NLOS situations occurred during the time steps $150\le k\le 170$. The simulation program was implemented using MATLAB and the simulation was performed using a PC with an Intel Core i5-1135G7 CPU at 2.40 GHz. Various localization algorithms based on EKF, IMM-EKF, IMM-PDAF and HIPFF were compared in the simulation.

The design parameters for the simulation were set as follows. The TOA measurement noise covariance was set as $r^2=0.5^2$. The mean and the covariance of the NLOS bias error were set as ${\mu }_b=10$ and ${\sigma }_b^2=5^2$, respectively. The process noise covariances for IMM filtering were set as ${\mathbf{Q}}_1=0.5^2{\mathbf{I}}_3$ and ${\mathbf{Q}}_1=5^2{\mathbf{I}}_3$. The detection and gate probabilities were set as $P_D=0.99$ and $P_G=0.99$, respectively. The gate threshold was set to $\gamma =11.345$. The horizon size of the EMVF filter was set to $M=6$. The initial mode probabilities for the IMM algorithms were set as $w_0^1=w_0^2=0.5$. The total number of TOA measurements was $N=8$ and the number of NN measurements used for HIPFF was set as $n=6$.

To evaluate the performances of the localization algorithms, we computed the following performance metrics for 100 Monte-Carlo (MC) simulations. The root mean square (RMS) position error at time step k was computed as

$$\begin{array}{c}\hfill \mathrm{RMS}=\sqrt{\frac{1}{100}\sum _{i=1}^{100}[{(x_k^i-{\widehat{x}}_k^i)}^2+{(y_k^i-{\widehat{y}}_k^i)}^2+{(z_k^i-{\widehat{z}}_k^i)}^2]},\end{array}$$

(23)

where the superscript i indicates the i-th MC run, $(x_k,y_k,z_k)$ and $({\widehat{x}}_k,{\widehat{y}}_k,{\widehat{z}}_k)$ are the true and estimated 3D positions, respectively. The root time-averaged mean square (RTAMS) position error for the whole simulation time was computed as

$$\begin{array}{c}\hfill \mathrm{RTAMS}=\sqrt{\frac{1}{360\times 100}\sum _{k=1}^{360}\sum _{i=1}^{100}[{(x_k^i-{\widehat{x}}_k^i)}^2+{(y_k^i-{\widehat{y}}_k^i)}^2+{(z_k^i-{\widehat{z}}_k^i)}^2]}.\end{array}$$

(24)

6.1. Effects of IMM Algorithm in 3D UAV Localization

We simulated 3D UAV localization using EKFs and IMM-EKF to evaluate the effects of the IMM algorithm under line-of-sight (LOS) conditions. In this simulation, the transitional probability matrix (TPM) $\Pi =\left[{\pi }_{ij}\right]$ was set as

$$\begin{array}{c}\hfill \Pi =\left[\begin{array}{cc}0.95& 0.05\\ 0.05& 0.95\end{array}\right].\end{array}$$

(25)

Figure 4 shows the localization results, in which the IMM-EKF exhibits the smallest RMS position errors, overall. The EKF using a small process noise ($q^2=0.5^2$) exhibits large errors when the target performs maneuvers. The target UAV performed three maneuvers and the EKF with a small process noise exhibited three significant increases in the RMS position error. However, the EKF with a small process noise exhibited the smallest errors when the target moved in a straight line (time intervals of $1\le k\le 100$ and $300\le k\le 360$). These results show that a small process noise is appropriate for the motion with a nearly constant velocity. The EKF using a large process noise ($q^2=5^2$) did not exhibit significant increase in error when the target performed maneuvers. However, the EKF using a large process noise was less accurate than that using a small process noise when the target moved with a nearly constant velocity. Figure 4c shows the mode probabilities of IMM-EKF, in which the mode probability of M1 ($q^2=0.5^2$) has higher values than that of M2 ($q^2=5^2$) when the target moved in a straight line, because the M1 model results in better (higher likelihood) estimation than the M2 model. In contrast, the mode probability of M2 has higher values than that of M2 when the target performs maneuvers. The simulation demonstrates that it is better to use multiple models (i.e., IMM) than a single model in situations where we do not know how the target will maneuver.

6.2. Effects of Using NN Measurements in NLOS Situation

We now evaluate the effects of using NN measurements in NLOS situations. Because the effectiveness of the IMM algorithm in 3D UAV localization has been proved, we will use the IMM-EKF instead of EKF. We compare three IMM-EKFs that use different numbers of measurements–four, six and eight measurements—and we refer to them as IMM-EKF4, IMM-EKF6 and IMM-EKF8 hereafter. IMM-EKF4 and IMM-EKF6 are based on the method of NN measurements, but IMM-EKF8 uses all eight measurements. Figure 5 shows the localization results using IMM-EKFs in LOS situations. The differences between the performances of the three algorithms are not significant in LOS situations. In Figure 5a, the estimated trajectories obtained using the three algorithms are difficult to distinguish. In Figure 5b, we see that the differences in errors are small (approximately $0.1$ m). Although the difference is small, IMM-EKF8 shows the least error. Thus, it has been demonstrated that the use of a larger number of measurements leads to higher accuracy under LOS conditions.

Figure 6 shows the 3D localization results using IMM-EKFs under NLOS conditions, in which one of the ANs is in the NLOS condition for the time steps $150\le k\le 170$. IMM-EKF8 exhibits sharp rises in RMS position errors for NLOS conditions, for all ANs (Figure 6a–h). This is because the measurement contaminated by NLOS error was inevitably used in IMM-EKF8. IMM-EKF4 exhibit a sharp rise in error under NLOS conditions for the four ANs (Figure 6c,e,f,h). IMM-EKF6 exhibit a sharp rise in error in six cases (Figure 6c–h). This results demonstrates that using NN measurements (IMM-EKF4 and IMM-EKF6) is better using all measurements (IMM-EKF8) under NLOS conditions. Moreover, the number of `localization failure (sharp increase in error)’ cases was small when a small number of NN measurements was used. Therefore, we conclude that use of NN measurements is helpful for NLOS mitigation and IMM-EKF4 is the most reliable algorithm among the three IMM-EKFs in NLOS situations.

6.3. Comparison of HIPFF and IMM-PDAF

Here, we show the difference between HIPFF and IMM-PDAF. In the HIPFF algorithm, IMM-PDAF is used as a main filter and an assisting EMVF filter recovers the main filter from failures. Thus, under normal conditions, HIPFF and pure IMM-PDAF provide the same localization results. IMM-PDAF using NN measurements has better robustness against NLOS errors, compared to IMM-EKFs using NN measurements. However, the IMM-PDAF algorithm sometimes fails if there is no surviving measurement after the validation process. Figure 7 shows the failures of the IMM-PDAF algorithm. In this simulation, the TPM was set using the values in (25). In Figure 7c, the mode probability of M1 has high values for $k\le 100$ because the target moves with nearly constant velocity. In Figure 7a, when the target performs the first maneuver ($100<k\le 130$), the position estimated by IMM-PDAF deviates the real position. This is because of a lack of validated (surviving after validation) measurements in the IMM-PDAF algorithm. Using the TPM in (25), the mode in IMM-PDAF did not switch from M1 to M2 quickly when the target performs a maneuver. Then, the validation region centered at the predicted measurement becomes far from the actual measurements, which leads to the divergence of IMM-PDAF. In the same condition, HIPFF did not diverge and successfully track the target owing to the assisting EMVF filter.

Next, we changed the TPM using the following values

$$\begin{array}{c}\hfill \Pi =\left[\begin{array}{cc}0.5& 0.5\\ 0.5& 0.5\end{array}\right],\end{array}$$

(26)

which makes transition between two modes easy. Using the TPM in (26), we simulated the 3D localization again and the results are presented in Figure 8. IMM-PDAF did not diverge and successfully track the target and hence the IMM-PDAF and HIPFF provided the same localization results. In Figure 8c, the mode probabilities of M1 and M2 are not much different, which means the estimates by the outputs of two model-matched filters are mixed in a similar ratio. It is noted that HIPFF was successful regardless of the TPM.

6.4. Comparison of HIPFF and IMM-EKF4

IMM-EKF4 shows the best robustness against NLOS errors when comparing the three types of IMM-EKFs. HIPFF shows better reliability than pure IMM-PDAF. We now compare HIPFF and IMM-EKF4. Figure 9 shows the localization results using HIPFF and IMM-EKF4 under NLOS conditions. HIPFF does not exhibit a significant error increase for all AN’s NLOS conditions while IMM-EKF4 exhibits very sharp error increases (Figure 9c,e,f,g). HIPFF exhibits smaller errors than IMM-EKF4 under LOS conditions for the time intervals $k<150$ and $k>170$. Thus, it has been demonstrated that HIPFF provides accurate and reliable 3D localization under both LOS and NLOS conditions.

We computed RTAMS position errors for HIPFF and IMM-EKF4 under NLOS conditions, which are listed in

Table 1. RTAMS position errors (unit: meter) under NLOS conditions.

Localization	AN in NLOS Condition
Algorithm	AN1	AN2	AN3	AN4	AN5	AN6	AN7	AN8
IMM-EKF4	0.34	0.34	0.74	0.34	0.41	0.67	0.84	0.36
HIPFF	0.29	0.29	0.30	0.30	0.30	0.30	0.29	0.29

. The RTAMS position errors of HIPFF are less than $0.3$ m in all cases, while the errors of IMM-EKF4 fluctuate. Thus, we can conclude that HIPFF can provide consistent and reliable 3D localization, regardless of the LOS/NLOS conditions.

Finally, we measured the computation times for a single estimation in the simulation using MATLAB and a PC (Intel Core i5-1135G7 CPU at 2.40 GHz), which are shown in

Table 2. Computation time.

Algorithm	Computation Time (ms)
IMM-EKF4	0.00447
IMM-PDAF	0.744
HIPFF	1.09

. Both IMM-PDAF and HIPFF required much more computation time than IMM-EKF4. However, both algorithms can operate in realtime (which means the computation time for a single estimation is less than the sampling interval). With the recent development of computing technology, artificial intelligence algorithms that require enormous computation are also being processed in real time. Thus, the computational complexity of the proposed algorithm does not cause a problem.

7. Conclusions

In this study, we proposed a 3D localization algorithm based on HIPFF for NLOS mitigation in FANET. The proposed algorithm was based on a hybrid filtering framework, in which IMM-PDAF and EMVF filter were used as the main and assisting filters, respectively. We also proposed a method to use NN measurements to mitigate NLOS effects. The HIPFF using NN measurements was applied to 3D UAV localization in FANET. In the simulation, the HIPFF did not exhibit a significant increase in the RMS position errors under NLOS conditions, whereas the IMM-EKFs exhibited sharp rises in error. In addition, the HIPFF provided reliable localization while the pure IMM-PDAF sometimes diverged. The HIPFF-based localization was highly accurate, producing RTAMS position errors less than $0.3m$. Because the HIPFF can provide accurate and reliable 3D localization, it is suitable for FANET-based localization systems that frequently encounter NLOS situations. However, the HIPFF-based localization is only applicable to TOA-based localization systems. Therefore, we are planning a study to extend our algorithm to localization systems based on different wireless measurements, such as TDOA and AOA.