A Robust Tracking Algorithm Based on a Probability Data Association for a Wireless Sensor Network

Abstract

As one of the core technologies of the Internet of Things, wireless sensor network technology is widely used in indoor localization systems. Considering that sensors can be deployed to non-line-of-sight (NLOS) environments to collect information, wireless sensor network technology is used to locate positions in complex NLOS environments to meet the growing positioning needs of people. In this paper, we propose a novel time of arrival (TOA)-based localization scheme. We regard the line-of-sight (LOS) environment and non-line-of-sight environment in wireless positioning as a Markov process with two interactive models. In the NLOS model, we propose a modified probabilistic data association (MPDA) algorithm to reduce the NLOS errors in position estimation. After the NLOS recognition, if the number of correct positions is zero continuously, it will lead to inaccurate localization. In this paper, the NLOS tracer method is proposed to solve this problem to improve the robustness of the probabilistic data association algorithm. The simulation and experimental results show that the proposed algorithm can mitigate the influence of NLOS errors and achieve a higher localization accuracy when compared with the existing methods.

1. Introduction

Due to there being many obstacles, it is difficult to provide accurate localization indoors. Applying wireless sensor network technology to indoor localization can solve the problem of indoor localization. Wireless positioning systems usually use time of arrival (TOA), time difference of arrival (TDOA), angle of arrival (AOA), and received signal strength (RSS) to estimate the location information to realize the tracking and positioning of a mobile node. This paper uses the TOA method to obtain the Euclidean distance measurements between the beacon nodes and the mobile node. The principle of the TOA method is to calculate the arrival time from the mobile node to the beacon nodes, and then these measurements are multiplied by the speed of light in order to obtain the Euclidean distance measurements between the beacon nodes and the mobile node. Trilateration techniques can be used to estimate the position of the mobile node. If the channels between the mobile node and the beacon nodes have a direct path, the channels are considered to be in line-of-sight (LOS) and the TOA measurements obtained in LOS environments are perfect. The accurate position can be obtained using the extended Kalman filter to process the TOA measurements. However, the assumption of the LOS channel is ideal and impractical [1]. In practice, obstacles such as persons, furniture, or walls block the propagation path, causing diffraction and refraction. The propagation path of the signal becomes long, and the channel is in a non-line-of-sight (NLOS) environment. The NLOS factor results in a positive bias of the TOA measurements, leading to a decreased positioning accuracy. Thus, algorithms that reduce NLOS errors can achieve high-precision positioning.

Many positioning algorithms have emerged based on various positioning methods. A semidefinite programming algorithm was proposed in Su et al. [2], where it transforms a TDOA model into a TOA model and uses new constraints to mitigate NLOS errors. A bisection-based approach is proposed in Tomic and Beko [3], which can accurately solve the maximum likelihood estimation derived from the measurement model through the bisection procedure to achieve accurate positioning. Yang et al. [4] proposed a high-precision and low-complexity localization algorithm based on an imported vector machine (IVM), which employs the probability output of an input vector machine, and has a higher localization accuracy than the corresponding methods, such as a support vector machine (SVM) and correlation vector machine (RVM). Zhang et al. [5] proposed a novel distributed consensus-based adaptive Kalman estimation algorithm. In order to estimate the states of the target more precisely, an optimal Kalman gain is obtained by minimizing the mean-squared estimation error. An adaptive consensus factor is employed to adjust the optimal gain, as well as to acquire a better filtering performance. In the filter’s information exchange, dynamic cluster selection and a two-stage hierarchical fusion structure are employed to get a more accurate estimation. When all the range measurements estimation is from the LOS environments, the position estimation obtained by the least-squares method is accurate; if there is at least a range measurement estimate from the NLOS condition, the position estimation of the mobile node will be inaccurate, which will deviate from the real position. The more NLOS data that is used, the less accurate the positioning is. According to the idea of data fusion, combining different positioning methods is also a common method to improving positioning accuracy. An improved positioning method was proposed to improve the accuracy by combining TOA and AOA positioning methods [6,7]. Vaghefi and Buehrer [8] used semidefinite programming to solve the problem of collaborative localization. In Vaghefi and Buehrer [9], a novel cooperative localization algorithm of source nodes is proposed. Based on the interrelation of multiple source nodes, the novel extended Kalman filter (EKF) integrated with semidefinite programming method is used for localization. It not only solves the problem of the cooperative localization using multiple source nodes, but also improves the localization performance compared with the classic EKF. In addition, it can also be used for traditional non-cooperative localization.

In the localization algorithms, many algorithms are studied to mitigate the interference of NLOS factors [10,11,12,13,14,15,16,17]. Chen [10] proposed a residual weighting (RWGH) algorithm, which can mitigate NLOS errors to a certain extent, but the computational complexity is high. The algorithm proposed by Park and Chang [11] also uses a residual weighting method.Jiao and coworkers [12,13] used a method that selects the smallest normalized residual combination using different methods, and then performs weighted summation, which reduces the complexity of the residual weighting algorithm. The introduction of iterative ideas in the NLOS algorithms and the optimization of the calculation results are also effective methods for reducing the NLOS errors. Li [15] proposed an iterative minimum residual algorithm, which iteratively selects the minimum residual combination as the final estimated position of the mobile station (MS) by iterating the residual size in each combination whose value is less than the predetermined threshold. Horiba and coworkers [16,17] used a TOA/AOA hybrid positioning method to improve the performance of the iterative minimum residual algorithm by selecting the appropriate iterative minimum residual criterion. In References [18,19,20], the NLOS mitigation algorithms proposed by the authors can reduce the NLOS errors without prior knowledge of the NLOS errors.

The NLOS identification methods can effectively eliminate the influence of NLOS errors on the positioning accuracy. Wylie and Holtzman [1] proposed a method for judging whether the range measurements contain NLOS errors based on the measurement variance (standard deviation). Location spoofing is an important factor for producing NLOS errors, and Liu et al. [21] eliminated NLOS errors by identifying location spoofing. Han proposed a probabilistic position selection algorithm [22], which is based on the received signal strength indication (RSSI) and pedestrian dead reckoning (PDR) in the mixed LOS environment and NLOS environment for low-complexity identification. In the case of an unknown LOS/NLOS propagation prior probability, NLOS propagation can be identified by examining whether the range measurements obey a Gaussian distribution [23,24,25]. In recent years, Kolmogorov-Smirnov (K-S), Aderson-Drling (A-D), chi-square, Gruss test, skewness test, and kurtosis test have appeared successfully [26,27,28]. If the LOS/NLOS propagation prior probability is known, the NLOS can be identified using a generalized likelihood ratio test based on the statistical distribution of the different probabilities of the error. Large outliers sometimes occur in the range measurements. These outliers can seriously interfere with the positioning result, resulting in a positioning failure, thus we need to discard the outliers.

In this paper, we propose a robust tracking algorithm based on an improved modified probabilistic data association (MPDA) and an interacting multiple model (IMM). In this paper, the improved probabilistic data association filter is used in the NLOS model. Through NLOS recognition, the position estimation with the NLOS error is discarded, and the correct position estimation is weighted with the corresponding correlation probability to obtain the final position estimation to reduce the NLOS error. The proposed algorithm in this paper has the following advantages:

(1) The traditional MPDA algorithm is used for the NLOS recognition, where there may be continuous incorrect position estimation, resulting in inaccurate positioning. Therefore, the NLOS tracer method is proposed in this paper to record the occurrence of an incorrect position estimation. When we use the NLOS tracer method to find that there are two or more consecutive occurrences of incorrect position estimation, the EKF is used for updating to reduce the adverse influence of this situation on positioning and improve the robustness of the algorithm.

(2) The improved probabilistic data association algorithm has a high positioning accuracy and robustness to NLOS errors, therefore it is used to reduce the NLOS errors.

(3) The simulation and experimental results show that the proposed algorithm in this paper can mitigate the influence of NLOS errors when the NLOS error obeys different distributions.

The paper is structured as follows. Section 2 introduces the signal model and provides an overview of existing techniques from the literature. The proposed algorithm is explained in Section 3, and Section 4 illustrates the simulation results. Conclusions are drawn in Section 5.

2. Problem Statement

2.1. Signal Model

The signal transmission channel between the mobile node and the beacon nodes changes between the LOS condition and NLOS condition, where this transformation is considered a switching mode system, as shown in the two-state Markov process of Figure 1. The state vector of the mobile node is $x\left(k\right)={\left[\begin{array}{cccc}x\left(k\right)& y\left(k\right)& \dot{x}\left(k\right)& \dot{y}\left(k\right)\end{array}\right]}^T$, where $\left(x\left(k\right),y\left(k\right)\right)$ denotes the position of the mobile node and $\left(\dot{x}\left(k\right),\dot{y}\left(k\right)\right)$ denotes the velocity of the mobile node.

The state vector of the mobile node changes over time according to a force model:

$$\mathit{x}\left(k\right)=A\mathit{x}\left(k-1\right)+C\omega \left(k-1\right); k=1, \dots , K,$$

(1)

where, $K$ is the number of time steps, and:

$$A=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],C=\left[\begin{array}{cc}\begin{array}{c}\Delta t^2/2\\ 0\\ \Delta t\\ 0\end{array}& \begin{array}{c}0\\ \Delta t^2/2\\ 0\\ \Delta t\end{array}\end{array}\right],$$

where, $A$ is the state transition matrix that describes the movement of the mobile node between two consecutive time steps. $C$ is the interference input matrix describing the mapping of the randomness and the velocity of the mobile node, and $\Delta t$ is the sampling period. The process driving noise $\omega \left(k\right)$ due to the acceleration of the mobile node is assumed to have a zero-mean, white Gaussian with a covariance matrix $Q\left(k\right)$. Assume that there are M beacon nodes around the mobile node to detect the signal of the mobile node. We can let $D\left(k\right)={\left[d_1\left(k\right),d_2\left(k\right),\dots ,d_M\left(k\right)\right]}^T$ denote the range measurements based on the TOA data between the mobile node and $M$ beacon nodes at time step $k$. Then:

$$D\left(k\right)=\mathit{h}\left(\mathit{x}\left(k\right)\right)+v\left(k\right), k=1, \dots , K,$$

(2)

where $\mathit{h}\left(\mathit{x}\left(k\right)\right)={\left[h_1\left(\mathit{x}\left(k\right)\right),h_2\left(\mathit{x}\left(k\right)\right),\dots ,h_M\left(\mathit{x}\left(k\right)\right)\right]}^T$ and the Euclidean distance between the mobile node and the $mth$ beacon node with the position $\left(x_{bn,m},y_{bn,m}\right)$ at time step $k$ is:

$$h_m\left(\mathit{x}\left(k\right)\right)=\sqrt{{\left(x\left(k\right)-x_{bn,m}\right)}^2+{\left(y\left(k\right)-y_{bn,m}\right)}^2}, m=1, \dots , M.$$

(3)

The noise vector $v\left(k\right)={\left[v_1\left(k\right),v_2\left(k\right),\dots ,v_M\left(k\right)\right]}^T$ contains random variables with a variance describing Gaussian sensor noise due to the NLOS propagation. The measurement covariance matrix is:

$$R\left(k\right)=E\left\{\left[\mathit{v}\left(k\right)-E\left\{\mathit{v}\left(k\right)\right\}\right]{\left[\mathit{v}\left(k\right)-E\left\{\mathit{v}\left(k\right)\right\}\right]}^T\right\},$$

(4)

which is defined as:

$$R\left(k\right)=diag\left[{\sigma }_1^2,{\sigma }_2^2,\dots ,{\sigma }_M^2\right],$$

(5)

where the elements ${\sigma }_m^2$ in $R\left(k\right)$ are defined as:

$${\sigma }_m^2=\{\begin{array}{cc}{\sigma }_L^2& \mathrm{if} \mathrm{LOS} \mathrm{condition}\hfill \\ {\sigma }_L^2+{\sigma }_{NLOS}^2 & \mathrm{if} \mathrm{NLOS} \mathrm{condition}\hfill \end{array}.$$

(6)

We assume that the sensor noise variance ${\sigma }_L^2$, and process covariance matrix $Q\left(k\right)$ are known. ${\sigma }_{NLOS}^2$ is unknown.

2.2. A Brief Introduction of Existing Methods

The interacting multiple model (IMM) algorithm is used to track and locate in NLOS environments. In Vaghefi and Buehrer [8], the IMM algorithm uses two Kalman filters to smooth the TOA range measurements in both the LOS model and the NLOS model. In order to reduce the NLOS errors, the TOA range measurements smoothed by the Kalman filter is used to subtract the NLOS mean error in the NLOS model. The distance estimates between the mobile node and the beacon nodes are determined by combining the state estimate in the LOS model with the state estimate in the NLOS model, which eliminates the NLOS errors. Then, the location of the mobile node is determined using a geometric method based on the smoothed distance estimates. According to the idea of data fusion, Chen et al. [29] proposed an IMM algorithm based on TOA and RSS data fusion to find the location of a mobile node. Almost all IMM algorithms need to presuppose NLOS statistical errors to solve the NLOS interference problem, but in practice, the NLOS statistical errors are unknown. The robust IMM (RIMM) algorithm proposed in Li et al. [30] does not need prior knowledge of the NLOS error. In the LOS model, EKF is used to estimate the location of a mobile node. In the NLOS model, REKF is used to transform the EKF equation into a linear regression problem, which is solved using robust techniques in Hammes and Zoubir [31]. Then, the state vectors of the two models are weighted using likelihood function values to determine the final state vectors of the mobile node.

EKF can achieve accurate positioning in LOS environments, but the positioning accuracy is not high in NLOS environments. Therefore, it is necessary to improve the filters of the IMM algorithm to improve the positioning accuracy. The RIMM algorithm is used to improve the filter of the NLOS model in the IMM algorithm. The MPDA algorithm in Hammes et al. [32] has a high positioning accuracy and can mitigate NLOS errors. The MPDA algorithm is a sub-optimal filtering algorithm based on the Bayesian formula, which divides the range measurements between the beacon nodes and the mobile node into different groups. Each group obtains the corresponding position estimation via a least-squares method and optimizes the position estimation by using the Gauss–Newton iteration method. The optimized location estimation is identified using NLOS detection, and the location estimation that does not fall into the validation gate is discarded. The location estimation that falls into the validation gate is weighted by the corresponding association probability to determine the location of the mobile node. Choosing an appropriate validation gate is a prerequisite for realizing the probability data association. The common validation gate forms are a rectangular gate, elliptic gate, sector gate, etc. Among them, the elliptic gate is the most widely used. The positioning accuracy of the MPDA algorithm is very high, but it is sensitive to outliers. These outliers often fail to locate, so it is necessary to abandon the outliers to ensure the positioning accuracy of the MPDA algorithm. Compared with the MPDA algorithm, the EKF algorithm has better robustness.

3. Proposed Method

3.1. General Concept

As shown in Figure 2, we assumed that the initial values of the state estimation and covariance matrix are known. The initial values of the model probability and the transition matrix are given according to empirical knowledge, and the mixed probability, mixed state estimation, and mixed covariance matrix are calculated to complete the interactive process, and then model matching is conducted. The proposed algorithm uses an extended Kalman filter in the LOS model and proposes an improved modified probabilistic data association filter in the NLOS model. The two filters work in parallel to achieve the model-matching process. In the NLOS model, the improved modified probabilistic data association filter first divides the measured values into N different groups. Each group gets a corresponding position estimation via a least-squares estimation. The N different position estimates are obtained for NLOS recognition through a validation gate, where the position estimates falling in the validation gate around the predicted position estimates are the correct position estimates. Furthermore, the position estimates that do not fall in the validation gate are discarded. The correlation probability corresponding to the correct position estimation is calculated, the correct position estimation with the corresponding correlation probability is weighted, and the updated position estimation is produced. Through the NLOS recognition, the position estimates with a large error are discarded to reduce the NLOS errors. When the NLOS interference is relatively serious, there will be no correct position estimation, which will have no significant impact on the positioning results. If there is no correct position estimation after successive attempts, a positioning failure will occur. Therefore, the NLOS tracer method is proposed to record the situation of incorrect position estimation. When no correct position estimation occurs the first time, the predicted state is regarded as the updated state. If no correct position estimation occurs twice or more times, the EKF will be used to update the position. The model probability is updated according to the result of model matching, and the updated model probability is used to weight the updated state estimation of the corresponding model to obtain the final state estimation.

3.2. Interaction

We determine the initial value of the prior probability ${\mu }_i\left(k-1\right)$, the Markov transition probabilities $p_{ij}$, the prior state ${\widehat{X}}_i\left(k-1|k-1\right)$, and covariance matrix $P_i\left(k-1|k-1\right)$ of the proposed algorithm. However, the transition probabilities $p_{ij}$ are not known in practice, so the initial of the transition probabilities $p_{ij}$ are chosen based on the prior knowledge. Before the interaction, determination of the mixing probabilities ${\mu }_{i|j}\left(k-1|k-1\right)$ is required. The mixing probability ${\mu }_{i|j}\left(k-1|k-1\right)$ is given as:

$${\mu }_{i|j}\left(k-1|k-1\right)=\left(1/{\overline{c}}_j\right)p_{ij}{\mu }_i\left(k-1\right), k=1, \dots , K.$$

(7)

The normalization factor ${\overline{c}}_j$ is:

$${\overline{c}}_j={\displaystyle \sum _i{p}_{ij}}{\mu }_i\left(k-1\right),$$

(8)

where, the mixed probabilities ${\mu }_{i|j}\left(k-1|k-1\right)$ obtained are applied to the interaction process. Furthermore:

$${\widehat{X}}_{0j}\left(k-1|k-1\right)={\displaystyle \sum _i{\widehat{X}}_i\left(k-1|k-1\right)}{\mu }_{i|j}\left(k-1|k-1\right),$$

(9)

$${\tilde{X}}_{ij}\left(k-1|k-1\right)={\widehat{X}}_i\left(k-1|k-1\right)-{\widehat{X}}_{0j}\left(k-1|k-1\right),$$

(10)

$$P_{0j}\left(k-1|k-1\right)={\displaystyle \sum _i{\mu }_{i|j}\left(k-1|k-1\right)}\left\{P_i\left(k-1|k-1\right)+{\tilde{X}}_{ij}\left(k-1|k-1\right){\tilde{X}}_{ij}^T\left(k-1|k-1\right)\right\},$$

(11)

where ${\widehat{X}}_{0j}\left(k-1|k-1\right)$ is the mixed state estimate of the LOS/NLOS model at time step $k-1$ and $P_{0j}\left(k-1|k-1\right)$ is the mixed state covariance matrix estimate of the LOS/NLOS model at time step $k-1$. Model matching of the proposed algorithm is performed after the interaction is completed.

3.3. Model Matching

Prediction: The mixed state estimates and the mixed covariance matrix estimates obtained after the interaction are provided as initial estimates to the two model-matched filters for prediction:

$${\widehat{X}}_j\left(k|k-1\right)=A{\widehat{X}}_{0j}\left(k-1|k-1\right)$$

(12)

$$P_j\left(k|k-1\right)=A{P}_{0j}\left(k-1|k-1\right)A^T+CQ\left(k\right)C^T$$

(13)

where ${\widehat{X}}_j\left(k|k-1\right)$ is the state prediction of the LOS/NLOS model at time step $k$ and $P_j\left(k|k-1\right)$ is the error covariance matrix prediction of the LOS/NLOS model at time step $k$.

Update: When $j=1$, the state estimate update and the error covariance matrix estimate update are performed using the extended Kalman filter. Furthermore:

$$H_1\left(k\right)=\frac{\partial h\left(X\left(k\right)\right)}{\partial X\left(k\right)}{|}_{X\left(k\right)={\widehat{X}}_1\left(k|k-1\right)},$$

(14)

$$v_1\left(k\right)=D\left(k\right)-h\left({\widehat{X}}_1\left(k|k-1\right)\right),$$

(15)

$$S_1\left(k\right)=H_1\left(k\right)P_1\left(k|k-1\right)H_1^T\left(k\right)+R_1^{\ast }\left(k\right),$$

(16)

where $H_1\left(k\right)$ is the Jacobian matrix at time step $k$. $v_1\left(k\right)$ denotes the innovation at time step $k$.$S_1\left(k|k-1\right)$ is the innovation covariance matrix. $R_1^{\ast }\left(k\right)$ is the covariance matrix of the measurement errors vector. Furthermore:

$$K_1\left(k\right)=P_1\left(k|k-1\right)H_1^T\left(k\right)S_1^{-1}\left(k|k-1\right),$$

(17)

$${\widehat{X}}_1\left(k|k\right)={\widehat{X}}_1\left(k|k-1\right)+K_1\left(k\right)v_1\left(k\right),$$

(18)

$$P_1\left(k|k\right)=\left(I_4-K_1\left(k\right)H_1\left(k\right)\right)P_1\left(k|k-1\right),$$

(19)

$${\mathsf{\Lambda }}_1\left(k\right)=N\left(v_1\left(k\right);0,S_1\left(k\right)\right),$$

(20)

where $K_1\left(k\right)$ is the extended Kalman gain at time $k$, ${\widehat{X}}_1\left(k|k\right)$ is the updated state estimate, $P_1\left(k|k\right)$ denotes the updated error covariance matrix, and ${\mathsf{\Lambda }}_1\left(k\right)$ is the likelihood function.

When $j=2$, the improved MPDA filter is used to update the state estimation prediction and error covariance matrix prediction in the NLOS model. The proposed method divides the range measurements into $N=\left(\begin{array}{c}M\\ 3\end{array}\right)$ different subgroups, which are used together with the positions of the corresponding beacon nodes to determine the position of the mobile node, and each subgroup uses the least squares estimation to obtain the corresponding position estimation $z_n\left(k\right)$. The position estimation prediction $\widehat{z}\left(k|k-1\right)$ of the mobile node is given as:

$$\widehat{z}\left(k|k-1\right)=B{\widehat{X}}_2\left(k|k-1\right),$$

(21)

$$v_{2,n}\left(k\right)=z_n\left(k\right)-\widehat{z}\left(k|k-1\right), n=1, \dots , N,$$

(22)

where $B=\left[\begin{array}{c}\begin{array}{cccc}1& 0& 0& 0\end{array}\\ \begin{array}{cccc}0& 1& 0& 0\end{array}\end{array}\right]$ is the observation matrix, $v_{2,n}\left(k\right)$ is the innovation for the position estimate from subgroup $n$ at time step $k$. Then, we perform the NLOS detection for the position estimation obtained to determine the correct position estimation.

NLOS detection: If all the innovations do not exist as NLOS errors, then there are:

$$v_{2,n}\left(k\right)~N\left(0,S_{2,n}\left(k\right)\right), n=1, \dots , N.$$

(23)

In order to validate Equation (21), we define N hypotheses and alternatives:

$${\zeta }_{0,n}:v_{2,n}\left(k\right)~N\left(0,S_{2,n}\left(k\right)\right), n=1, \dots , N$$

(24)

$${\zeta }_{1,n}: \mathrm{not} {\zeta }_{0,n}, n=1, \dots , N$$

(25)

If the beacon nodes from subgroup $n$ are in the LOS condition, the hypothesis ${\zeta }_{0,n}$ holds true, and the position estimate $z_n\left(k\right)$ from subgroup n falls into the validation region; otherwise, the hypothesis ${\zeta }_{1,n}$ holds true. The test statistic $T_n\left(k\right)$ can be expressed as:

$$T_n\left(k\right)=v_{2,n}^T\left(k\right)S_{2,n}^{-1}\left(k\right)v_{2,n}\left(k\right).$$

(26)

$T_n\left(k\right)$ is verified with the validation gate $\gamma$ to determine whether the position estimate $z_n\left(k\right)$ is within the validation region. If $T_n\left(k\right)$ is not larger than the validation gate $\gamma$, the hypothesis ${\zeta }_{0,n}$ holds true, and $z_n\left(k\right)$ is accurate. If $T_n\left(k\right)$ is larger than the validation gate $\gamma$, the hypothesis ${\zeta }_{0,n}$ is rejected, and $z_n\left(k\right)$ is discarded due to it being an NLOS error. Furthermore:

$$S_{2,n}\left(k\right)=B{P}_2\left(k|k-1\right)B^T+{\sigma }_G^2{\left(H_{2,n}^T\left(z_n\right)H_{2,n}\left(z_n\right)\right)}^{-1},$$

(27)

$$H_{2,n}\left(z_n\right)=\frac{\partial h_{2,n}\left({\left[x,y\right]}^T\right)}{\partial \left[x,y\right]}{|}_{x=\widehat{x},y=\widehat{y}},$$

(28)

where $S_{2,n}\left(k\right)$ is the innovation covariance matrix for the position estimation from subgroup $n$ at time step $k$. The number of $z_n\left(k\right)$ that falls into the validation region is calculated and is referred to as $N_V\left(k\right)\left(0\le N_V\left(k\right)\le N\right)$. In order to judge whether $N_V\left(k\right)=0$ is continuous or not, we propose the NLOS tracer method, which is used to record $N_V\left(k\right)=0$. Therefore, we define:

$$L\left(k\right)=\{\begin{array}{cc}1& \mathrm{if} N_V\left(k\right)>0\\ 0& \mathrm{if} N_V\left(k\right)=0\end{array},$$

(29)

When $N_V\left(k\right)$ is larger than zero, we need to calculate the association probabilities used to weight the position estimation $z_n\left(k\right)$, which are also called the posterior probabilities. The premise of accurately calculating the association probabilities are to determine the threshold value of the validation region, and the selection of the threshold is related to the tracking threshold probability $P_G$. The tracking threshold probability $P_G$ denotes the probability that the position estimation $z_n\left(k\right)$ determined from LOS beacon nodes with smallest error covariance falls into the valid region. Furthermore:

$${\displaystyle \underset{0}{\overset{\gamma }{\int }}{f}_{x^2\left(2\right)}}\left(x\right)dx=P_G=1-P_{FA},$$

(30)

where $f_{x^2\left(2\right)}(\cdot )$ is the chi-square probability density function with two degrees of freedom, and $P_{FA}$ is the preset false alarm rate. Giving the probability $P_G$, the threshold value $\gamma$ can be determined using the chi-square distribution table.

Data Association: In order to facilitate the calculation of the association probabilities, we define the following association events, following the approach taken in [33]:

${\theta }_l\left(k\right)$: {$z_l\left(k\right)$ is determined from the LOS beacon nodes with the smallest error covariance, $l=1,\dots ,N_V\left(k\right)$}.

${\theta }_0\left(k\right)$: {none of the position estimates $z_l\left(k\right)$ at time step $k$ stems from the LOS beacon nodes}.

The associated probabilities are:

$${\beta }_l\left(k\right)=\mathrm{Pr}\left\{{\theta }_l\left(k\right)|Z^k\right\}=\mathrm{Pr}\left\{{\theta }_l\left(k\right)|Z\left(k\right),N_V\left(k\right),Z^{k-1}\right\}, l=1, \dots , N_V\left(k\right),$$

(31)

where $Z\left(k\right)={\left\{Z_l\left(k\right)\right\}}_{l=1}^{N_V\left(k\right)}$ and $Z^k$ is the cumulative set of the position estimation, i.e., $Z^k={\left\{Z\left(i\right)\right\}}_{i=1}^k$.

Since the associated probabilities are calculated based on the Bayesian formula, assuming that the innovations $v_{2,n}\left(k\right)$ are independent, Equation (31) can be rewritten as:

$${\beta }_l\left(k\right)=\frac{1}{c}f\left[Z\left(k\right)|{\theta }_l\left(k\right),N_V\left(k\right),Z^{k-1}\right]\times \mathrm{Pr}\left\{{\theta }_l\left(k\right)|N_V\left(k\right),Z^{k-1}\right\}, l=0, 1, \dots , N_V\left(k\right),$$

(32)

where $c$ is the normalization factor and $f(\cdot )$ is the joint probability density function for the position estimation.

Assuming that the probability density function of the correct position estimates obeys the Gaussian distribution, and the probability density function of the inaccurate position estimate obeys the uniform distribution, the probability density function for the correct position estimates is:

$$f\left[z_l\left(k\right)|{\theta }_l\left(k\right),N_V\left(k\right),Z^{k-1}\right]=P_G^{-1}N\left(z_l\left(k\right);\widehat{z}\left(k|k-1\right),S_{2,l}\left(k\right)\right),$$

(33)

$$=P_G^{-1}N\left(v_{2,l}\left(k\right);0,S_{2,l}\left(k\right)\right),$$

(34)

$$=P_G^{-1}\frac{\exp \left\{-\frac{1}{2}{v}_{2,l}^T\left(k\right)S_{2,l}^{-1}\left(k\right)v_{2,l}\left(k\right)\right\}}{2\pi {\left|S_{2,l}\left(k\right)\right|}^{0.5}},$$

(35)

where $\left|S_{2,l}\left(k\right)\right|$ denotes the determinant of matrix $S_{2,l}\left(k\right)$. The probability density function of the inaccurate position estimate is:

$$f\left[z_l\left(k\right)|{\theta }_l\left(k\right),N_V\left(k\right),Z^{k-1}\right]=V_l^{-1}\left(k\right),$$

(36)

$$V_l\left(k\right)=\gamma \pi {\left|S_{2,l}\left(k\right)\right|}^{0.5},$$

(37)

where $V_l\left(k\right)$ is the area of the validation region [33] of the $N_V\left(k\right)$ accepted hypothesis, and the prior probabilities in Equation (32) are:

$$\mathrm{Pr}\left\{{\theta }_l\left(k\right)|N_V\left(k\right),Z^{k-1}\right\}=\frac{P_d{P}_G}{N_V\left(k\right)}, l=1, \dots , N_V\left(k\right),$$

(38)

$$\mathrm{Pr}\left\{{\theta }_l\left(k\right)|N_V\left(k\right),Z^{k-1}\right\}=1-P_d{P}_G, l=0,$$

(39)

where the prior probabilities of the correct position estimates are given by Equation (35), and the prior probability of the inaccurate position estimate is given by Equation (36). The detection probability $P_d$ represents the probability that the position estimation that falls into the verification area can be correctly detected. The joint probability density function for the correct position estimates is:

$$f\left[Z\left(k\right)|{\theta }_l\left(k\right),N_V\left(k\right),Z^{k-1}\left(k\right)\right]={\displaystyle \prod _{\begin{array}{cc}i=1& i\ne l\end{array}}^{N_V\left(k\right)}\frac{N\left(v_{2,l}\left(k\right);0,S_{2,l}\left(k\right)\right)}{V_i\left(k\right)P_G}}, l=1, \dots , N_V\left(k\right).$$

(40)

The joint probability density function for an inaccurate position estimation is:

$$f\left[Z\left(k\right)|{\theta }_l\left(k\right),N_V\left(k\right),Z^{k-1}\left(k\right)\right]={\displaystyle \prod _{i=1}^{N_V\left(k\right)}{V}_i^{-1}}\left(k\right), l=0.$$

(41)

Multiplying the probability density function of the position estimates by the prior probabilities gives:

$${\beta }_l^{\prime }\left(k\right)=\frac{N\left(v_{2,l}\left(k\right);0,S_{2,l}\left(k\right)\right)}{P_G}\left(\frac{P_d{P}_G}{N_V\left(k\right)}\right){\displaystyle \prod _{\begin{array}{cc}i=1& i\ne l\end{array}}^{N_V\left(k\right)}{V}_i^{-1}}\left(k\right), l=1, \dots , N_V\left(k\right),$$

(42)

$${\beta }_0^{\prime }\left(k\right)=\left(1-P_d{P}_G\right){\displaystyle \prod _{i=1}^{N_V\left(k\right)}{V}_i^{-1}}\left(k\right), l=0,$$

(43)

Thus, the association probabilities of the correct position estimate are modeled as:

$${\beta }_l\left(k\right)=\frac{{\beta }_l^{\prime }\left(k\right)}{{\beta }_0^{\prime }\left(k\right)+{\displaystyle \sum _{l=1}^{N_V\left(k\right)}{\beta }_l^{\prime }\left(k\right)}}.$$

(44)

The association probability of an inaccurate position estimation is modeled as:

$${\beta }_0\left(k\right)=\frac{{\beta }_0^{\prime }\left(k\right)}{{\beta }_0^{\prime }\left(k\right)+{\displaystyle \sum _{l=1}^{N_V\left(k\right)}{\beta }_l^{\prime }\left(k\right)}}.$$

(45)

When updating, we need to obtain the innovation covariance matrix $S_2\left(k\right)$ and the Kalman gain $K_2\left(k\right)$:

$$S_2\left(k\right)=B{P}_2\left(k|k-1\right)B^T+{\sigma }_G^2{I}_2,$$

(46)

$$K_2\left(k\right)=P_2\left(k|k-1\right)B^T{S}_2^{-1}\left(k\right).$$

(47)

The updated state estimation ${\widehat{X}}_2\left(k|k\right)$ is given as:

$${\widehat{X}}_2\left(k|k\right)={\widehat{X}}_2\left(k|k-1\right)+K_2\left(k\right){\displaystyle \sum _{l=1}^{N_V\left(k\right)}{\beta }_l}\left(k\right)v_{2,l}\left(k\right).$$

(48)

The error covariance matrix $P_2\left(k|k\right)$ update is modeled as:

$$P_2\left(k|k\right)={\beta }_0\left(k\right)P_2\left(k|k-1\right)+\left(1-{\beta }_0\left(k\right)\right)P_c\left(k\right)+\tilde{P}\left(k\right),$$

(49)

$$P_c\left(k\right)=\left(I_4-K_2\left(k\right)B\right)P_2\left(k|k-1\right),$$

(50)

$$\tilde{P}\left(k\right)=K_2\left(k\right)\left[{\displaystyle \sum _{l=1}^{N_V\left(k\right)}{\beta }_l\left(k\right)v_{2,l}\left(k\right)v_{2,l}^T\left(k\right)-v_2\left(k\right)v_2^T\left(k\right)}\right]K_2^T\left(k\right),$$

(51)

$$v_2\left(k\right)={\displaystyle \sum _{l=1}^{N_V\left(k\right)}{\beta }_l\left(k\right)v_{2,l}\left(k\right)},$$

(52)

$${\mathsf{\Lambda }}_2\left(k\right)={\beta }_l\left(k\right){\mathsf{\Lambda }}_{2,l}\left(k\right)$$

(53)

$$={\beta }_l\left(k\right)N\left(v_{2,l}\left(k\right);0,S_{2,l}\left(k\right)\right),$$

(54)

where $P_c\left(k\right)$ is the posterior covariance matrix of the standard Kalman filter and $\tilde{P}\left(k\right)$ with the weighted innovation $v_2\left(k\right)$ corrects for the measurement uncertainty. ${\mathsf{\Lambda }}_2\left(k\right)$ is the likelihood function when $N_V\left(k\right)$ is larger than zero at time step $k$.

When $N_V\left(k\right)$ is zero at time step $k$, the algorithm judges whether $N_V\left(k\right)$ is also zero at time step $k-1$. The proposed algorithm records the situation of $N_V\left(k\right)$ at time step $k-1$ using the NLOS tracer method. If $L\left(k-1\right)$ is 1, it indicates that $N_V\left(k\right)=0$ appears separately, and the prediction of the state estimation and error covariance matrix are used as the updated state estimation and error covariance matrix:

$${\widehat{X}}_2\left(k|k\right)={\widehat{X}}_2\left(k|k-1\right),$$

(55)

$$P_2\left(k|k\right)=P_2\left(k|k-1\right),$$

(56)

$$S_2\left(k\right)=B{P}_2\left(k|k\right)B^T+3{\sigma }_G^2{I}_2,$$

(57)

$${\mathsf{\Lambda }}_2\left(k\right)=\frac{1}{2\pi {\left|S_2\left(k\right)\right|}^{0.5}}.$$

(58)

If $L\left(k-1\right)$ is zero, it indicates that $N_V\left(k\right)=0$ appears successively. Thus, we use the extended Kalman filter to update the state estimate and the error covariance matrix estimate in the NLOS model:

$$H_2\left(k\right)=\frac{\partial h\left(X\left(k\right)\right)}{\partial X\left(k\right)}{|}_{X\left(k\right)={\widehat{X}}_2\left(k|k-1\right)},$$

(59)

$$v_2\left(k\right)=D\left(k\right)-h\left({\widehat{X}}_2\left(k|k-1\right)\right),$$

(60)

$$S_2\left(k\right)=H_2\left(k\right)P_2\left(k|k-1\right)H_2\left(k\right)+R_2^{\ast }\left(k\right),$$

(61)

$$K_2\left(k\right)=P_2\left(k|k-1\right)H_2^T\left(k\right)S_2^{-1}\left(k|k-1\right),$$

(62)

$${\widehat{X}}_2\left(k|k\right)={\widehat{X}}_2\left(k|k-1\right)+K_2\left(k\right)v_2\left(k\right),$$

(63)

$$P_2\left(k|k\right)=\left(I_4-K_2\left(k\right)H_2\left(k\right)\right)P_2\left(k|k-1\right),$$

(64)

$${\mathsf{\Lambda }}_2\left(k\right)=N\left(v_2\left(k\right);0,S_2\left(k\right)\right).$$

(65)

3.4. Model Probability Update

The model probabilities are updated using:

$${\mu }_j\left(k\right)=\left(1/c\right){\mathsf{\Lambda }}_j\left(k\right){\overline{c}}_j,$$

(66)

$$c={\displaystyle \sum _j{\mathsf{\Lambda }}_j\left(k\right)}{\overline{c}}_j,$$

(67)

where ${\mu }_j\left(k\right)$ is the updated model probability and c is the normalization factor.

3.5. Combination

The combination output estimation results are as follows:

$$\widehat{X}\left(k|k\right)={\displaystyle \sum _j{\widehat{X}}_j\left(k|k\right)}{\mu }_j\left(k\right),$$

(68)

$$P\left(k|k\right)={\displaystyle \sum _j\left\{P_j\left(k|k\right)+\left[{\widehat{X}}_j\left(k|k\right)-X\left(k|k\right)\right]\times {\left[{\widehat{X}}_j\left(k|k\right)-X\left(k|k\right)\right]}^T\right\}}{\mu }_j\left(k|k\right).$$

(69)

4. Experiment and Result Analysis

This part mainly discusses the simulation results of the experiment. In this paper, six beacon nodes were randomly deployed in a 100 × 100 area, and the mobile node ran along a fixed trajectory shown in Figure 3 with a moving step of 100. The sampling period was $\Delta t=0.5 \mathrm{s}$ and the mobile node initial state $X_j\left(0\right)={\left[\begin{array}{cccc}1 \mathrm{m}& 20 \mathrm{m}& 1 \frac{\mathrm{m}}{\mathrm{s}}& 0.5 \frac{\mathrm{m}}{\mathrm{s}}\end{array}\right]}^T$. The initial of the error covariance matrix was set to $P_j\left(0\right)=I_4$, the Markov transition matrix initial value was $p=\left[\begin{array}{cc}0.995& 0.005\\ 0.005& 0.995\end{array}\right]$, and the measurement noise covariance matrix was $R_j^{\ast }\left(0\right)={\sigma }_m^2{I}_M$. The tracking threshold probability $P_G$ was 0.99, and the detection probability $P_d$ was 0.9. In order to simulate an NLOS environment, a probability value was generated randomly. Comparing this probability value with the NLOS probability threshold, if the probability that was generated randomly was less than the NLOS probability, the mobile node and the corresponding beacon nodes were considered to be in the NLOS condition. The simulation experiments were carried out under the conditions of NLOS errors obeying Gaussian, uniform, and exponential distributions. In this paper, we compared the proposed algorithm with the EKF [34], IMM-EKF [30], and MPDA [35]. The simulation results were obtained using 1000 Monte Carlo runs, and the root mean square error (RMSE) and the error cumulative distribution function (CDF) of the average positioning errors were used as the performance indicators for the evaluation algorithm:

$$RMSE=\sqrt{\frac{1}{MC}\frac{1}{K}{\displaystyle \prod _{j=1}^{MC}{\displaystyle \prod _{k=1}^K\left({\left({\widehat{x}}_j\left(k\right)-x_j\left(k\right)\right)}^2+{\left({\widehat{y}}_j\left(k\right)-y_j\left(k\right)\right)}^2\right)}}},$$

(70)

$$ALE=\frac{1}{MC\cdot K}{\displaystyle \sum _{j=1}^{MC}{\displaystyle \sum _{k=1}^K\sqrt{{({\widehat{x}}_j(k)-x_j(k))}^2+{({\widehat{y}}_j(k)-y_j(k))}^2}}},$$

(71)

where ALE is the average localization error, which represents the average Euclidean distance between the position estimate and the true position. $K=100$ is the number of moving steps, $MC=1000$ is the number of time the Monte Carlo simulation ran. $\left({\widehat{x}}_j\left(k\right),{\widehat{y}}_j\left(k\right)\right)$ is the position estimate of the interactive output in the $jth$ Monte Carlo run and $\left(x_j\left(k\right),y_j\left(k\right)\right)$ is the true position of the mobile terminal during the $jth$ Monte Carlo run.

4.1. Gaussian Distribution

It was assumed that the NLOS errors obeyed the Gaussian distribution $N\left({\mu }_{NLOS},{\sigma }_{NLOS}^2\right)$, the measurement noise obeyed the Gaussian distribution $N\left(0,{\sigma }_L^2\right)$, and the range measurements were irrelevant. The default parameters of the Gaussian distribution simulation experiment are shown in

Table 1. Gaussian distribution parameter.

Parameter	Symbol	Default Values
The number of beacon nodes	$M$	6
NLOS error probability	$P_{NLOS}$	0.5
The measurements’ noise	$N\left(0,{\sigma }_L^2\right)$	$N\left(0,1^2\right)$
The NLOS errors	$N\left({\mu }_{NLOS},{\sigma }_{NLOS}^2\right)$	$N\left(5,6^2\right)$

.

As shown in Figure 4a, with the number of beacon nodes increasing from 4 to 9, the RMSEs of the four algorithms decreased. This indicates that increasing the number of beacon nodes was helpful for reducing the NLOS errors and improving the positioning accuracy of the algorithms. When the number of beacon nodes varied from 4 to 7, the RMSEs of the IMM-EKF, MPDA, and the proposed algorithm decreased rapidly. When the number of beacon nodes increased to 7, the RMSEs of the MPDA and the proposed algorithm decreased slowly, and the RMSE of the IMM-EKF hardly changed. The RMSE of the EKF decreased slowly. From Figure 4a, we can see that the positioning accuracy of the proposed algorithm was better than that of the EKF, IMM-EKF, and MPDA. When the number of beacon nodes was 9, the proposed algorithm had a higher positioning accuracy than the EKF, IMM-EKF, and MPDA at about 65.70%, 59.97%, and 25.16%, respectively.

Figure 4b shows the impact of the NLOS errors probability on RMSE. Compared with the EKF and IMM-EKF algorithms, the RMSE of the proposed algorithm increased slowly when the probability of the NLOS error was less than 0.4. However, when the probability of the NLOS error was greater than 0.4, the RMSE of the proposed algorithm increased rapidly with the increase of the NLOS errors probability. The change of root mean square error of the MPDA algorithm was similar to that of the proposed algorithm, but its growth rate was faster than that of the proposed algorithm. When compared with EKF, IMM-EKF, and MPDA, the proposed algorithm had a higher positioning accuracy.

Figure 4c shows the relationship between the RMSE and the mean value of the NLOS errors. As the mean value of the NLOS error varied from 3 to 10, the RMSE of the EKF, IMM-EKF, and MPDA increased. Although the RMSE of the proposed algorithm also increased, its RMSE was always smaller than that of the EKF, IMM-EKF, and MPDA. From Figure 5, we can see that the growth rate of the root mean square error of the proposed algorithm was obviously less than that of other three algorithms. When the mean value of NLOS error was 3, the proposed algorithm improved the positioning accuracy by about 47.22%, 32.72%, and 28.98% compared with the EKF, IMM-EKF, and MPDA, respectively. When the mean value of the NLOS error was 10, the proposed algorithm had a higher positioning accuracy than the EKF, IMM-EKF, and MPDA at about 35.76%, 36.67%, and 30.06%, respectively.

Figure 4d shows the impact of the standard deviation of NLOS errors on the RMSE. The RMSE of the EKF clearly increased with the increase of the standard deviation, which led to a rapid decline in its positioning accuracy. When the standard deviation was 10, the RMSE of the EKF, IMM-EKF, MPDA, and the proposed algorithm were 5.3948 m, 3.9240 m, 3.2254 m, and 2.0257 m, respectively. Compared with the EKF, IMM-EKF, and MPDA, the proposed algorithm improved the positioning accuracy by about 62.45%, 48.38%, and 37.20%, respectively.

Figure 4e shows the error cumulative distribution function of the localization errors, which shows that the 90th percentile of the errors of the proposed algorithm was less than 3.263 m. In contrast, the 90th percentile of errors of the EKF, IMM-EKF, and MPDA were 6.668 m, 5.494 m, and 4.191 m, respectively.

4.2. Uniform Distribution

It was assumed that the measurements’ noise obeyed a Gaussian distribution $N\left(0,{\sigma }_L^2\right)$ and the NLOS errors obeyed a uniform distribution $U\left(a,b\right)$. The default parameters for the uniform distribution simulation experiment are shown in

Table 2. Uniform distribution parameter.

Parameter	Symbol	Default Values
The number of beacon nodes	$M$	6
The NLOS error probability	$P_{NLOS}$	0.5
The measurements’ noise	$N\left(0,{\sigma }_L^2\right)$	$N\left(0,1^2\right)$
The NLOS errors	$U\left(a,b\right)$	$U\left(0,14\right)$

.

Figure 5a shows the relationship between the RMSE and the number of beacon nodes when the NLOS errors took on a uniform distribution. It can be seen that the number of beacon nodes varied from 4 to 9, and the RMSEs of the EKF and IMM-EKF algorithms decreased slowly in a similar trend. The change in the number of beacon nodes had a great impact on the positioning results of the MPDA and the proposed algorithm. The proposed algorithm had a higher positioning accuracy than the EKF, IMM-EKF, and MPDA with about 39.70%, 34.81%, and 18.33%, respectively, on average.

As shown in Figure 5b, increasing the probability of the NLOS error aggravated the interference of the NLOS factors for positioning, which resulted in increasing the RMSE of the EKF, IMM-EKF, MPDA, and the proposed algorithm. We can see that the RMSE of the proposed algorithm increased slowly with the increase of the probability of the NLOS errors when the probability of the NLOS errors was not more than 0.4, but the growth rate increased gradually. On the whole, the proposed algorithm showed a significant improvement in positioning accuracy compared with the EKF and IMM-EKF algorithms when the NLOS error probability was not higher than 0.6. Compared with the MPDA, the proposed algorithm had a higher positioning accuracy.

Figure 5c shows how the RMSE varied with the maximum value of the NLOS errors. The RMSEs of the EKF, IMM-EKF, MPDA, and the proposed algorithm increased almost linearly as the maximum value of the NLOS errors gradually increased. Compared with the EKF and IMM-EKF algorithms, the proposed algorithm had a greatly improved positioning accuracy. When the maximum of the NLOS error was 15 m, the proposed algorithm had a higher positioning accuracy than the EKF, IMM-EKF, and MPDA with about 39.90%, 34.89%, and 21.67%, respectively. It can be seen from Figure 5c that the proposed algorithm significantly outperformed the other three algorithms in terms of inhibiting NLOS errors.

The error cumulative distribution function of the localization errors for the same example is depicted in Figure 5d, which shows that the 90th percentile of the proposed algorithm was less than 5.555 m. In contrast, the 90th percentile of the EKF, IMM-EKF, and MPDA were 8.566 m, 8.228 m, and 7.829 m, respectively.

4.3. Exponential Distribution

It was assumed that the measurements’ noise obeyed a Gaussian distribution $N\left(0,{\sigma }_L^2\right)$ and the NLOS errors obeyed an exponential distribution E(λ). The default parameters of the exponential distribution are shown in the

Table 3. Exponential distribution parameter.

Parameter	Symbol	Default Values
The number of beacon nodes	M	6
The NLOS error probability	$P_{NLOS}$	0.5
The measurements noise	$N\left(0,{\sigma }_L^2\right)$	$N\left(0,1^2\right)$
The NLOS errors	E(λ)	E(8)

.

We investigated how the positioning accuracy of the EKF, IMM-EKF, MPDA, and the proposed algorithm varied with the number of beacon nodes when the NLOS errors were exponential distribution (Figure 6a). As the number of beacon nodes increased, the RMSEs of the EKF, IMM-EKF, MPDA, and the proposed algorithm all decreased, but the difference was that the RMSEs of the EKF and IMM-EKF decreased slowly and the improvement of the positioning accuracy was not high, while the RMSEs of the MPDA and the proposed algorithms decreased rapidly and the positioning accuracy improved significantly. In the case of a few beacon nodes, the positioning accuracy of the proposed algorithm was higher than that of the EKF, IMM-EKF, and MPDA algorithms. In the case of more beacon nodes, the positioning accuracy of the proposed algorithm was slightly higher than that of the MPDA, and far higher than that of the EKF and IMM-EKF. Compared with the EKF, IMM-EKF, and MPDA algorithms, the proposed algorithm improved the positioning accuracy by 63.60%, 59.57%, and 20.27%, respectively, on average.

We also explored the influence of different NLOS errors probabilities on the positioning results of the EKF, IMM-EKF, MPDA, and the proposed algorithm when NLOS errors were given as an exponential distribution. From Figure 6b, we can see that the positioning accuracy of the EKF, IMM-EKF, MPDA, and the proposed algorithm declined with the increasing probability of NLOS errors. With the increase of the NLOS error probability, the RMSEs of the EKF and IMM-EKF increased almost linearly, and the RMSE growth rate of the MPDA and the proposed algorithm gradually accelerated. When the NLOS error was less than 0.3, the positioning accuracy of the proposed algorithm was slightly higher than that of the MPDA, but significantly higher than that of the EKF and IMM-EKF. When the probability of the NLOS errors was relatively large, the proposed algorithm had a better localization performance than the EKF, IMM-EKF, and MPDA. The proposed algorithm had a higher positioning accuracy than the EKF, IMM-EKF, and MPDA with about 65.83%, 53.01%, and 18.56%, respectively, on average.

In Figure 6c, the influence of changing the parameter λ of exponential distribution on the localization accuracy is shown. We can see that when the parameter λ changed from 3 to 10, the RMSEs of the EKF and IMM-EKF algorithm increased rapidly, while the RMSE of the MPDA algorithm grew slowly. The change of the parameter λ from 3 to 10 had no significant impact on the localization accuracy of the proposed algorithm. Compared with the EKF, IMM-EKF, and MPDA algorithms, the proposed algorithm improved the positioning accuracy by at least 36.16%, 33.35%, and 11.43%, respectively.

The error cumulative distribution function of the localization errors is depicted in Figure 6d. It shows that the 90th percentile of the proposed algorithm was less than 3.882 m. In contrast, the 90th percentile of the EKF, IMM-EKF, and MPDA were achieved at 12.22 m, 10.54 m, and 5.065 m, respectively.

4.4. Experimental Results

In order to further verify the positioning accuracy of the proposed algorithm, we carried out experiments in a real environment. Ultra-wideband (UWB) technology was used to transmit signals between beacon nodes and a mobile node to obtain the range measurements. The time-based UWB positioning system makes use of the accurate TOA of the information exchange between devices [36] and has a high positioning accuracy. Therefore, in recent years, UWB has been widely used in indoor positioning. As shown in Figure 7a, there were six beacon nodes and a mobile node moved uniformly along the trajectory shown. In order to avoid the reflection of UWB signal from the ground, the mobile node was moved to 1.2 m above the ground. The room was 10 meters long and 7 meters wide. Because there were many obstacles in the room, the measurements were prone to be disturbed by NLOS factors, resulting in larger errors. Six beacon nodes sampled mobile nodes every 0.6 m for a total of 16 times. Each beacon node took 20 measurements at each sampling position, and the average value was taken as the final measurement.

The initial state of the mobile node was set $X\left(0\right)={\left[\begin{array}{cccc}1.2 \mathrm{m}& 1.8 \mathrm{m}& 0.6 \mathrm{m}/\mathrm{s}& 0 \mathrm{m}/\mathrm{s}\end{array}\right]}^T$, the sampling period Δt was 1s, and other parameters were consistent with the simulation. The error distribution of each sample point is shown in Figure 7b, and the CDF of the localization error is shown in Figure 7c. Because there were some obstacles in the experimental environment, and the interference of NLOS factors was serious, the localization errors of the EKF and IMM-EKF were relatively large in most cases. Compared with the EKF and IMM-EKF, the localization errors of the MPDA and the proposed algorithm were smaller at most sample points, but the positioning accuracy of the proposed algorithm was slightly higher than that of the MPDA.

5. Conclusions

This paper proposed an NLOS error mitigation algorithm for tracking a mobile node based on the TOA measurements in a mixed LOS/NLOS environment. An improved MPDA algorithm with a stronger robustness was proposed in this paper. We applied the improved MPDA filter to the IMM-EKF algorithm framework. In this paper, an extended Kalman filter was used in the LOS model, and an improved MPDA filter was used in parallel with the NLOS model. The second filter discarded the position estimates with large errors through NLOS recognition to mitigate the NLOS errors. After updating the model probability of each model, the state estimation calculated using the two filters was based on the updated model probability weighted combination to obtain the final state estimation. The simulation and experimental results showed that the proposed algorithm outperformed the EKF, IMM-EKF, and MPDA in an NLOS environment. The proposed algorithm had a high positioning accuracy in the case of a small NLOS error probability. In the case of a high probability of NLOS error, the positioning accuracy of the proposed algorithm decreased greatly. In the future, the intended further improvement involved increasing the positioning accuracy of the proposed algorithm in the case of a high probability of NLOS error.