An Indoor Localization Algorithm based on Improved Particle Filter and Directional Probabilistic Data Association for Wireless Sensor Network

Abstract

As an important technology of the internetwork, wireless sensor network technique plays an important role in indoor localization. Non-line-of-sight (NLOS) problem has a large effect on indoor location accuracy. A location algorithm based on improved particle filter and directional probabilistic data association (IPF-DPDA) for WSN is proposed to solve NLOS issue in this paper. Firstly, the improved particle filter is proposed to reduce error of measuring distance. Then the hypothesis test is used to detect whether measurements are in LOS situations or NLOS situations for N different groups. When there are measurements in the validation gate, the corresponding association probabilities are applied to weight retained position estimate to gain final location estimation. We have improved the traditional data association and added directional information on the original basis. If the validation gate has no measured value, we make use of the Kalman prediction value to renew. Finally, simulation and experimental results show that compared with existing methods, the IPF-DPDA performance better.

1. Introduction

Wireless sensor network (WSN) is an important technology of the Internet, which is more and more widely used in daily life [1]. Indoor positioning is becoming more and more important in WSN. Indoor localization technology based on distance general uses time difference of arrival (TDOA), received signal strength (RSS), time of arrival (TOA), angle of arrival (AOA), [2] [3] [4], etc.

In this paper, the Euclidean distance measurements are obtained between mobile node (MN) and beacon nodes (BNs) through TOA method. In a NLOS case, by reason of the presence of barriers, the measured time will be longer than in LOS. This will lead to measured distance to be larger than Euclidean distance, which is called NLOS error [5]. In practice, all kinds of indoor objects can be regarded as obstacles. Therefore, the designed algorithm is necessary to reduce the NLOS errors.

There are some traditional mitigate NLOS errors algorithms, Particle Filter (PF), Average Filter (AF), Gaussian Filter (GF), Kalman Filter (KF), Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) so on. These algorithms can’t achieve high positioning accuracy. PF [6] uses some random sampling points, namely particles, to describe the probability of target location. Then, through the weighted value of each particle, the estimated value of the MN is obtained.

In LOS case, EKF algorithm [7] has high positioning exactitude, but the positioning exactitude is dramatically reduced when there is NLOS interference. The interactive multi-model (IMM) algorithm has strong robustness, so many new algorithms are derived from the IMM algorithm to solve the problem of NLOS interference [8] [9] [10]. The disadvantage of IMM algorithm is that the statistical error needs to be known in advance.

REKF algorithm is a semi-parametric estimation algorithm based on TOA estimation [11], which has certain robustness. The REKF algorithm uses the Huber robust M estimator [12] to generate a linear regression model to process the estimated position, and to find the estimated value that meets the preset accuracy through multiple iterations, thereby mitigating the influence of NLOS. Replace NLOS filter in IMM algorithm with REKF, which is RIMM algorithm [13]. Although the statistical information of NLOS errors of RIMM does not need to be known in advance, cc1 and cc2 are set by ourselves. The MPDA [14] performs well in LOS. However, it is often easy to lose the target when the NLOS probability increases.

Due to the separate operation of each BN in the original PF, there is no connection between the various BNs. We hope to improve the existing PF to enable BNs to work together, thereby further reducing the impact of NLOS. Traditional PDA filters only use distance information when calculating correlation probabilities, and we found that direction is also an important factor. Therefore, we attempted to add direction information to the data association. In this paper, an algorithm IPF-DPDA is proposed for the above problems, this paper mainly makes the following contributions:

1) We have improved the traditional data association and added directional information on the original basis. The DPDA uses Mahalanobis distance and trajectory direction to compute the association probabilities to obtain more accurate location estimate.

2) Parallel with the previous particle filter BNs working alone, the improved particle filter BNs work together to alleviate negative impact of NLOS error. Improved particle filter significantly reduces the measurement distance error caused by NLOS.

3) The IPF-DPDA does not need to know the prior knowledge of NLOS errors in advance. Nevertheless, simulation and experimental results display that IPF-DPDA is better than those algorithms that need to know a priori information and performs better when the NLOS errors follow different distributions.

The whole paper is arranged as follows. Section II describes the related work. Section III introduces proposed algorithm. Simulations and experiment are described in Section IV, and Section V gives the conclusion.

2. Related Works

Many scholars have done research in the field of indoor location for the sake of improving positioning accuracy. In [15], Zhou proposed a method of Asymptotic Relative Efficiency (ARE), and based on this idea, the new hybrid hypothesis test is designed, which is applied to raise the accuracy of Wi-Fi indoor location. Compared to traditional WIFI positioning, the author not only considers the diversity of signal distribution but also considers the error of RSS measurement values, thus improving the positioning accuracy. In [16], Generative countermeasure network is proposed for RSSI data enhancement. The network uses a small part of real labeled data to generate false RSSI data. This algorithm also improves the location accuracy of indoor positioning. Compared to traditional methods, this method reduces the cost of data collection and reduces training data. A new ultrasonic signal classification and suppression algorithm is proposed for NLOS [17]. The algorithm processes the amplitude and residual of the received signal, and uses these two characteristics to sort and suppress NLOS. This algorithm has great advantages compared to other algorithms in classifying narrowband signals containing severe multipath effects. In [18], Zhou studied the location error based on hybrid fingerprint. The final results show that using the relevance between different forms of fingerprints can significantly mitigate positioning errors. When the number of fingerprints decreases, the location errors will increase. This is the drawback of this algorithm.

Combinational localization algorithm is also a current research trend [19-23]. In [19], A Pedestrian Dead Reckoning (PDR) and TDOA idea are integrated for indoor position. Launch a wall mirror of a NLOS BSs to generate a virtual base station (VBS), which is considered LOS BSs. After completing this process, more LOS BSs will participate in positioning. Compared to traditional algorithms, this algorithm solves the problem of unknown regions being unable to locate. In [20], Chang combined RSS and AOA methods to solve the positioning of mobile node in 3D. Simulations demonstrate their algorithm can achieve great performance in different situations. The main advantage of this algorithm is that it can work effectively regardless of whether the target's transmission power is known or unknown, which reflects the universality of the algorithm. In [21], identifies and selects the LOS and NLOS measurements which have better channel condition and geometric dilution of precision (GDOP) that derived by the hybrid Round-Trip Time (RTT), TDOA and direction of arrival (DOA). More measurement information fusion can reduce positioning errors, which is also a current research trend. In [22], an indoor location method combining Wi-Fi RTT and PDR is proposed. For the sake of increasing positioning accuracy of Wi-Fi RTT, an adaptive filtering system is formed by combining multiple EKF with a new anomaly test means. Tomic transformed the location issue into a generalized trust region subproblem based on TOA and RSS [23], and then solves it by bisection program. Because the difficulty of the research content, converting to a generalized trust domain is only an approximate transformation, which may affect the accuracy of the algorithm.

3. Proposed Method

3.1 Signal Model

In the two-dimensional positioning area, it has MM BNs around MN. There may or may not be obstacles between MN and BNs. The state vector of MN is X(l) = [x(l) y(l) ẋ(l) ẏ(l) ]^T, (x(l), y(l)) denotes the position of the MN and (ẋ(l), ẏ(l)) denotes the velocity of MN. Then the state equation is modeled as:

X(l) = FX(l - 1) + w       (1)

where, \(\begin{align}F=\left[\begin{array}{cccc}1 & 0 & T_{S} & 0 \\ 0 & 1 & 0 & T_{S} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right]\end{align}\) denotes the state transition matrix of MN, and w denotes measurement noise, T_S is sampling interval. We suppose MM BNs to detect the location of MN. We let d_i(l) = [d₁(l), d₂(l), … d_M(l)] express that the measured distance is between MM BNs and MN:

d_i(l) = h(X(l) + n, l = 1, ..., L;i = 1, ..., M       (2)

where, h(X(l)) = [h₁(X(l)), h₂(X(l)), … , h_M(X(l))]^T indicate the Euclidean distance between MN and the m − th BN with the position (x_bn,m, y_bn,m) is:

\(\begin{align}h_{m}(X(l))=\sqrt{\left(x(l)-x_{b n, \mathrm{~m}}\right)^{2}+\left(y(l)-y_{b n, m}\right)^{2}}, m=1, \ldots, M\end{align}\)       (3)

The noise n is defined as:

\(\begin{align}n=\left\{\begin{array}{cc}n_{L O S} & \text { LOS condition } \\ n_{L O S}+n_{N L O S} & \text { NLOS condition }\end{array}\right.\end{align}\)       (4)

where, n_LOS follows the Gaussian distribution n_LOS~N(0, 1²) The n_LOS represents the NLOS error, as the environment changes, it may follow different distributions.

3.2 General Concept

The flow-process diagram of IPF-DPDA is showed in Fig. 1. We take for that incipient value and covariance of state estimation are computed ahead of time in this essay. We make use of M BNs and P particles to locate MN. At time ll, the measured distance between BN and MN is recorded as d^l_i. The distance between the p − th particle and the i − th BN is recorded as d^l_i,p. Then calculate the residuals of d^l_i and d^l_i,p. Select P particles according to the obtained residuals. The first choice is to calculate the sum of residuals from each particle to each BN, and then normalize it. Particles with normalized weight greater than the average weight are retained. The second choice is to calculate the residual of the remaining particles again. If it is not less than the threshold (LOS) of the calculated, the particles will be retained, otherwise they will be discarded. Distance selection: carry out residual processing between the estimated distance and the measured distance. If the residual outweigh the average distance residual (regarded as NLOS), measured distance is updated with the estimated distance obtained from remaining particles. On the contrary, the distance is still measured.

Fig. 1. The flowchart of IPF-DPDA

Then, using the grouping idea and the least-squares method, C³_M measurements are obtained, C³_M = N. These N different position estimates are detected through the validation gate, and what inside validation gate is correct location estimated. Furthermore, those left outside the validation gate are discarded. If there are measured values inside validation gate, the corresponding association probabilities are applied to weight retained position estimate to obtain final position estimation. If the number of inside validation gate is 0, update with predicted state estimate.

3.3 Improved Particle Filter

1) Particles Weighting: The weight of particles is related to the final distance calculation and update. The Weighting Particles has three steps:

Firstly, at time l, the distance difference between the p − th particle and the MN to the i − th BN is calculated. The distance difference can be represented as res^l_i,p. The specific expression is as follows:

res^l_i,p = |d^l_i,p - d^l_i|, i = 1, ...M, p = 1, ..., P       (5)

Secondly, we compute the total residual from the p − th particle to each BN, which can be represented as res^l_p. The specific expression is as follows:

\(\begin{align}\operatorname{res}_{p}^{l}=\sum_{i=1}^{M}\left|d_{i, p}^{l}-d_{i}^{l}\right|, p=1, \ldots, P\end{align}\)       (6)

Finally, the reciprocal of the total of the residuals of the p − th particle is obtained. The specific expression is as follows:

W^l_p = 1/res^l_p       (7)

2) First Selection: We can know that when the particle is closer to MN, res^l_p is smaller and W^l_p is larger. When making the first selection, the particles with greater weight are retained.

\(\begin{align}\begin{array}{l}H_{0}: \mathrm{W}_{j}^{l} \geq \sum_{i=1}^{P} W_{i}^{l} / P, j=1, \ldots, P \\ H_{1}: \mathrm{W}_{j}^{l}<\sum_{i=1}^{P} W_{i}^{l} / P, j=1, \ldots, P\end{array}\end{align}\)       (8)

If H₁ is false, the j − th particle is retained. Otherwise, the j − th particle is discarded. We assume that after the first selection, S particles are retained. However, if multiple BNs misjudge the particles, the particles can also be selected for the first time. Therefore we make a second selection.

3) Second Selection: We can use the residuals of the first step to approximate the probability of NLOS error.

\(\begin{align}\begin{array}{l}H_{0}: \operatorname{res}_{i, j}^{l} \geq \sum_{p=1}^{p} \operatorname{res}_{p}^{l} / M * P, i=1, \ldots, M ; j=1, \ldots, P \\ H_{1}: \operatorname{res}_{i, j}^{l}<\sum_{p=1}^{p} \operatorname{res}_{p}^{l} / M * P, i=1, \ldots, M ; j=1, \ldots, P\end{array}\end{align}\)       (9)

We define the starting value of m as 0. If H₀ holds true, then m plus one, the cumulative value of m is obtained at last. Finally, the percentage of the value of m in the total is the probability of NLOS, which is recorded as λ.

λ = m/P * M       (10)

So, there are approximately M ∗ (1 − λ) BNs are in LOS.

thr = M * (1 - λ)       (11)

We use the particles left by the first selection to make the second selection. The second selection avoids possible errors in the first selection. The specific explanation is as follows:

 \(\begin{align}\begin{array}{l}H_{0}: \operatorname{res}_{i, j}^{l} \leq \sum_{p=1}^{p} \operatorname{res}_{p}^{l} / P^{*} M, i=1, \ldots, M ; j=1, \ldots, S \\ H_{1}: \operatorname{res}_{i, j}^{l}>\sum_{p=1}^{p} \operatorname{res}_{p}^{l} / P^{*} M, i=1, \ldots, M ; j=1, \ldots, S\end{array}\end{align}\)       (12)

We define the starting value of n as 0. Calculate whether the residual from the j − th particle to M BNs meets H₀. If H₀ holds true, then nn plus one, the cumulative value of n is obtained at last. If n ≥ thr, particle reserved, otherwise, particle discard. After calculating the j − th particle, redefine nn as 0 and continue to calculate the next particle. Repeat this process until all saved particles are calculated for the first time.

When the two choices are completed, the algorithm uses the retained particles to calculate the distance to each BN. If the second selection does not leave any particles. Then, we select particles again by reducing the threshold by 1. Suppose U particles are retained after two selections.

4) Distance Update: We normalize weight of the remaining U particles, after normalization, weight of the p − th particle is MW^k_p at time l. The estimated distance dis^l_i between MN and i − th BN is fitted by weight of the reserved particle and the distance to the i − th BN.

\(\begin{align}d i s_{i}^{l}=\sum_{p=1}^{U} M W_{p}^{l} * d_{i, p}^{l}, i=1, \ldots, M\end{align}\)       (13)

After obtaining the estimated distance, we use the actual measured distance and estimated distance to calculate the residual and average residual as follows:

res1^l_i = |dis^l_i - d^l_i|, i = 1, ..., M       (14)

We apply the acquired distance residuals to judge whether BN is in LOS relative to MN. It should be emphasized here that the i − th residual at this time is greater than the average residuals this time. If this condition is met, d^l_i = dis^l_i. That is the estimated distance obtained by the improved particle filter is be applied to update measured distance. Otherwise, the measured distance remains unchanged. The reason for this is to avoid the measurement distance being incorrectly updated.

5) Particle Copy and Movement: After the distance update is complete, the particles retained after two selections will be used in the next distance update. We constructed a replication layer for this purpose. The length of each layer is weight value of the particles, and total length is 1. We set layer L as follows:

\(\begin{align}L=\left(0, M W_{1}^{l}, M W_{1}^{l}+M W_{2}^{l}, \ldots, \sum_{i=1}^{L-1} M W_{i}^{l}, 1\right)\end{align}\)       (15)

Then generate a random number between 0 and 1, we set the generated random number as r. On which layer is the random number located, the corresponding particle is copied once, P times in total. This method ensures that the larger weight, the greater probability of particles will be copied.

The resulting new particles move in a small range, preventing these particles from concentrating in one area. The moved particles are used for the next distance update. The following equation represents the movement of particles:

(X^l+1_p, Y^l+1_p) = (X^l_p, Y^l_p) + N(0, σ²_p)       (16)

3.4 Grouping and Positioning

We use LS method to estimate the position of MN, and estimate one position coordinate every three distance measurements. We assume the position of MN is(x(l), y(l)) , There is the following relation between BNs and MN:

\(\begin{align}\left\{\begin{array}{c}\left(x_{1}-x(l)\right)^{2}+\left(y_{1}-y(l)\right)^{2}=\left(d_{1}(l)\right)^{2} \\ \vdots \\ \left(x_{M}-x(l)\right)^{2}+\left(y_{M}-y(l)\right)^{2}=\left(d_{M}(l)\right)^{2}\end{array}\right.\end{align}\)       (17)

Equation (17) can be represented by matrix AP = B.

\(\begin{align}\begin{array}{l}A=2\left[\begin{array}{ccc}\left(x_{1}-x_{2}\right) & , & \left(y_{1}-y_{2}\right) \\ \left(x_{1}-x_{3}\right) & , & \left(y_{1}-y_{3}\right) \\ & \vdots & \\ \left(x_{1}-x_{M}\right) & , & \left(y_{1}-y_{M}\right)\end{array}\right] \quad \boldsymbol{P}=\left[\begin{array}{c}x(l) \\ y(l)\end{array}\right] \\ B=\left[\begin{array}{c}d_{2}(l)^{2}-d_{1}(l)^{2}-\left(x_{2}{ }^{2}+y_{2}{ }^{2}\right)+\left(x_{1}{ }^{2}+y_{1}{ }^{2}\right) \\ d_{3}(l)^{2}-d_{1}(l)^{2}-\left(x_{3}{ }^{2}+y_{3}{ }^{2}\right)+\left(x_{1}{ }^{2}+y_{1}{ }^{2}\right) \\ \vdots \\ d_{M}(l)^{2}-d_{1}(l)^{2}-\left(x_{M}{ }^{2}+y_{M}{ }^{2}\right)+\left(x_{1}{ }^{2}+y_{1}{ }^{2}\right)\end{array}\right]\end{array}\end{align}\)       (18)

At time l, the estimated position coordinates can be derived from the above equation as follows:

z(l) = [x(l), y(l)]^T = (A^T A)^-1A^TB       (19)

So total estimated position coordinates are z_n(l), n = 1, ...N. Then we use the Gauss Newton algorithm [24] to iteratively discover minimum value of quadratic cost function, in which the nonlinear h(·) is approximated by the first-order Taylor series.

z_n,s+1 = z_n,s + (H_n(z_n,s)^TH_n(z_n,s))^-1 * H_n(z_n,s)(d - h_n(z_n,s)), n = 1,2, ..., N       (20)

where s is the iteration index, ss plus one for each iteration. h_n(z_n,s) represents in subgroup nn the Euclidean distances between the three corresponding BSs and z_n,s. The Jacobian matrix H_n(z_n) is defined as:

\(\begin{align}H_{n}\left(\mathrm{z}_{n}\right)=\left.\frac{\partial h_{n}\left([x, y]^{T}\right)}{\partial[x, y]^{T}}\right|_{x=x, y=y}\end{align}\)       (21)

The iterations are repeated until between z_n,s and z_n,s+1 is smaller than a certain threshold and when iteration stops the value of s is less than 7. Finally, let z_n,s+1 be the final position estimate.

3.5 Algorithm

1) Kalman Filter Prediction: we assume that initial value \(\begin{align}\hat{X}\end{align}\)(0|0) and covariance of state estimation \(\begin{align}\hat{P}\end{align}\)(0|0) are computed ahead of time. The prediction state and covariance matrix estimation are obtained by KF:

\(\begin{align}\hat{X}(l \mid l-1)=F \hat{X}(l-1 \mid l-1)\end{align}\)       (22)

P(l|l - 1) = FP(l - 1|l - 1)F^T + CQC^T       (23)

\(\begin{align}\hat{z}(l \mid l-1)=B \hat{X}(l \mid l-1)\end{align}\)       (24)

where \(\begin{align}B=\left[\begin{array}{llll}1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0\end{array}\right]\end{align}\), \(\begin{align}C=\left[\begin{array}{cc}T_{S}{ }^{2} / 2 & 0 \\ 0 & T_{S}{ }^{2} / 2 \\ T_{S} & 0 \\ 0 & T_{S}\end{array}\right]\end{align}\), Q = I₂, and \(\begin{align}\hat{z}(l \mid l-1)\end{align}\) represents the predicted coordinates of MN.

2) NLOS Detection: First, we get the difference between N measurements and predicted values.

\(\begin{align}v_{n}(l)=z_{n}(l)-\hat{z}(l \mid l-1), n=1, \ldots, N\end{align}\)       (25)

Then, we judge whether the N position estimation is in LOS or NLOS. If there are no NLOS errors, then there are:

v_n(l) ~ N(0, S_n(l)), n = 1, ..., N       (26)

where, S_n(l) is innovation covariance matrix of the n − th subgroup, calculated by the following equation:

S_n(l) = BP(l|l - 1)B^T + σ²_L(H^T_n(z_n)H_n(z_n))^-1       (27)

We can calculate the area of the validation gate through the innovation covariance:

V_n(l) = γπ|S_n(l)|^0.5       (28)

To test (26), we define N hypotheses and another N alternative hypotheses:

ζ_0,n : v_n(l) ~ N(0, S_n(l), n = 1, ..., N

ζ_1,n : not ζ_0,n, n = 1, ..., N       (29)

If ζ_0,n is true, BNs of the n − th subgroups are in LOS. Otherwise, the BNs are in NLOS. To detect z_n(l) whether is in the LOS condition, that is, whether it's inside validation gate. We can compare test statistic T_n(l) with threshold of the validation gate γ to judge the position estimation z_n(l) Whether is in LOS or NLOS condition. T_n(l) is calculated as follows:

T_n(l) = v^T_n(l)S^-1_n(l)v_n(l)       (30)

If T_n(l) is smaller than γ, z_n(l) is in LOS, z_n(l) is inside validation gate, 𝜁_0,n is true. Otherwise, 𝜁_1,n holds true. γ is associated with the threshold probability P_G, P_G means the probability of a measurement from LOS BNs falls in the validation gate. The relationship between the two is as follows:

\(\begin{align}\int_{0}^{\gamma} f_{x^{2}(2)}(x) d x=P_{G}=1-P_{F A}\end{align}\)       (31)

where f_𝜒2(2)(·) indicates the probability density function of chisquare distribution with two degrees of freedom. Due to the complexity of the indoor environment, it is impossible for us to know in advance the NLOS situation in this indoor environment. Therefore, we chose a suitable empirical P_G value [14]. When P_G is known, the chi-square distribution table can get value of γ.

We ruled out z_n(l) outside the validation gate and count number that in the validation gate, record it as N_v(l).

3) Data Association and Update: We set up following associated events according to the method of [25]:

𝜃_q(l) : {z_q(l) is depend on the LOS BSs with the smallest error covariance, q = 1, …, N_V(l)}.

𝜃₀(l): {none of the location estimations z_q(l) originates from the LOS BNs}.

The associated probabilities are:

β_q(l) = Pr{θ_q(l)|Z^l} = Pr{θ_q(l)|Z(l), N_V(l), Z^l-1}. q = 1, ..., N_V(l)       (32)

where Z(l) = {Z_q(l)}^N_V(l)_q=1 and Z^l is the cumulative set of the location estimated.

Since Bayesian formula is the basis of association probability calculation, assuming that the innovations v_q(l), q = 1, ..., N_V(l) are mutually independence, then (32) is:

\(\begin{align}\beta_{q}(l)=\frac{1}{c} f\left[Z(l) \mid \theta_{q}(l), N_{V}(l), Z^{l-1}\right] \times \operatorname{Pr}\left\{\theta_{q}(l) \mid N_{V}(l), Z^{l-1}\right\}, q=0,1, \ldots, N_{V}(l)\end{align}\)       (33)

where c and f(·) represent normalization factor and joint probability density function for the location estimated, respectively.

We assume that the probability density functions of correct location estimation and incorrect location estimation obey Gaussian distribution and uniform distribution respectively. The probability density functions for correct and incorrect location estimation are as follows:

\(\begin{align}f\left[z_{q}(l) \mid \theta_{q}(l), N_{V}(l), Z^{l-1}\right]=P_{G}^{-1} N\left(z_{q}(l) ; \hat{z}(l \mid l-1), S_{q}(l)\right)\end{align}\)       (34)

= P^-1_GN(v_q(l);0, S_q(l))       (35)

\(\begin{align}=P_{G}^{-1} \frac{\exp \left\{-\frac{1}{2} v_{q}^{T}(l) S_{q}^{-1}(l) v_{q}(l)\right\}}{2 \pi\left|S_{q}(l)\right|^{0.5}}\end{align}\)       (36)

where |S_q(l)| indicates the determinant of matrix S_q(l).

f[z_q(l)|θ_q(l), N_V(l), Z^l-1] = V^-1_q(l)       (37)

where V_q(l) is area of the validation gate (28) of the N_V(l) accepted hypothesis. The prior probabilities in (33) are:

\(\begin{align}\begin{array}{l}\operatorname{Pr}\left\{\theta_{q}(l) \mid N_{V}(l), \mathrm{Z}^{l-1}\right\}=\frac{P_{d} P_{G}}{N_{V}(l)}, q=1, \ldots, N_{V}(l) \\ \operatorname{Pr}\left\{\theta_{q}(l) \mid N_{V}(l), \mathrm{Z}^{l-1}\right\}=1-P_{d} P_{G}, q=0\end{array}\end{align}\)       (38)

Where P_d represents the detection probability.

It can be obtained from (36) and (37), the joint probability density function of correct and incorrect location estimation are:

\(\begin{align}f\left[Z(l) \mid \theta_{q}(l), N_{V}(l), Z^{l-1}\right]=\prod_{i=1 \neq q}^{N_{V}(l)} \frac{N\left(v_{q}(l) ; 0, S_{q}(l)\right)}{V_{i}(l) P_{G}}, q=1, \ldots, N_{V}(l)\end{align}\)       (39)

\(\begin{align}f\left[Z(l) \mid \theta_{q}(l), N_{V}(l), Z^{l-1}\right]=\prod_{i=1}^{N_{V}(l)} V_{i}^{-1}(l), q=0\end{align}\)       (40)

Finally, we get the association probabilities as follows:

\(\begin{align}\begin{array}{l}\beta_{q}^{\prime}(l)=\frac{N\left(v_{q}(l) ; 0, S_{q}(l)\right)}{P_{G}}\left(\frac{P_{d} P_{G}}{N_{V}(l)}\right) \prod_{i=1, i \neq q}^{N_{V}(l)} V_{i}^{-1}(l), q=1, \ldots, N_{V}(l) \\ \beta_{0}^{\prime}(l)=\left(1-P_{d} P_{G}\right) \prod_{i=1}^{N_{V}(l)} V_{i}^{-1}(l), q=0\end{array}\end{align}\)       (41)

When we study indoor location, we find that MN direction is also a crucial information. For us, it is a kind of visual data [26]. It can be applied to data association to increase the accuracy of positioning. In order to add direction information, the introduction of pseudo direction is very necessary, because direction information can't be easily obtained in distance-based positioning. The pseudo direction connects the verified measurement position and the predicted position at current time with estimated position at previous time, as shown in Fig. 2. \(\begin{align}\hat{z}(l \mid l-1)\end{align}\) is the predicted state value of the target trajectory at time l, and \(\begin{align}\hat{z}(l -1)\end{align}\) is the estimated position value at time l-1. We use 𝛥_q to indicate the direction difference. The expression is as follows:

Fig. 2. Illustration of concept of pseudo direction

∆_q = |γ_q - γ|, q = 1, ..., N_V(l)       (42)

where γ_q represents the included angle between vector orientation and horizontal orientation of the q − th position estimation, γ indicates the angle between vector orientation and horizontal orientation of the state prediction value.

We use the angle difference between γ and γ_q to decide orientation weight. When Gaussian weightexp(− 𝛥^T𝛿⁻¹_𝜙 𝛥/2)is applied, it is multiplied by the association probability of PDA. In the exponential the parameter 𝛿_𝛷 used that is the variance of direction of MN. Define the direction of MN and the speed as follows:

\(\begin{align}\phi=\tan ^{-1}\left(\frac{ \dot{x}(l)}{\dot{y}(l)}\right)\end{align}\),       (43)

\(\begin{align}v(l)=\sqrt{\dot{x}(l)^{2}+\dot{y}(l)^{2}}\end{align}\)       (44)

We use the linearization method to calculate the first and second moments, and ignore the higher-order terms.

\(\begin{align}\Delta \phi=\frac{\partial \phi}{\partial \dot{x}(l)} \Delta \dot{x}(l)+\frac{\partial \phi}{\partial \dot{y}(l)} \Delta \dot{y}(l)+\mathrm{O}\left(\Delta \dot{x}(l)^{2}, \Delta \dot{y}(l)^{2}, \Delta \dot{x}(l) \Delta \dot{y}(l)\right)\end{align}\)       (45)

By differentiating (45), we get

\(\begin{align}\frac{\partial \phi}{\partial \dot{x}(l)}=\frac{\dot{y}(l)}{v(l)^{2}}, \quad \frac{\partial \phi}{\partial \dot{y}(l)}=-\frac{\dot{x}(l)}{v(l)^{2}}\end{align}\)       (46)

And then,

\(\begin{align}\Delta \phi=\frac{\dot{y}(l)}{v(l)^{2}} \Delta \dot{x}(l)-\frac{\dot{x}(l)}{v(l)^{2}} \Delta \dot{y}(l)\end{align}\)       (47)

Then calculate the expectation and the variance from (47):

\(\begin{align}\begin{array}{l}E\{\Delta \phi\}=\frac{1}{v(l)^{2}}[\dot{y}(l) E\{\Delta \dot{x}(l)\}-\dot{x}(l) E\{\Delta \dot{y}(l)\}]=0 \\ E\left\{(\Delta \phi)^{2}\right\}=\delta_{\phi}^{2}=\left[\dot{y}(l)^{2} P_{\dot{x}(l)}-2 \dot{x}(l) \dot{y}(l) P_{\dot{x}(l) \dot{y}(l)}+\dot{x}(l)^{2} P_{\dot{y}(l)}\right] / v(l)^{4}\end{array}\end{align}\)       (48)

where the covariance terms are from P as follows:

\(\begin{align}P=\left(\begin{array}{cccc}P_{x} & P_{x y} & P_{x \dot{x}} & P_{x \dot{y}} \\ P_{x y} & P_{y} & P_{y \dot{x}} & P_{y \dot{y}} \\ P_{x \dot{x}} & P_{y \dot{x}} & P_{\dot{x}} & P_{x y} \\ P_{x \dot{y}} & P_{y \dot{y}} & P_{x \dot{y}} & P_{\dot{y}}\end{array}\right)\end{align}\)       (49)

After adding the direction, the association probabilities of the DPDA becomes:

\(\begin{align}\begin{array}{l}\beta_{q}^{\prime}(l)=\frac{N\left(v_{q}(l) ; 0, S_{q}(l)\right)}{P_{G}}\left(\frac{P_{d} P_{G}}{N_{V}(l)}\right) \prod_{i=1, i \neq q}^{N_{V}(l)} V_{i}^{-1}(l) \times \exp \left(-\frac{\Delta_{q}{ }^{T} \delta_{\phi}^{-1} \Delta_{q}}{2}\right), q=1, \ldots, N_{V}(l) \\ \beta_{0}^{\prime}(l)=\left(1-P_{d} P_{G}\right) \prod_{i=1}^{N_{V}(l)} V_{i}^{-1}(l), q=0\end{array}\end{align}\)       (50)

Due to the fact that the base of the exponential function is greater than 1, it is an increasing function. Therefore, a larger \(\begin{align}-\frac{\Delta_{q}{ }^{T} \delta_{\phi}{ }^{-1} \Delta_{q}}{2}\end{align}\) represents a higher probability of allocation, while a larger \(\begin{align}-\frac{\Delta_{q}{ }^{T} \delta_{\phi}{ }^{-1} \Delta_{q}}{2}\end{align}\) represents a smaller direction difference. So smaller directional differences will be assigned a higher probability.

Finally, the association probabilities are modeled as:

\(\begin{align}\beta_{q}(l)=\frac{\beta_{q}^{\prime}(l)}{\beta_{0}^{\prime}(l)+\sum_{q=1}^{N_{V}(l)} \beta_{q}^{\prime}(l)}\end{align}\)       (51)

\(\begin{align}\beta_{0}(l)=\frac{\beta_{0}^{\prime}(l)}{\beta_{0}^{\prime}(l)+\sum_{q=1}^{N_{V}(l)} \beta_{q}^{\prime}(l)}\end{align}\)       (52)

The innovation covariance and Kalman gain are respectively expressed by the following equation:

S(l) = BP(l|l - 1)B^T + σ²_LI₂       (53)

K(l) = P(l|l - 1)B^TS^-1(l)       (54)

We make use of the association probabilities calculated above to get final state estimate \(\begin{align}\stackrel{\wedge}{X}(l \mid l)\end{align}\) and covariance matrix P(l|l).

\(\begin{align}\hat{X}(l \mid l)=\hat{X}(l \mid l-1)+K(l) \sum_{q=1}^{N_{V}(l)} \beta_{q}(l) v_{q}(l)\end{align}\)       (55)

\(\begin{align}P(l \mid l)=\beta_{0}(l) P(l \mid l-1)+\left(1-\beta_{0}(l)\right) P_{c}(l \mid l)+\tilde{P}(l)\end{align}\)       (56)

\(\begin{align}P_{c}(l \mid l)=\left(\mathrm{I}_{4}-K(l) B(l)\right) P(l \mid l-1)\end{align}\)       (57)

\(\begin{align}\bar{P}(l)=K(l)\left[\sum_{q=1}^{N_{v}(l)} \beta_{q}(l) v_{q}(l) v_{q}{ }^{T}(l)-v(l) v^{T}(l)\right] K^{T}(l)\end{align}\)       (58)

\(\begin{align}v(l)=\sum_{q=1}^{N_{v}(l)} \beta_{q}(l) v_{q}(l)\end{align}\)       (59)

4. Simulation and Experimental Results

4.1 Simulation

In this part, we conduct simulation experiments and the simulation results are analyzed. The trajectory and scene of the simulation experiment are shown in Fig. 3. Positioning 100 points and sampling interval is 0.5s. We set the initial state and covariance matrix as X(0) = [1m 19.99m 1 m/s 0.1 m/s]^T and P(0) = I₄ respectively. The P_G is 0.99, P_d is 0.9. We randomly generate a number between 0 and 1 to simulate the NLOS environment. We compare the random number with the NLOS probability threshold to judge whether BN and the MN are in NLOS state. If the digit is less than NLOS probability threshold, this BN is in NLOS state. The NLOS errors comply with Gaussian distribution in simulation experiment. We compare IPF-DPDA with the MJPDA [27], RIMM [13], IMM-EKF [9], REKF [13] EKF [7]. 1000 Monte Carlo runs are used to get simulation results. And the good or bad of algorithm is estimated through the root mean square error (RMSE) and cumulative distribution function (CDF) of the average localization error.

Fig. 3. Trajectory of MN

\(\begin{align}R M S E=\sqrt{\frac{1}{M C} \frac{1}{L} \sum_{j=1}^{M C} \sum_{l=1}^{L}\left(\left(\hat{x}_{j}(l)-x_{j}(l)\right)^{2}+\left(\hat{y}_{j}(l)-y_{j}(l)\right)^{2}\right)}\end{align}\)       (60)

Where L = 100 and MC = 1000 represent the number of moving steps and the number of Monte Carlo simulation runs, respectively. \(\begin{align}\left(\hat{x}_{j}(l), \hat{y}_{j}(l)\right)\end{align}\) is the position estimation and (x_j(l), y_j(l)) is the true location of MN during the j − th Monte Carlo run.

Gaussian Distribution: We suppose that NLOS errors and measurement noise obeyed the Gaussian distribution N(𝜇_NLOS, 𝜎²_NLOS) and N(0, 𝜎²_L) ,respectively. The specific parameter settings are shown are displayed in Table 1. When we do simulation experiments, one parameter changes while the other parameters remain unchanged, for the sake of observing the effect of this parameter on location accuracy of algorithm.

Table 1. Default Parameters of Gaussian Distribution

Fig. 4 mirrors the transform when the 𝜇_NLOS raises from 1 to 7. The black curve represents my algorithm, it can be obviously seen that the location accuracy of IPF-DPDA is better. In more details, average positioning error of IPF-DPDA, MJPDA, RIMM, IMM-EKF, REKF and EKF are 2.017m, 2.776m, 2.936m, 3.081m 3.963m, and 4.043m respectively, and IPF-DPDA improves average location accuracy by 27.34%, 31.30%, 34.53%, 49.10% and 50.11% separately.

Fig. 4. RMSE under various μ_NLOS

Fig. 5 distinctly reflects the average localization errors an ascending tendency as P_NLOS increases. The average location accuracy of IPF-DPDA is 1.944m, while MJPDA, RIMM, IMM-EKF, REKF and EKF are 2.473m, 2.617m, 2.744m, 3.649m, 3.761m respectively. The IPF-DPDA raises average location accuracy by 21.39%, 25.72%, 29.15%, 47.85% and 48.31% respectively.

Fig. 5. RMSE under various P_NLOS

With the BN raises in Fig. 6, the location accuracy of all algorithms has been improved. However, RIMM, IMM-EKF, REKF and EKF improve slowly, while IPF-DPDA and MJPDA improve faster than the previous four algorithms. In more details, the location accuracy of IPF-DPDA, MJPDA, RIMM, IMM-EKF, REKF and EKF are 2.381m, 3.032m, 3.21m, 3.368m, 4.401m, and 4.491m respectively. The IPF-DPDA raises average location accuracy by 21.47%, 25.83%, 29.31%, 45.90% and 46.98% respectively.

Fig. 6. RMSE under various BN

In Fig. 7, CDF reflects the positioning accuracy of my algorithm, the 90% localization error of IPF-DPDA, MJPDA, RIMM, IMM-EKF, REKF and EKF are less than 3.357 m, 3.863m, 5.192m, 5.375m, 6.601m and 6.693m separately.

Fig. 7. CDF of localization error

4.2 Experiment

Because the simulations experiment can’t fully explain the localization accuracy of the IPF-DPDA, we designed a true experiment to verify it. The ultra-wideband (UWB) location has low transmission power and can realize accurate information exchange between equipment, so as to realize high-precision positioning. The UWB equipment used is displayed in Fig. 8 and 9 in this paper. Fig. 8 shows the MN and power supply, Fig. 9 shows the BN.

Fig. 8. MN and Power Supply

Fig. 9. BN

The experiment is carried out in our laboratory, as shown in Fig. 10. The length of laboratory is 12.6 meters and the width is 6.6 meters. There are seven beacon nodes in the experimental scene, and the person moves along the trajectory. The blue and green positions are MN and BN respectively. Place MN and BNs 1.2m above the floor, for the sake of avoiding UWB signal reflecting from the floor. The initial state of MN is set to X(0) = [1.8m 6m 0m/s −0.6 m/s]^T. The seven BNs are located at(x₁ = 1.2m, y₁ = 3.6m), (x₂ = 3.3m, y₂ = 0.6m), (x₃ = 2.4m, y₃ = 4.6m), (x₄ = 5.4m, y₄ = 4.2m), (x₅ = 9.1m, y₅ = 1.2m), (x₆ = 9.6m, y₆ = 3.6m), and (x₇ = 11.4m, y₇ = 5.2m) respectively. We sampled a total of 30 points, with a 1-second interval between each sampling point. Obtain 20 values at each sampling point and take the average as the current distance measurement value. Other parameters remain unchanged, the same as the simulation.

Fig. 10. (a) Real Scene of the Laboratory (b) Plan of Laboratory

The error distribution of each sampling point and the CDF of positioning error are displayed in Fig. 11 and Fig. 12 separately.

Fig. 11. Sampling point positioning error

Fig. 12. CDF of localization error

As can be observed from Fig. 11, the black curve represents my algorithm, the location accuracy of IPF-DPDA is the highest more than half of the sampling points. The positioning accuracy of IPF-DPDA, MJPDA, RIMM, IMM-EKF, REKF and EKF are 0.6647m, 0.7693m, 0.8532m, 0.8442m, 0.8313m, and 0.8317m respectively on average.

From Fig. 12, the CDF of each algorithm is shown when experiment in the laboratory. The light blue line is my algorithm, which is obviously better than other algorithms. In more detail, the 90% localization error of IPF-DPDA, MJPDA, RIMM, IMM-EKF, REKF and EKF are less than 1.105 m, 1.541, 1.577m. 1.709m, 1.837m and 1.701m separately.

5. Conclusion

A location method based on improved particle filter and directional probability data association is proposed to track MN. Firstly, an improved particle filter is applied to reduce error of measuring distance. Then the hypothesis test is applied to detect whether measurements are in LOS situations or in NLOS situations for N different groups. If there are measurements in the validation gate, the corresponding association probabilities are applied to weight retained position estimate to gain the final location estimation. If the number of inside validation gate is 0, update with predicted state estimate. Simulation and experimental results display IPF-DPDA performance better than MJPDA, RIMM, IMM-EKF, REKF and EKF in all aspects. The next work we need to do is to enhance the robustness of IPF-PDDA algorithm when NLOS error probability is large.