# A Robust Indoor Mobile Localization Algorithm for Wireless Sensor Network in Mixed LOS/NLOS Environments

Authors: Ou Yong Kang; Cheng Long
Journal: Journal of Sensors
 (2020)

---

## Abstract

Wireless sensor network (WSN) is a self-organizing network which is composed of a large number of cheap microsensor nodes deployed in the monitoring area and formed by wireless communication. Since it has the characteristics of rapid deployment and strong resistance to destruction, the WSN positioning technology has a wide application prospect. In WSN positioning, the nonline of sight (NLOS) is a very common phenomenon affecting accuracy. In this paper, we propose a NLOS correction method algorithm base on the time of arrival (TOA) to solve the NLOS problem. We firstly propose a tendency amendment algorithm in order to correct the NLOS error in geometry. Secondly, this paper propose a particle selection strategy to select the standard deviation of the particle swarm as the basis of evolution and combine the genetic evolution algorithm, the particle filter algorithm, and the unscented Kalman filter (UKF) algorithm. At the same time, we apply orthogon theory to the UKF to make it have the ability to deal with the target trajectory mutation. Finally we use maximum likelihood localization (ML) to determine the position of the mobile node (MN). The simulation and experimental results show that the proposed algorithm can perform better than the extend Kalman filter (EKF), Kalman filter (KF), and robust interactive multiple model (RIMM).


---

## Body

## 1. Introduction

Due to the development of microelectromechanical systems, wireless communication, and microprocessor, WSN has been developed rapidly. The application of WSN attracts many researchers’ attention. The application scenarios include military industrial communities and health surveillance. WSN is a system which is composed of many small sensors which have the ability to calculate and communicate, so it has lots of advantages such as self-organization, low cost, and high precision. In recent years, with the increasing requirement of positioning accuracy, wireless positioning system technology has been developing rapidly. We urgently need a positioning system which is less disturbed by obstacles and has high positioning accuracy compared with GPS. At this time, wireless positioning system has been increasingly valued by people, which can accurately locate in a complex environment such as an indoor or urban area. Wireless positioning system set a number of beacon nodes (BN) in the area to locate the position of MN. It can be widely used in many ways. In the military, it can track the enemy target, and in civilian, it can be used to help people to get the location of friends and so on.In wireless positioning system, the most commonly used methods are the TOA and the time difference of arrival (TDOA). These methods can achieve low-cost positioning without replacing the user’s equipment. The principle of TOA is to measure the time of signals transmitted from BNs to MN and calculate the position of the MS though geometric relations according to the TOA.The most serious problem in indoor localization is NLOS noise. In [1], the simulation results of Chen et al. on the computer show that the NLOS error can significantly reduce the positioning accuracy, and it is difficult to distinguish the measurement value of the line-of-sight (LOS) distance from the NLOS distance. In [2], Berdugo et al. tested different communication protocols under NLOS links and interference with a wireless sensor network (WSN) testbed located in Balanquilla, Colombia. Although the effect is good, due to NLOS, the performance still does not achieve the desired results. In [3], the authors mentioned that the availability of positioning using GPS is greatly reduced due to the reflection of GPS signals by buildings. The error caused by the NLOS almost exists in everywhere. If the signal propagation will be blocked in the high and dense buildings in urban areas, due to the diffraction, reflection, interference, and other conditions. The noise caused by NLOS (NLOS noise) usually would not show a distribution like LOS, which seriously damages the positioning accuracy. Therefore, how to mitigate the NLOS noise has become the most important problem.In this paper, we propose the genetic evolution algorithm and a tendency amendment algorithm into the filter algorithms to mitigate the NLOS noise. The main contributions of this paper are as follows:(1)
Let the standard deviation of the data be used as the standard of genetic evolution algorithm elimination and introduced it into the unscented particle filter (UPF)(2)
A multilevel processing thought is used in the algorithm. At first, we use the orthogon filter (OF) and robust extend Kalman filter (REKF) together to weaken the big NLOS noise, then genetic evolution algorithm is used to help UPF deal with these data. In this way, we can get better effort though we do not know whether NLOS appear at this time(3)
The algorithm can apply in many different NLOS environment, such as Gaussian distribution, uniform distribution, and exponential distribution(4)
Apply orthogon theory to the UKF to make it have the ability to deal with the sudden change of the environmentThe structure of this paper is as follows: the related works are described in Section2. The measurement model and a brief introduction of some algorithm, such as KF, PF, UKF, and OF are given in Section 3. In Section 4, we describe the algorithm we proposed in detail. The performance evaluation of the proposed algorithm is shown in Section 5. The conclusion of the paper is made in Section 6.


## 2. Related Works

To solve the measurement deviation cause by NLOS, some filters are widely used. The most popular one is the extend Kalman filter (EKF), which uses the Taylor expansion method to append the Jacobian matrix into the equation, turning the linear model KF to a nonlinear model. Another method is UKF, which utilizes unscented transformation to calculate the posterior mean and covariance after the transmission of nonlinear function through a series of sigma points with an independent identically distributed. Unscented particle filter (UPF), which combines the UKF and the PF together, taking the result of the UKF as the initial sampling.Nowadays, in order to solve this problem, a lot of new methods have been proposed which are not limited to the use of various filtering, and then we classify these methods by whether the received signal is NLOS or LOS.In [4], a NLOS discriminate algorithm called PPSA (probabilistic position selection algorithm) is proposed. This algorithm concludes three steps: first, the generation of random particles, particle selection, and the process of evaluating. They made full use of the map information, arranged particles, and the distance after NLOS detection to determine a well solution. However, in practice, obstacles can move at any time, so the algorithm still has great limitations. In [5], Berdugo et al. propose a NLOS identification algorithm with feature selection strategy and a localization algorithm based on the import vector machine (IVM) with high accuracy and low complexity. They use IVM to output the result, but compared with their algorithm, our algorithm has the advantage of high flexibility and real-time performance, because it does not use machine learning algorithm. In [6], a NLOS identification algorithm based on residual analysis and fuzzy C-means method is proposed. Then, it presents an algorithm base on voting to weaken the NLOS and finally use KF, UKF, and ML together to obtain the final position; the idea of combining multiple filters and then using ML is worth learning. However, due to the lack of robust algorithm, the accuracy will still be greatly affected when facing strong NLOS.To solve the problem caused by NLOS, a group of researchers propose a Wi-Fi localization based on unsupervised fusion of an extended candidate location set (ECLS) [7]. Due to the ECLS providing a fusion space that may include the true location of the user. They intend to construct an ECLS by augmenting CLS with other location estimates (locations with predication probability greater than a certain threshold) from each classifier, and using a point of inflection searching algorithm is proposed to construct it. Moreover, the ECLS is derived from the joint optimization of weights and the location of the user. In [8], a new distributed consensus-based adaptive Kalman estimation algorithm is proposed to track a MN. In order to estimate the states of the target, they try to minimize the mean-squared estimation error to obtain a well Kalman gain. In the filter, they set a dynamic cluster selection. Also, a two-stage hierarchical fusion structure is used to get a perfect accurate estimation. In [9], Alshamaa et al. present an algorithm to localize sensors in indoor environments. Firstly, divide the targeted area into several sectors. Then, the RSS information to estimate the sensor sections. Finally, fuse all the data of the calculator to get the position of MN. In [10], Wang et al. combine EKF and the PF together and solve the problems of particle degradation by reducing the number of particles at some stage, Because particle filter is based on the Monte Carlo theory, the objective probability density function is approximated by the randomly selected particle subset from posterior probability, so it can be applied to any case and UKF only performs well when the error presents the Gaussian distribution, but because of the long running time and the loss of sample validity and diversity caused by resampling in some cases, this method still needs to be improved. In [11], a framework called Maxlifd by using ML is proposed, which combined mutual distance information with fingerprint. And in Maxlifd, the ML plays an important role. In [12], Xu et al. proposed a key data preprocessing algorithm base on empirical mode decomposition interval threshold filter firstly to weaken the noises and offer more accurate information for the latter process. Then, a nonlinear autoregressive least-squares support vector machine with exogenous input (NARX) model (LSSVM-NARX) and mix it with KF to construct a new LSSVM-NARX/KF hybrid algorithm is design. This idea is worthy of reference because after removing the large NLOS noise, all the remaining small errors can be treated as LOS noise. A semitightly coupled integration scheme which integrates INS/GNSS with grid-based simultaneous localization and mapping is proposed in [13]. The proposed scheme is based on EKF. In this method, they make full use of the direction and velocity information. A special idea of this scheme is to improve fusion of GNSS/INS with the use of grid-based SLAM serves so that we can get accurate measurement. In [14], an ensemble learner-based classification and information fusion algorithm is proposed and in [15], the researchers propose a new square root cubature particle filter to solve the NLOS problem. The authors also employ self-adaptive artificial fish swarm algorithm to optimize the particle swarm. Besides, they also use a sensor selection method, which reduces energy consumption of the network by waking up several BNs at each time. Then, the authors propose a new fusion method called similarity fusion method to fuse local estimates together to get a better result for distributed fusion architecture. The idea of using the intelligent optimization algorithm to improve the abundance of the particles in the particle filtering is very well; besides artificial fish swarm algorithm, the intelligent optimization algorithm also include gray wolf strategy, ant colony algorithm, and the evolution algorithm. In our proposed algorithm, we have tried the evolutionary algorithm and achieved good results. In [16], the researchers think that the problem can be solved exactly by a bisection procedure. The algorithm they proposed add ML in it so that it not need to know the priori information, and the computational complexity of the proposed algorithm is linear in the number of reference nodes. In [17], Chang et al. take the transmit power as a constant and proposed a novel nonconvex weighted least squares estimator which can turn the NLOS problem into a second-order cone programming problem for a well answer; they also propose a hybrid maximum likelihood SOCP algorithm to alternatively estimate the target node locations and transmit power. In [18], a robust approach to smooth the NLOS is used. It is turned into a generalized trust region subproblem by applying certain approximations, and finally solve it as a bisection procedure. In [19], the algorithm uses UWB value to improve the accuracy of the PDR system and the particle filter is used to fuse data and achieves excellent results. The most outstanding advantage of the system is that it has very low power consumption. In [20], Yang proposes a novel NLOS mitigation method. The UWB ranging module has the advantages of high transmission speed, strong anti-interference ability, and long transmission distance; interacting multiple model (IMM) is proposed in [21]; two KF were used in parallel by using the Markov chain. After data fusion, the final location estimation can be calculated and realize a high precision positioning, but it uses the residuals with true location when calculating the estimate location, which is impossible to get in real condition. The RIMM is proposed in [22], which concedes the REKF and the EKF. The reason they use REKF to improve this algorithm is that REKF is not sensitive to NLOS. In [23], Cheng et al. propose a voting algorithm to get the position of MN.


## 3. Problem Statement

This part is made up of two parts. Section3.1 is the symbol description, Section 3.2 is the signal model and Section 3.3 is some brief introduction about the UKF and USTF.


### 3.1. Symbol Description

Key notions are shown in Table1:Table 1
Key notions.
SymbolExplanationSymbolExplanationNThe number of beacon nodesdkiThe measured distance between mobile node andith beacon node at time kTiThe interval time of samplingρThe weakening factor in STUKFP^k1The covariance of theith beacon node at time k (STUKF)P^k2The covariance of theith beacon node at time k (REKF)X^k1The distance for iteration (STUKF)X^k2The distance for iteration (REKF)X1,X2,⋯,XnThe distance value of each individual in genetic evolution algorithmxk,ykTThe coordinates of MN in a 2D planeVk1The covariance correction factor at the time kvk−1,vk−2,vk−3The speed value of the previous three momentsPGEUPF,kiThe covariance matrix at timek (GEUPF)NNThe number of population sizestandThe terminate condition of genetic evolution algorithmd^kiThe fuse distance from STUKF and REKFd^1,kiThe distance after STUKF processingd^2,kiThe distance after REKF processing


### 3.2. Signal Model

In this part, we will introduce the signal model in the LOS environment and in NLOS environment, supposingN BNs are deployed in an area which is 100×100. The position of MN at the kthtime step is Zi=xi,yiT,i=1,⋯,N. In the LOS environment, the time measurement model can be written as follows:
(1)TTOA=tTOA+nTOA.In the NLOS environment, the signal will not transmit in a straight line, which leads the measured distance to be much larger than the real value due to interference diffraction or reflection and other conditions. The measurement model in NLOS is(2)TTOA=tTOA+nTOA+nNLOS,where tTOA is the true time between BN and MN, nTOAis a Gaussian white noise with zero mean variance and follows the Gaussian distribution with a positive mean value and a variance of σ2.The nNLOS is the error caused by NLOS. The true distance between the ith BN and the MN at the moment k is
(3)Dki=xki−xi2+yki−yi2.The actual distance between theith BN and the MN at the moment k is
(4)dki=c×TTOA.


### 3.3. Brief Introduction

#### 3.3.1. A Brief Introduction of Kalman Filter (KF)

KF is an algorithm for optimal estimation of system state by using a linear system state equation and system input and output observation data. It is suitable for linear, discrete, and finite dimensional systems.We can use a KF in any place where we have uncertain information about some dynamic system, and we can make an educated guess about what the system is going to do next. At the same time, they have the advantage that they are light on memory for they do not need to keep any history other than the previous state.In KF, the most important part is the Kalman gain matrixK, When the covariance matrix of measurement noise increases, the gain matrix K becomes smaller, that is, the greater the noise is, the smaller the influence of noise on the final result, and the proportion of the predicted value increases.


#### 3.3.2. A Brief Introduction of PF

The idea of particle filter is based on the Monte Carlo method, which uses particle subset to represent probability. The core idea of it is to express the distribution by random state particles extracted from posterior probability. It is a sequential sampling method, which can express more extensive distribution than Gaussian the model and has stronger modeling ability for nonlinear characteristics of variable parameters. In the proposed algorithm, we use PF to achieve better results when the error presents a non-Gaussian distribution.


#### 3.3.3. A Brief Introduction of UKF

UKF and EKF are the same recursive Bayesian estimation method. It is an algorithm based on KF and unscented transformation (UT), but unlike EKF, it uses UT as its linearization method.The characteristics of untracked transformation (UT) method are as follows:(1)
The approximate object of probability density of nonlinear function is distributed, and the explicit expression of nonlinear function does not need to be known in approximation(2)
It has the same order of computation as EKF filtering algorithm(3)
Nondifferentiable nonlinear functions can be dealt with because it is not necessary to derive the Jacobian matrix


#### 3.3.4. A Brief Introduction of UKF Base on Orthogon Theory (OF)

Research shows whether the output residual sequence is orthogonal or not can be used as the basis to distinguish the performance of filter [24]. The UKF is a suboptimal filter algorithm using Gaussian distribution to approximate the posterior probability density of the system, and the output residual sequence cannot be completely unrelated but as long as the residual sequence shows a weak autocorrelation; it can be considered that the filter can obtain a perfect effect. In the filter process, if there is a difference between the estimated value of the filtering state and the actual state of the system, the difference can be reflected in the mean value and amplitude of the output residual sequence. At this time, we should adjust the Kalman gain matrix K online in order to make the residual sequence is still orthogonal with each other and has the property similar to the Gaussian white noise.Based on this theory, a fading factor is introduced in the algorithm to adjust the Kalman gain matrix online, so that high accuracy can be achieved when the state of MN changes suddenly.


## 3.1. Symbol Description

Key notions are shown in Table1:Table 1
Key notions.
SymbolExplanationSymbolExplanationNThe number of beacon nodesdkiThe measured distance between mobile node andith beacon node at time kTiThe interval time of samplingρThe weakening factor in STUKFP^k1The covariance of theith beacon node at time k (STUKF)P^k2The covariance of theith beacon node at time k (REKF)X^k1The distance for iteration (STUKF)X^k2The distance for iteration (REKF)X1,X2,⋯,XnThe distance value of each individual in genetic evolution algorithmxk,ykTThe coordinates of MN in a 2D planeVk1The covariance correction factor at the time kvk−1,vk−2,vk−3The speed value of the previous three momentsPGEUPF,kiThe covariance matrix at timek (GEUPF)NNThe number of population sizestandThe terminate condition of genetic evolution algorithmd^kiThe fuse distance from STUKF and REKFd^1,kiThe distance after STUKF processingd^2,kiThe distance after REKF processing


## 3.2. Signal Model

In this part, we will introduce the signal model in the LOS environment and in NLOS environment, supposingN BNs are deployed in an area which is 100×100. The position of MN at the kthtime step is Zi=xi,yiT,i=1,⋯,N. In the LOS environment, the time measurement model can be written as follows:
(1)TTOA=tTOA+nTOA.In the NLOS environment, the signal will not transmit in a straight line, which leads the measured distance to be much larger than the real value due to interference diffraction or reflection and other conditions. The measurement model in NLOS is(2)TTOA=tTOA+nTOA+nNLOS,where tTOA is the true time between BN and MN, nTOAis a Gaussian white noise with zero mean variance and follows the Gaussian distribution with a positive mean value and a variance of σ2.The nNLOS is the error caused by NLOS. The true distance between the ith BN and the MN at the moment k is
(3)Dki=xki−xi2+yki−yi2.The actual distance between theith BN and the MN at the moment k is
(4)dki=c×TTOA.


## 3.3. Brief Introduction

### 3.3.1. A Brief Introduction of Kalman Filter (KF)

KF is an algorithm for optimal estimation of system state by using a linear system state equation and system input and output observation data. It is suitable for linear, discrete, and finite dimensional systems.We can use a KF in any place where we have uncertain information about some dynamic system, and we can make an educated guess about what the system is going to do next. At the same time, they have the advantage that they are light on memory for they do not need to keep any history other than the previous state.In KF, the most important part is the Kalman gain matrixK, When the covariance matrix of measurement noise increases, the gain matrix K becomes smaller, that is, the greater the noise is, the smaller the influence of noise on the final result, and the proportion of the predicted value increases.


### 3.3.2. A Brief Introduction of PF

The idea of particle filter is based on the Monte Carlo method, which uses particle subset to represent probability. The core idea of it is to express the distribution by random state particles extracted from posterior probability. It is a sequential sampling method, which can express more extensive distribution than Gaussian the model and has stronger modeling ability for nonlinear characteristics of variable parameters. In the proposed algorithm, we use PF to achieve better results when the error presents a non-Gaussian distribution.


### 3.3.3. A Brief Introduction of UKF

UKF and EKF are the same recursive Bayesian estimation method. It is an algorithm based on KF and unscented transformation (UT), but unlike EKF, it uses UT as its linearization method.The characteristics of untracked transformation (UT) method are as follows:(1)
The approximate object of probability density of nonlinear function is distributed, and the explicit expression of nonlinear function does not need to be known in approximation(2)
It has the same order of computation as EKF filtering algorithm(3)
Nondifferentiable nonlinear functions can be dealt with because it is not necessary to derive the Jacobian matrix


### 3.3.4. A Brief Introduction of UKF Base on Orthogon Theory (OF)

Research shows whether the output residual sequence is orthogonal or not can be used as the basis to distinguish the performance of filter [24]. The UKF is a suboptimal filter algorithm using Gaussian distribution to approximate the posterior probability density of the system, and the output residual sequence cannot be completely unrelated but as long as the residual sequence shows a weak autocorrelation; it can be considered that the filter can obtain a perfect effect. In the filter process, if there is a difference between the estimated value of the filtering state and the actual state of the system, the difference can be reflected in the mean value and amplitude of the output residual sequence. At this time, we should adjust the Kalman gain matrix K online in order to make the residual sequence is still orthogonal with each other and has the property similar to the Gaussian white noise.Based on this theory, a fading factor is introduced in the algorithm to adjust the Kalman gain matrix online, so that high accuracy can be achieved when the state of MN changes suddenly.


## 3.3.1. A Brief Introduction of Kalman Filter (KF)

KF is an algorithm for optimal estimation of system state by using a linear system state equation and system input and output observation data. It is suitable for linear, discrete, and finite dimensional systems.We can use a KF in any place where we have uncertain information about some dynamic system, and we can make an educated guess about what the system is going to do next. At the same time, they have the advantage that they are light on memory for they do not need to keep any history other than the previous state.In KF, the most important part is the Kalman gain matrixK, When the covariance matrix of measurement noise increases, the gain matrix K becomes smaller, that is, the greater the noise is, the smaller the influence of noise on the final result, and the proportion of the predicted value increases.


## 3.3.2. A Brief Introduction of PF

The idea of particle filter is based on the Monte Carlo method, which uses particle subset to represent probability. The core idea of it is to express the distribution by random state particles extracted from posterior probability. It is a sequential sampling method, which can express more extensive distribution than Gaussian the model and has stronger modeling ability for nonlinear characteristics of variable parameters. In the proposed algorithm, we use PF to achieve better results when the error presents a non-Gaussian distribution.


## 3.3.3. A Brief Introduction of UKF

UKF and EKF are the same recursive Bayesian estimation method. It is an algorithm based on KF and unscented transformation (UT), but unlike EKF, it uses UT as its linearization method.The characteristics of untracked transformation (UT) method are as follows:(1)
The approximate object of probability density of nonlinear function is distributed, and the explicit expression of nonlinear function does not need to be known in approximation(2)
It has the same order of computation as EKF filtering algorithm(3)
Nondifferentiable nonlinear functions can be dealt with because it is not necessary to derive the Jacobian matrix


## 3.3.4. A Brief Introduction of UKF Base on Orthogon Theory (OF)

Research shows whether the output residual sequence is orthogonal or not can be used as the basis to distinguish the performance of filter [24]. The UKF is a suboptimal filter algorithm using Gaussian distribution to approximate the posterior probability density of the system, and the output residual sequence cannot be completely unrelated but as long as the residual sequence shows a weak autocorrelation; it can be considered that the filter can obtain a perfect effect. In the filter process, if there is a difference between the estimated value of the filtering state and the actual state of the system, the difference can be reflected in the mean value and amplitude of the output residual sequence. At this time, we should adjust the Kalman gain matrix K online in order to make the residual sequence is still orthogonal with each other and has the property similar to the Gaussian white noise.Based on this theory, a fading factor is introduced in the algorithm to adjust the Kalman gain matrix online, so that high accuracy can be achieved when the state of MN changes suddenly.


## 4. Proposed Method

The two parallel filters (REKF and OF) are used at first to reduce NLOS noise, and then the Markov chain is used to fuse them. Then, we use the trend-based correction algorithm to further reduce noise. Finally, the distance data is processed by GEUPF, and the final location of MS is obtained by using ML together with other BN’s data.


### 4.1. Algorithm Structure

Figure1 is the flow chart of the proposed algorithm in this paper. The input of the algorithm is dkii=1,2,3⋯,N, and N is the number of beacon nodes. d^1∣ki is the output of OF while the d^2∣ki is the output of REKF, then fuse these data with the Markov chain and the output is d^ki, after that, use the tendency amendment to eliminate the distance data with obvious NLOS noise. Finally, use the GEUPF to smooth the data and calculate the position of MN with the ML.Figure 1
The structure of the proposed algorithm.


### 4.2. The Unscented Kalman Filter Based on Orthogon Theory (OF)

In UKF,α determines the dispersion degree of the sigma point, usually taking a small positive value; in the simulation of this paper, the value is 1. The default value of κ is usually 0. β is used to describe the distribution information of x, and in the case of Gaussian distribution, the optimal value is 2. R is the measurement error.
(5)α=1,β=2,κ=0,R=1,λ=α2⋅n+κ−n.Then, calculate2n+1 sigma points by
(6)X0=x¯,Xi=x¯+n+1Pxi=1,2,⋯,n,Xi=x¯−n+1Pxi=n+1,⋯,2n,Their weights can be calculated separately in this way:(7)w0m=λn+λ,w0c=λn+λ+1−α2+β,wim=wic=12n+λThen, we calculate the transfer result of the sigma point through the nonlinear functionf⋅.
(8)Yi=fXi,d^k=∑i=02nwimYiPy=∑i=02nwicYi−y¯Yi−y¯T,Pxy=∑i=02nwicYi−x¯Yi−y¯Twherew0m is the weight of the mean value and w0c is the weight of the variance.The residual sequence is defined below:(9)ε=dk−d^k.Then, the covariance matrix of the actual output residual sequence can be estimated by the following formula:(10)Vk=ρVk−1+εkεkT1+ρ.Generally, strong tracking weakening factorρ=0.95 is taken.Define(11)N=Vk−HQk−1HT−R,M=HPk−1HT,AndH is the state transition matrix.
(12)λ0,k=trNtrM,λk=1λk,0<1,λk=λk,02>λk,0≥1,λk=2λk≥2.tr is the matrix tracing operator.Finally, the fading factor is introduced into the prediction error covariance matrix:(13)Pk,k−1=λkPy,We calculate the Kalman gain matrix by(14)K=Pk∣k−1HkTHkPk−1HkT+R−1.Output filtering distance and the covariance is calculated as(15)d1,ki=∑i=02nwimd^k+Kkdk1−d^k.A part of STUKF is as follows (Algorithm1).Algorithm 1. Part of STUKF.
Input:d^1,k−1idkiP^k−11Vk−11Output:P^k1Vk1Set:α=1,β=0,κ=2,RSTUKF=1,QSTUKF=1,ρ=0.95Begin:r=absdki‐d^1,kiV=ρ⋅V+r⋅rT/1+ρN=V‐QSTUKF‐R;M=Pk,k−1;uk=sqrttraceN/traceMifλ0,k≥2λ0,k=2else ifλ0,k≥1elseλ0,k=1;endVk=λ0,kPk=Vk⋅Pk,k−1End


### 4.3. The Tendency Amendment Algorithm

If we own at least four distance value, we can calculate the speed of the last three moments,vk−1, vk−2, and vk−3. If the three speeds increase, it means that the target is accelerating away from it. If the three speeds decrease, it means that the target is decelerating. If the speed at this time is consistent with the previous three performance ideas, then the speed at this time will be cleared(vk=0). Then, use the following formula to calculate a threshold, and if the observation exceeds this limit, use this value instead of the observation.
(16)d^ki=dki2+dk−1i+vk×Ti2dki+dk−1i+vk×Ti.


### 4.4. The Particle Filter Based on Genetic Evolution Algorithm and Unscented Kalman Filter (GEUPF)

Put the fused position information which comes from REKF and OF into UKF first to get the initial particle swarm, assuming that the corresponding value of each particle isxi, and its corresponding covariance is Pi.Use the genetic evolution algorithm to increase the diversity of particle swarm. It is set that when the standard deviation of particle swarm reachesstand (in this paper, we set stand to 0.6) or the number of running time exceeds 50, the calculation must stop to make sure the algorithm is real time, the cross rate (in this paper, we set cross rate to 0.3), and the mutation rate (set mutate rate to 0.3) must be set initially, the whole population is traversed with a step size of 2, and a random number RAND is generated each time. If the RAND>crossrate, the crossover is conducted as follows:
(17)Xc∣i=α⋅Xi+1−α⋅Xi−1+φ,Xc∣i−1=α⋅Xi−1+1−α⋅Xi+φ,where α and φ are random numbers, X is the value of each particle in the population, and in this simulation, φ obeys the Gaussian fraction with mean of 0 and variance of 1, while α is the uniform distribution between [0, 1].Then, compare the absolute value of the difference between the two numbers, and if the absolute value of the difference becomes larger, then replace the original value with this value.(18)Xi=Xc∣i,Xi−1=Xc∣i−1.Otherwise, no substitution is made.Similarly, we randomly generated a RAND, ifRAND<mutate, then the variation is calculated as follows:
(19)Xm∣i=Xi+β,Xm∣i−1=Xi−1+β.Use the same way to judge whether to accept the value.(20)Xi=Xm∣i,Xi−1=Xm∣i−1.whereβ is the random number generated twice following the Gaussian distribution of zero mean value and repeat the process until the data meet the standard.


#### 4.4.1. Importance Sampling

First, calculate the factorlik according to the following formula:
(21)lik=e−d−x∧i2/2RR.Then, we calculate the weight of each particle according to the following formula:(22)Wi=lik⋅e−x∧i+Xi⋅Pie−d−x∧i2/2Pi.After calculating the weight value of each particle, normalize the weight value:(23)Wi=Wi∑Wi.Then, do a residual resampling to solve the problem of particle scarcity in the classical Monte Carlo method. Finally obtain the position of MN by using these particles and their weights.The pseudocode of residual resampling is listed as follows:Algorithm 2. Description of Residual resampling.
Input:N,WiOutput:outIndexL,arb=sizeqN_babies=zeros1,Lq_res=L⋅qTN_babies=fixq_resN_res=L−sumN_babiesifNres≠0q_res=q_res−N_babies/N_rescumDist=cumsumq_resu=fliplrcumprodrand1,Nres1/N_res:−1:1j=1fori=1:N_reswhileu1,i>cumDist1,jj=j+1endN_babies1,j=N_babies1,j+1endendindex=1for i=1:Lif (N_babies(1,i)>0)for j=index:index+N_babies(1,i)-1outIndex(j) = inIndex(i)endendindex= index+N_babies(1,i)end


### 4.5. Extend Kalman Filter Based onM-Estimation

The REKF is a filter that combined KF and the robust algorithm together to obtain the robust when in a nonlinear environment. Firstly, the state transition matrix is constructed:(24)A=I2Ti⋅I20I2.The error transfer matrix is calculated as follows:(25)G=Ti22⋅I2Ti⋅I2.The states vector is(26)Xk=xkykẋkẏkT.where xk and yk represent the x-coordinate and the y-coordinate of MN, ẋk and ẏk respectively, represent the horizontal and vertical coordinates of the target node in the two-dimensional plane, and, respectively, represent the velocity of the target node in the 2D plane. Then, the following equation can be obtained:
(27)Xk=AXk−1.A deviation between observation and prediction is shown as follows:(28)Eki=dki−d^2,ki.We observe the first derivative of the equation:(29)Hki=∂d^2,ki∂xk.We calculate the Kalman gain:(30)K2i=P2,ki⋅HkiT⋅S−1,where factorSis calculated as follows:
(31)Ski=Hki⋅P2,ki⋅HkiT.The final coordinate estimate is(32)X^ki=X^k−1i+K2i⋅Eki.And then, the covariance goes like this:(33)P2,ki=I4−K2,ki⋅Hki⋅P2,ki.Then,M-estimation was carried out like this.State value and measured value can be rewritten as(34)Xk=I4H2,k−1AX^k−1dk−hx^2,k−1,k−1+H2,kX2,k+ek.H2,k is the Jacobian matrix. A is the state-transition matrix,ek can be calculated in this way:
(35)ek=Ax,k−1−X^k−1−vk.So, you have a linear regression model with this form(36)y~=Dx+bfυ+v~.Among them,(37)y^=Ck−1X^kdk1−d^ki+H2,k⋅X^2,k,D=Ck−1I4H2,k,v^=−Ck−1ek.And the factors can be calculated like this:(38)X^k+1=X^k+μDTD−1DTψv^/σV,(39)μ=1/1.25max∣ψ′v^/σV(40)ψv=v∣v∣≤c1,b⋅tanh0.5bc2−∣v∣sgnv,0∣v∣>c2,where the choice of b requires the function to be continuous at c1 and c2, and c1=1.5 and c2=3; in this simulation, if ∣X^l+1−X^l∣<ε stop, otherwise go to (Equation (38)).


### 4.6. Data Fusion Algorithm Based on Interacting Multiple Model (IMM)

The model transformation probability is expressed asT:(41)T=T1,1T1,2T2,1T2,2,The probability of NLOS and LOS in the initial stage isp1=0.50.5.After that, we first update the probability(42)c=p×T.Then, the data is fused and input into the parallel model(43)X^k−1=X∧k−1T×pmix.Among them,(44)pmix=T1,1⋅pn,1c1T1,2⋅pn,1c2T2,1⋅pn,2c1T2,2⋅pn,2c2,X^k−1=X^k−11X^k−12.Input the mixed data into the model, and then calculate the covariance of the state prediction residual as (Equation (29)), then calculate the model likelihood according to the following formula:
(45)λ=exp−E/S⋅E/22π⋅detS,where exp() represents exponential operator and det() represents matrix determinant operator. Then, update matrixp.
(46)p=λ1⋅c1c×λλ2⋅c2c×λ.


### 4.7. Maximum Likelihood Method

Assume that the horizontal and vertical coordinate matrices of randomly set nodes arexi and yi, respectively.
(47)A=x1−x2y1−y2::x1−xNy1−yN,B=d∧2k2−d∧1k2−x22+y22+x12+y12::d∧Nk2−d∧1k2−xN2+yN2+x12+y12.Then, we can obtain the 2D coordinate position as(48)xk,ykT=ATA−1ATB.


## 4.1. Algorithm Structure

Figure1 is the flow chart of the proposed algorithm in this paper. The input of the algorithm is dkii=1,2,3⋯,N, and N is the number of beacon nodes. d^1∣ki is the output of OF while the d^2∣ki is the output of REKF, then fuse these data with the Markov chain and the output is d^ki, after that, use the tendency amendment to eliminate the distance data with obvious NLOS noise. Finally, use the GEUPF to smooth the data and calculate the position of MN with the ML.Figure 1
The structure of the proposed algorithm.


## 4.2. The Unscented Kalman Filter Based on Orthogon Theory (OF)

In UKF,α determines the dispersion degree of the sigma point, usually taking a small positive value; in the simulation of this paper, the value is 1. The default value of κ is usually 0. β is used to describe the distribution information of x, and in the case of Gaussian distribution, the optimal value is 2. R is the measurement error.
(5)α=1,β=2,κ=0,R=1,λ=α2⋅n+κ−n.Then, calculate2n+1 sigma points by
(6)X0=x¯,Xi=x¯+n+1Pxi=1,2,⋯,n,Xi=x¯−n+1Pxi=n+1,⋯,2n,Their weights can be calculated separately in this way:(7)w0m=λn+λ,w0c=λn+λ+1−α2+β,wim=wic=12n+λThen, we calculate the transfer result of the sigma point through the nonlinear functionf⋅.
(8)Yi=fXi,d^k=∑i=02nwimYiPy=∑i=02nwicYi−y¯Yi−y¯T,Pxy=∑i=02nwicYi−x¯Yi−y¯Twherew0m is the weight of the mean value and w0c is the weight of the variance.The residual sequence is defined below:(9)ε=dk−d^k.Then, the covariance matrix of the actual output residual sequence can be estimated by the following formula:(10)Vk=ρVk−1+εkεkT1+ρ.Generally, strong tracking weakening factorρ=0.95 is taken.Define(11)N=Vk−HQk−1HT−R,M=HPk−1HT,AndH is the state transition matrix.
(12)λ0,k=trNtrM,λk=1λk,0<1,λk=λk,02>λk,0≥1,λk=2λk≥2.tr is the matrix tracing operator.Finally, the fading factor is introduced into the prediction error covariance matrix:(13)Pk,k−1=λkPy,We calculate the Kalman gain matrix by(14)K=Pk∣k−1HkTHkPk−1HkT+R−1.Output filtering distance and the covariance is calculated as(15)d1,ki=∑i=02nwimd^k+Kkdk1−d^k.A part of STUKF is as follows (Algorithm1).Algorithm 1. Part of STUKF.
Input:d^1,k−1idkiP^k−11Vk−11Output:P^k1Vk1Set:α=1,β=0,κ=2,RSTUKF=1,QSTUKF=1,ρ=0.95Begin:r=absdki‐d^1,kiV=ρ⋅V+r⋅rT/1+ρN=V‐QSTUKF‐R;M=Pk,k−1;uk=sqrttraceN/traceMifλ0,k≥2λ0,k=2else ifλ0,k≥1elseλ0,k=1;endVk=λ0,kPk=Vk⋅Pk,k−1End


## 4.3. The Tendency Amendment Algorithm

If we own at least four distance value, we can calculate the speed of the last three moments,vk−1, vk−2, and vk−3. If the three speeds increase, it means that the target is accelerating away from it. If the three speeds decrease, it means that the target is decelerating. If the speed at this time is consistent with the previous three performance ideas, then the speed at this time will be cleared(vk=0). Then, use the following formula to calculate a threshold, and if the observation exceeds this limit, use this value instead of the observation.
(16)d^ki=dki2+dk−1i+vk×Ti2dki+dk−1i+vk×Ti.


## 4.4. The Particle Filter Based on Genetic Evolution Algorithm and Unscented Kalman Filter (GEUPF)

Put the fused position information which comes from REKF and OF into UKF first to get the initial particle swarm, assuming that the corresponding value of each particle isxi, and its corresponding covariance is Pi.Use the genetic evolution algorithm to increase the diversity of particle swarm. It is set that when the standard deviation of particle swarm reachesstand (in this paper, we set stand to 0.6) or the number of running time exceeds 50, the calculation must stop to make sure the algorithm is real time, the cross rate (in this paper, we set cross rate to 0.3), and the mutation rate (set mutate rate to 0.3) must be set initially, the whole population is traversed with a step size of 2, and a random number RAND is generated each time. If the RAND>crossrate, the crossover is conducted as follows:
(17)Xc∣i=α⋅Xi+1−α⋅Xi−1+φ,Xc∣i−1=α⋅Xi−1+1−α⋅Xi+φ,where α and φ are random numbers, X is the value of each particle in the population, and in this simulation, φ obeys the Gaussian fraction with mean of 0 and variance of 1, while α is the uniform distribution between [0, 1].Then, compare the absolute value of the difference between the two numbers, and if the absolute value of the difference becomes larger, then replace the original value with this value.(18)Xi=Xc∣i,Xi−1=Xc∣i−1.Otherwise, no substitution is made.Similarly, we randomly generated a RAND, ifRAND<mutate, then the variation is calculated as follows:
(19)Xm∣i=Xi+β,Xm∣i−1=Xi−1+β.Use the same way to judge whether to accept the value.(20)Xi=Xm∣i,Xi−1=Xm∣i−1.whereβ is the random number generated twice following the Gaussian distribution of zero mean value and repeat the process until the data meet the standard.


### 4.4.1. Importance Sampling

First, calculate the factorlik according to the following formula:
(21)lik=e−d−x∧i2/2RR.Then, we calculate the weight of each particle according to the following formula:(22)Wi=lik⋅e−x∧i+Xi⋅Pie−d−x∧i2/2Pi.After calculating the weight value of each particle, normalize the weight value:(23)Wi=Wi∑Wi.Then, do a residual resampling to solve the problem of particle scarcity in the classical Monte Carlo method. Finally obtain the position of MN by using these particles and their weights.The pseudocode of residual resampling is listed as follows:Algorithm 2. Description of Residual resampling.
Input:N,WiOutput:outIndexL,arb=sizeqN_babies=zeros1,Lq_res=L⋅qTN_babies=fixq_resN_res=L−sumN_babiesifNres≠0q_res=q_res−N_babies/N_rescumDist=cumsumq_resu=fliplrcumprodrand1,Nres1/N_res:−1:1j=1fori=1:N_reswhileu1,i>cumDist1,jj=j+1endN_babies1,j=N_babies1,j+1endendindex=1for i=1:Lif (N_babies(1,i)>0)for j=index:index+N_babies(1,i)-1outIndex(j) = inIndex(i)endendindex= index+N_babies(1,i)end


## 4.4.1. Importance Sampling

First, calculate the factorlik according to the following formula:
(21)lik=e−d−x∧i2/2RR.Then, we calculate the weight of each particle according to the following formula:(22)Wi=lik⋅e−x∧i+Xi⋅Pie−d−x∧i2/2Pi.After calculating the weight value of each particle, normalize the weight value:(23)Wi=Wi∑Wi.Then, do a residual resampling to solve the problem of particle scarcity in the classical Monte Carlo method. Finally obtain the position of MN by using these particles and their weights.The pseudocode of residual resampling is listed as follows:Algorithm 2. Description of Residual resampling.
Input:N,WiOutput:outIndexL,arb=sizeqN_babies=zeros1,Lq_res=L⋅qTN_babies=fixq_resN_res=L−sumN_babiesifNres≠0q_res=q_res−N_babies/N_rescumDist=cumsumq_resu=fliplrcumprodrand1,Nres1/N_res:−1:1j=1fori=1:N_reswhileu1,i>cumDist1,jj=j+1endN_babies1,j=N_babies1,j+1endendindex=1for i=1:Lif (N_babies(1,i)>0)for j=index:index+N_babies(1,i)-1outIndex(j) = inIndex(i)endendindex= index+N_babies(1,i)end


## 4.5. Extend Kalman Filter Based onM-Estimation

The REKF is a filter that combined KF and the robust algorithm together to obtain the robust when in a nonlinear environment. Firstly, the state transition matrix is constructed:(24)A=I2Ti⋅I20I2.The error transfer matrix is calculated as follows:(25)G=Ti22⋅I2Ti⋅I2.The states vector is(26)Xk=xkykẋkẏkT.where xk and yk represent the x-coordinate and the y-coordinate of MN, ẋk and ẏk respectively, represent the horizontal and vertical coordinates of the target node in the two-dimensional plane, and, respectively, represent the velocity of the target node in the 2D plane. Then, the following equation can be obtained:
(27)Xk=AXk−1.A deviation between observation and prediction is shown as follows:(28)Eki=dki−d^2,ki.We observe the first derivative of the equation:(29)Hki=∂d^2,ki∂xk.We calculate the Kalman gain:(30)K2i=P2,ki⋅HkiT⋅S−1,where factorSis calculated as follows:
(31)Ski=Hki⋅P2,ki⋅HkiT.The final coordinate estimate is(32)X^ki=X^k−1i+K2i⋅Eki.And then, the covariance goes like this:(33)P2,ki=I4−K2,ki⋅Hki⋅P2,ki.Then,M-estimation was carried out like this.State value and measured value can be rewritten as(34)Xk=I4H2,k−1AX^k−1dk−hx^2,k−1,k−1+H2,kX2,k+ek.H2,k is the Jacobian matrix. A is the state-transition matrix,ek can be calculated in this way:
(35)ek=Ax,k−1−X^k−1−vk.So, you have a linear regression model with this form(36)y~=Dx+bfυ+v~.Among them,(37)y^=Ck−1X^kdk1−d^ki+H2,k⋅X^2,k,D=Ck−1I4H2,k,v^=−Ck−1ek.And the factors can be calculated like this:(38)X^k+1=X^k+μDTD−1DTψv^/σV,(39)μ=1/1.25max∣ψ′v^/σV(40)ψv=v∣v∣≤c1,b⋅tanh0.5bc2−∣v∣sgnv,0∣v∣>c2,where the choice of b requires the function to be continuous at c1 and c2, and c1=1.5 and c2=3; in this simulation, if ∣X^l+1−X^l∣<ε stop, otherwise go to (Equation (38)).


## 4.6. Data Fusion Algorithm Based on Interacting Multiple Model (IMM)

The model transformation probability is expressed asT:(41)T=T1,1T1,2T2,1T2,2,The probability of NLOS and LOS in the initial stage isp1=0.50.5.After that, we first update the probability(42)c=p×T.Then, the data is fused and input into the parallel model(43)X^k−1=X∧k−1T×pmix.Among them,(44)pmix=T1,1⋅pn,1c1T1,2⋅pn,1c2T2,1⋅pn,2c1T2,2⋅pn,2c2,X^k−1=X^k−11X^k−12.Input the mixed data into the model, and then calculate the covariance of the state prediction residual as (Equation (29)), then calculate the model likelihood according to the following formula:
(45)λ=exp−E/S⋅E/22π⋅detS,where exp() represents exponential operator and det() represents matrix determinant operator. Then, update matrixp.
(46)p=λ1⋅c1c×λλ2⋅c2c×λ.


## 4.7. Maximum Likelihood Method

Assume that the horizontal and vertical coordinate matrices of randomly set nodes arexi and yi, respectively.
(47)A=x1−x2y1−y2::x1−xNy1−yN,B=d∧2k2−d∧1k2−x22+y22+x12+y12::d∧Nk2−d∧1k2−xN2+yN2+x12+y12.Then, we can obtain the 2D coordinate position as(48)xk,ykT=ATA−1ATB.


## 5. Simulation and Experiment Results

### 5.1. Simulation Results

In this part, we evaluate the performance of the proposed algorithm. The simulation platform is MATLAB. We compare the algorithm proposed with KF [3], EKF [21], and RIMM [21]. The results are based on 500 times Monte Carlo realizations. In order to evaluate the accuracy performance of the algorithm, we adopt the root-mean-square error (RMSE) as the performance metric:
(49)RMSE=1tn∑i=1t∑m=1mxm−x∧im2+ym−y∧im2.where tn is the total number of data. xm and ym are the transverse and longitudinal coordinates of MN. x^im and y^im are the horizontal ordinate of the MN at the time of the i. In this simulation, tn=70.UWB technology is a kind of wireless carrier technology, which has the advantages of low system complexity, high positioning accuracy, and wide spectrum range. In simulation, the default setting of UWB nodes is 7, other default parameters are given in Table2.Table 2
The default parameter values.
ParameterSymbolDefault valuesThe number of beacon nodesN7The probability of NLOS propagationφ0.7The standard deviation of measurement noiseσLOS1The number of sample pointstn70


#### 5.1.1. NLOS Obeys Gaussian Distribution

Figure2 is one of the simulate experiment. The red line in the Figure 2(a) represents the real track, the blue line represents the track after RIMM processing, the yellow line and the green line represent KF and EKF, respectively, and the black line represents the proposed algorithm. From the simulation results, we can see that the KF has the worst effect, while RIMM and the proposed algorithm are very close to the real track, and we can clearly distinguished that in the right figure. The proposed algorithm is better than RIMM commonly. According to the Monte Carlo simulation, the probability of triggering threshold correction is 87%.Figure 2
The results of a simulation experiment.
(a)(b)As shown in Figure3, when NLOS does not exist, the effect of proposed algorithm almost consistent with the RIMM effect, and slightly better than EKF. Along with the rising of the LOS standard deviation, the RMSE of the four algorithms increases and the increasing speed in ascending order from large to small is KF, EKF, RIMM, and proposed algorithm, when LOS standard deviation is 8, the Algorithm excel RIMM by about 13.41%, compared with the EKF, the effect is about 23.18% increased while the visibility of standard deviation is 4, 15.51% more than the RIMM effect.Figure 3
The standard deviation of LOS errors versus RMSE.Figure4 shows the CDF of each algorithm when the NLOS mean value equal to 7; from the picture ,we can clearly know that the slope of the algorithm proposed in CDF graph is obviously higher than that of the other three algorithms. It can be concluded that the algorithm is relatively stable. By comparing the robustness of the four algorithms, we can find that the best is the proposed algorithm (90% probability of error within 5 meters), followed by RIMM, EKF, and KF.Figure 4
The CDF of location error when NLOS mean value is equal to 5.As shown in Figure5, with the increase of NLOS standard deviation, all the four algorithms show an increasing trend. When the NLOS standard deviation is equal to LOS standard deviation (no NLOS exists), the algorithm proposed in this paper is not obvious compared with RIMM. But when the standard deviation of NLOS increases, KF and EKF increase rapidly, while RIMM is basically parallel to the proposed algorithm. Compared with RIMM, the localization accuracy of the proposed algorithm is improved by about 16.56%, and compared with EKF, the localization accuracy improved by about 34.99%.Figure 5
The standard deviation of NLOS errors versus RMSE.


#### 5.1.2. NLOS Obeys Exponential Distribution

As shown in Figure6, it can be seen from the figure that, under the exponential distribution, the RMSE shows a trend of increasing, and the increasing speed in ascending order is KF, EKF, RIMM, and proposed algorithm. When the standard deviation of NLOS is 2, the effect of this algorithm increases by about 15.95% compared with RIMM, and the effect of this algorithm increases by about 24.88% compared with EKF.Figure 6
The exponential standard deviation of NLOS errors versus RMSE.Figure7 is the algorithm performance when NLOS is in exponential distribution. When the NLOS mean value is 5, the probability of error within 5 meters is 87% for proposed algorithm, while the probability of KF, RIMM, and EKF is 50%.Obviously we can know that the proposed algorithm has better robustness than RIMM and the EKF.Figure 7
The CDF of location error when NLOS mean value is equal to 5.As shown in Figure8, when the error presents exponential distribution, the advantage of proposed algorithm is more obvious. The mean value of NLOS error is equal to 2, the effect is improved by about 27.96% compared with EKF, and by about 20.96% compared with RIMM. When the NLOS mean error exceeds 7, the rising speed of the curve slows down obviously for all four algorithms.Figure 8
The exponential distribution NLOS mean value versus RMSE.


#### 5.1.3. NLOS Obeys Uniform Distribution

As shown in Figure9, under uniform distribution, the effect of this algorithm is better than that of EKF and RIMM with the increase of NLOS standard deviation. The trend of the four algorithms is parallel. The effect is about 18.10% better than that of RIMM and 24.21% better than that of EKF.Figure 9
The uniform distribution standard deviation of NLOS errors versus RMSE.As shown in Figure10.When the mean value of NLOS error change from 2 to 8, the effect of this algorithm is better than that of EKF and RIMM. And when the mean value of NLOS error is 2, the effect of EKF and RIMM is very close, with an increase of about 14.35%. When the mean value is 8, the effect of this algorithm is increased by about 7.65% compared with RIMM.Figure 10
The uniform distribution mean value of NLOS errors versus RMSE.


### 5.2. Experimental Results

#### 5.2.1. Experimental Equipment

Figure11 shows the equipment used in the experiment. The hardware version of the UWB beacon node used in the real experiment is D-DWM-PG1.7V, which is an evaluation board based on DWM1000 official positioning module. USB serial port adopts scheme CP2102 to make the hardware serial port is more stable.Figure 11
UWB beacon node.


#### 5.2.2. Real Experiment Conclusion

In order to further verify the positioning accuracy of the proposed algorithm, we carried out experiments in real environments. As shown in Figure12, there are seven BNs set in the room; the room is 10 meters long and 12 meters wide. Because there are many obstacles in the room, the measurements are prone to be disturbed by NLOS factors, and in order to avoid the reflection of UWB signal from the ground, the MN is moved 1.5 m above the ground.Figure 12
The deployment plan for the test.The simulation results demonstrate good performance for the proposed algorithm; we verified the performance of the proposed algorithm in real indoor environment. As shown in Figure12, we randomly deploy 7 beacon nodes in a 10m×10m area. In the entire process, each node obtained 80 sets of distance dki.As shown in Figure13, the effect of the algorithm proposed in this paper is the best one among the four algorithms totally, followed by the RIMM algorithm. The KF and EKF show a very close effect, but there is still a gap compared with RIMM.Figure 13
The RMSE in experimental test.From Figure14, we can know the curve of four algorithm increase with the same speed at the beginning, but KF preforms quiet bad in the latter. RIMM and EKF have similar performances, while the proposed algorithm is the best.Figure 14
The CDF graphs of four algorithms in real environment.


## 5.1. Simulation Results

In this part, we evaluate the performance of the proposed algorithm. The simulation platform is MATLAB. We compare the algorithm proposed with KF [3], EKF [21], and RIMM [21]. The results are based on 500 times Monte Carlo realizations. In order to evaluate the accuracy performance of the algorithm, we adopt the root-mean-square error (RMSE) as the performance metric:
(49)RMSE=1tn∑i=1t∑m=1mxm−x∧im2+ym−y∧im2.where tn is the total number of data. xm and ym are the transverse and longitudinal coordinates of MN. x^im and y^im are the horizontal ordinate of the MN at the time of the i. In this simulation, tn=70.UWB technology is a kind of wireless carrier technology, which has the advantages of low system complexity, high positioning accuracy, and wide spectrum range. In simulation, the default setting of UWB nodes is 7, other default parameters are given in Table2.Table 2
The default parameter values.
ParameterSymbolDefault valuesThe number of beacon nodesN7The probability of NLOS propagationφ0.7The standard deviation of measurement noiseσLOS1The number of sample pointstn70


### 5.1.1. NLOS Obeys Gaussian Distribution

Figure2 is one of the simulate experiment. The red line in the Figure 2(a) represents the real track, the blue line represents the track after RIMM processing, the yellow line and the green line represent KF and EKF, respectively, and the black line represents the proposed algorithm. From the simulation results, we can see that the KF has the worst effect, while RIMM and the proposed algorithm are very close to the real track, and we can clearly distinguished that in the right figure. The proposed algorithm is better than RIMM commonly. According to the Monte Carlo simulation, the probability of triggering threshold correction is 87%.Figure 2
The results of a simulation experiment.
(a)(b)As shown in Figure3, when NLOS does not exist, the effect of proposed algorithm almost consistent with the RIMM effect, and slightly better than EKF. Along with the rising of the LOS standard deviation, the RMSE of the four algorithms increases and the increasing speed in ascending order from large to small is KF, EKF, RIMM, and proposed algorithm, when LOS standard deviation is 8, the Algorithm excel RIMM by about 13.41%, compared with the EKF, the effect is about 23.18% increased while the visibility of standard deviation is 4, 15.51% more than the RIMM effect.Figure 3
The standard deviation of LOS errors versus RMSE.Figure4 shows the CDF of each algorithm when the NLOS mean value equal to 7; from the picture ,we can clearly know that the slope of the algorithm proposed in CDF graph is obviously higher than that of the other three algorithms. It can be concluded that the algorithm is relatively stable. By comparing the robustness of the four algorithms, we can find that the best is the proposed algorithm (90% probability of error within 5 meters), followed by RIMM, EKF, and KF.Figure 4
The CDF of location error when NLOS mean value is equal to 5.As shown in Figure5, with the increase of NLOS standard deviation, all the four algorithms show an increasing trend. When the NLOS standard deviation is equal to LOS standard deviation (no NLOS exists), the algorithm proposed in this paper is not obvious compared with RIMM. But when the standard deviation of NLOS increases, KF and EKF increase rapidly, while RIMM is basically parallel to the proposed algorithm. Compared with RIMM, the localization accuracy of the proposed algorithm is improved by about 16.56%, and compared with EKF, the localization accuracy improved by about 34.99%.Figure 5
The standard deviation of NLOS errors versus RMSE.


### 5.1.2. NLOS Obeys Exponential Distribution

As shown in Figure6, it can be seen from the figure that, under the exponential distribution, the RMSE shows a trend of increasing, and the increasing speed in ascending order is KF, EKF, RIMM, and proposed algorithm. When the standard deviation of NLOS is 2, the effect of this algorithm increases by about 15.95% compared with RIMM, and the effect of this algorithm increases by about 24.88% compared with EKF.Figure 6
The exponential standard deviation of NLOS errors versus RMSE.Figure7 is the algorithm performance when NLOS is in exponential distribution. When the NLOS mean value is 5, the probability of error within 5 meters is 87% for proposed algorithm, while the probability of KF, RIMM, and EKF is 50%.Obviously we can know that the proposed algorithm has better robustness than RIMM and the EKF.Figure 7
The CDF of location error when NLOS mean value is equal to 5.As shown in Figure8, when the error presents exponential distribution, the advantage of proposed algorithm is more obvious. The mean value of NLOS error is equal to 2, the effect is improved by about 27.96% compared with EKF, and by about 20.96% compared with RIMM. When the NLOS mean error exceeds 7, the rising speed of the curve slows down obviously for all four algorithms.Figure 8
The exponential distribution NLOS mean value versus RMSE.


### 5.1.3. NLOS Obeys Uniform Distribution

As shown in Figure9, under uniform distribution, the effect of this algorithm is better than that of EKF and RIMM with the increase of NLOS standard deviation. The trend of the four algorithms is parallel. The effect is about 18.10% better than that of RIMM and 24.21% better than that of EKF.Figure 9
The uniform distribution standard deviation of NLOS errors versus RMSE.As shown in Figure10.When the mean value of NLOS error change from 2 to 8, the effect of this algorithm is better than that of EKF and RIMM. And when the mean value of NLOS error is 2, the effect of EKF and RIMM is very close, with an increase of about 14.35%. When the mean value is 8, the effect of this algorithm is increased by about 7.65% compared with RIMM.Figure 10
The uniform distribution mean value of NLOS errors versus RMSE.


## 5.1.1. NLOS Obeys Gaussian Distribution

Figure2 is one of the simulate experiment. The red line in the Figure 2(a) represents the real track, the blue line represents the track after RIMM processing, the yellow line and the green line represent KF and EKF, respectively, and the black line represents the proposed algorithm. From the simulation results, we can see that the KF has the worst effect, while RIMM and the proposed algorithm are very close to the real track, and we can clearly distinguished that in the right figure. The proposed algorithm is better than RIMM commonly. According to the Monte Carlo simulation, the probability of triggering threshold correction is 87%.Figure 2
The results of a simulation experiment.
(a)(b)As shown in Figure3, when NLOS does not exist, the effect of proposed algorithm almost consistent with the RIMM effect, and slightly better than EKF. Along with the rising of the LOS standard deviation, the RMSE of the four algorithms increases and the increasing speed in ascending order from large to small is KF, EKF, RIMM, and proposed algorithm, when LOS standard deviation is 8, the Algorithm excel RIMM by about 13.41%, compared with the EKF, the effect is about 23.18% increased while the visibility of standard deviation is 4, 15.51% more than the RIMM effect.Figure 3
The standard deviation of LOS errors versus RMSE.Figure4 shows the CDF of each algorithm when the NLOS mean value equal to 7; from the picture ,we can clearly know that the slope of the algorithm proposed in CDF graph is obviously higher than that of the other three algorithms. It can be concluded that the algorithm is relatively stable. By comparing the robustness of the four algorithms, we can find that the best is the proposed algorithm (90% probability of error within 5 meters), followed by RIMM, EKF, and KF.Figure 4
The CDF of location error when NLOS mean value is equal to 5.As shown in Figure5, with the increase of NLOS standard deviation, all the four algorithms show an increasing trend. When the NLOS standard deviation is equal to LOS standard deviation (no NLOS exists), the algorithm proposed in this paper is not obvious compared with RIMM. But when the standard deviation of NLOS increases, KF and EKF increase rapidly, while RIMM is basically parallel to the proposed algorithm. Compared with RIMM, the localization accuracy of the proposed algorithm is improved by about 16.56%, and compared with EKF, the localization accuracy improved by about 34.99%.Figure 5
The standard deviation of NLOS errors versus RMSE.


## 5.1.2. NLOS Obeys Exponential Distribution

As shown in Figure6, it can be seen from the figure that, under the exponential distribution, the RMSE shows a trend of increasing, and the increasing speed in ascending order is KF, EKF, RIMM, and proposed algorithm. When the standard deviation of NLOS is 2, the effect of this algorithm increases by about 15.95% compared with RIMM, and the effect of this algorithm increases by about 24.88% compared with EKF.Figure 6
The exponential standard deviation of NLOS errors versus RMSE.Figure7 is the algorithm performance when NLOS is in exponential distribution. When the NLOS mean value is 5, the probability of error within 5 meters is 87% for proposed algorithm, while the probability of KF, RIMM, and EKF is 50%.Obviously we can know that the proposed algorithm has better robustness than RIMM and the EKF.Figure 7
The CDF of location error when NLOS mean value is equal to 5.As shown in Figure8, when the error presents exponential distribution, the advantage of proposed algorithm is more obvious. The mean value of NLOS error is equal to 2, the effect is improved by about 27.96% compared with EKF, and by about 20.96% compared with RIMM. When the NLOS mean error exceeds 7, the rising speed of the curve slows down obviously for all four algorithms.Figure 8
The exponential distribution NLOS mean value versus RMSE.


## 5.1.3. NLOS Obeys Uniform Distribution

As shown in Figure9, under uniform distribution, the effect of this algorithm is better than that of EKF and RIMM with the increase of NLOS standard deviation. The trend of the four algorithms is parallel. The effect is about 18.10% better than that of RIMM and 24.21% better than that of EKF.Figure 9
The uniform distribution standard deviation of NLOS errors versus RMSE.As shown in Figure10.When the mean value of NLOS error change from 2 to 8, the effect of this algorithm is better than that of EKF and RIMM. And when the mean value of NLOS error is 2, the effect of EKF and RIMM is very close, with an increase of about 14.35%. When the mean value is 8, the effect of this algorithm is increased by about 7.65% compared with RIMM.Figure 10
The uniform distribution mean value of NLOS errors versus RMSE.


## 5.2. Experimental Results

### 5.2.1. Experimental Equipment

Figure11 shows the equipment used in the experiment. The hardware version of the UWB beacon node used in the real experiment is D-DWM-PG1.7V, which is an evaluation board based on DWM1000 official positioning module. USB serial port adopts scheme CP2102 to make the hardware serial port is more stable.Figure 11
UWB beacon node.


### 5.2.2. Real Experiment Conclusion

In order to further verify the positioning accuracy of the proposed algorithm, we carried out experiments in real environments. As shown in Figure12, there are seven BNs set in the room; the room is 10 meters long and 12 meters wide. Because there are many obstacles in the room, the measurements are prone to be disturbed by NLOS factors, and in order to avoid the reflection of UWB signal from the ground, the MN is moved 1.5 m above the ground.Figure 12
The deployment plan for the test.The simulation results demonstrate good performance for the proposed algorithm; we verified the performance of the proposed algorithm in real indoor environment. As shown in Figure12, we randomly deploy 7 beacon nodes in a 10m×10m area. In the entire process, each node obtained 80 sets of distance dki.As shown in Figure13, the effect of the algorithm proposed in this paper is the best one among the four algorithms totally, followed by the RIMM algorithm. The KF and EKF show a very close effect, but there is still a gap compared with RIMM.Figure 13
The RMSE in experimental test.From Figure14, we can know the curve of four algorithm increase with the same speed at the beginning, but KF preforms quiet bad in the latter. RIMM and EKF have similar performances, while the proposed algorithm is the best.Figure 14
The CDF graphs of four algorithms in real environment.


## 5.2.1. Experimental Equipment

Figure11 shows the equipment used in the experiment. The hardware version of the UWB beacon node used in the real experiment is D-DWM-PG1.7V, which is an evaluation board based on DWM1000 official positioning module. USB serial port adopts scheme CP2102 to make the hardware serial port is more stable.Figure 11
UWB beacon node.


## 5.2.2. Real Experiment Conclusion

In order to further verify the positioning accuracy of the proposed algorithm, we carried out experiments in real environments. As shown in Figure12, there are seven BNs set in the room; the room is 10 meters long and 12 meters wide. Because there are many obstacles in the room, the measurements are prone to be disturbed by NLOS factors, and in order to avoid the reflection of UWB signal from the ground, the MN is moved 1.5 m above the ground.Figure 12
The deployment plan for the test.The simulation results demonstrate good performance for the proposed algorithm; we verified the performance of the proposed algorithm in real indoor environment. As shown in Figure12, we randomly deploy 7 beacon nodes in a 10m×10m area. In the entire process, each node obtained 80 sets of distance dki.As shown in Figure13, the effect of the algorithm proposed in this paper is the best one among the four algorithms totally, followed by the RIMM algorithm. The KF and EKF show a very close effect, but there is still a gap compared with RIMM.Figure 13
The RMSE in experimental test.From Figure14, we can know the curve of four algorithm increase with the same speed at the beginning, but KF preforms quiet bad in the latter. RIMM and EKF have similar performances, while the proposed algorithm is the best.Figure 14
The CDF graphs of four algorithms in real environment.


## 6. Conclusions

The article presented addresses the problem of robust tracking, using TOA measurements in NLOS environments. We reduce the influence of NLOS phenomenon by using REKF. Then, we combine OF and GEUPF to further weaken the noise of the signal in the process of transmission. After filtering, we use a dynamically adjustable threshold to limit the step size of each step. Finally, we use ML to determine the position of the MN. The purposed algorithm preform great when the environment contains serious NLOS because theM-estimation, adjustable threshold, and the genetic algorithm are introduced in it. The real experiment and computer simulation all show that the algorithm has strong robustness and better than RIMM, EKF, and KF. In the future, we will continue to improve the trend judgment algorithm and investigate an adaptive algorithm to the corresponding filter, so that it can achieve better results in a more complex environment.



---

*Source: 8854389-2020-09-01.xml*