A positioning algorithm based on improved robust extended Kalman filter with NLOS identification and mitigation

Abstract

With the development of the information age and the maturity of Internet of Things technology, wireless sensor network has been widely applied in indoor localization. However, the non-line-of-sight (NLOS) propagation in complicated environment and the inherent noise of the sensor will introduce errors in the measurements, which will seriously lead to inaccurate positioning. In this paper, a novel localization scheme based on the mean reconstruction method is proposed, which reconstructs the distance measurements from all beacon nodes by taking the average twice to weaken the adverse effects of NLOS. At the same time, the noise average is re-estimated when the distance difference is not too large. Next, the robust extended Kalman filter (REKF) is used to process the reconstructed distance measurements to obtain positioning results. To make the positioning results more accurate, hypothesis test is used as NLOS identification to classify the position estimates generated from all distance combinations by least-squares. Then, the residual weighting (RWGH) method is utilized to combine the position estimates that fall into the validation region. At last, we merge the results from RWGH and REKF. The simulation and experimental results show that the proposed algorithm has high positioning accuracy and strong positioning robustness.

1 Introduction

A certain number of low-energy sensors are deployed in a specific range indoors or outdoors, thus forming a wireless sensor network (WSN). Using communication technology to receive relevant information about the target, such as time of arrival (TOA), time difference of arrival (TDOA), angle of arrival (AOA) and received signal strength (RSS). The researchers have extended wireless positioning technology, which has good effects on science, military and social life [1]. In WSN, the pre-fixed sensors are called beacon nodes (BNs), while the state of the object to be located is indeterminate, usually in motion, so we call it mobile node (MN). According to whether there are obstacles between the BN and the MN, the channel can be divided into two types: line-of-sight (LOS) and non-line-of-sight (NLOS) propagation. In the real world, obstacles such as humans, tables, cars, and plants will cause diffraction and refraction of the electric signal during the NLOS transmission process and produce the signal time delay, which make the measurements become larger than actual values [2]. Thus eventually lead to the deviation between the positioning trajectory and the actual mobile trajectory [3]. The errors of contaminated measurements in NLOS transmission are mainly NLOS errors, which make the measurements have positive errors that can’t be ignored. The NLOS errors are adverse to the positioning accuracy, so it is especially crucial to alleviate and eliminate the NLOS errors.

Nowadays, several NLOS processing algorithms have been proposed. Some algorithms directly process all original measurement data [4,5,6,7,8,9]; while others identify the measurement data and divide them into several categories [10,11,12,13,14,15,16], even discarding the measurement values with larger errors and only utilizing the measurement values with smaller errors into subsequent steps. Therefore, we can divide these algorithms into NLOS mitigation and NLOS identification.

In this paper, we propose a positioning algorithm based on mean reconstruction REKF by using the TOA measurements, which combines NLOS mitigation and NLOS identification. The proposed algorithm has made some improvements in positioning performance and algorithm applicability. The main contributions in the proposed algorithm as follows:

• 1. The proposed algorithm is based on REKF, which integrates robust regression into EKF and can be applied to nonlinear systems. Unlike the IMM framework, the REKF has a significant advantage that does not depend on the prior knowledge of the NLOS errors, so REKF is more in line with the actual circumstances.

• 2. In the worst case, our algorithm time complexity is $(r + K) \cdot O(n)$, where r is the number of Newton–Raphson iterations and K is the total number of distance combinations.

• 3. The proposed algorithm has a significant advantage in localization performance with high accuracy and strong robustness. In addition, the proposed algorithm is suitable for different NLOS error distributions and independent of BNs distribution. Particularly, the proposed algorithm works better for the environments with lots of NLOS errors.

The subsequent structure of the paper is arranged as follows: Sect. 2 introduces related works. Sections 3 and 4 describe the signal model and the proposed algorithm in detail respectively. We conducted simulations and experiments, and analyzed the results in Sect. 5. The conclusion is drawn in Sect. 6.

2 Related works

2.1 NLOS mitigation

There are two strategies for NLOS mitigation: the first one is only to remedy range errors. The residual weighting (RWGH) method is to take the reciprocal of the distance difference as the residual, which removes the effect of outliers by giving less weight to points farther away from the measurements [4]. However, the complexity of the method needs to be reduced, some researchers were committed to improving the residual weighting algorithm. For instance, by reserving the combinations with the smallest residuals and performing summation, the RWGH method becomes computationally simple [5]. The iterative minimum residual algorithm is proposed in [6], which sets a minimum residual threshold, and iteratively retains position estimates with residuals less than this threshold as final results. In TDOA positioning, solving optimization problems by convex relaxation is also a hot topic for NLOS mitigation. Through l(1)-norm robustification and neurodynamic optimization in NLOS, the convex optimization problems with inequality constraints are solved by redefining Augmented Lagrangian effectively [7].

The second strategy is to directly suppress the NLOS impact by using dedicated localization techniques. The authors in [8] proposes a NLOS nodes localization scheme based on Harris Hawke optimization algorithm to promote reliable data propagation between vehicle nodes in emergency situations. It can locate NLOS nodes based on their dynamic characteristics and adaptive positioning styles obtained through reference nodes. A novel self-learning LS positioning algorithm has been proposed in [9], which can not only solve the distortion problem of LS in NLOS environment, but also not be limited to the condition that LS requires at least three nodes in localization problems.

2.2 NLOS identification

The NLOS identification part is generally conducted before NLOS mitigation, which can recognize whether the propagation state between nodes is LOS or NLOS, after identification different error elimination technologies are used for different conditions. The NLOS identification methods include range, channel and location aspects. The identification method based on channel is challenging, and the method based on distance can be measured by UWB, which is relatively easy to obtain, and the position can also be calculated using distance. The method based on distance mainly use the probability density function (PDF) or variance of range estimates to distinguish between LOS and NLOS [10]. In location identification, probability theory [11] or machine learning [12] can be used to determine whether the position estimate is generated by NLOS measurements.

There exist some methods to identify whether the state is in LOS or NLOS. The Wylie method mainly calculates the measurement variance and judges whether the distance measurements are contaminated by NLOS errors [13]. The decision theory and z-test are used for identifying NLOS propagation [14], and then the NLOS measurement data are discarded. Even if the prior information of NLOS error is unknown, the algorithm is still effective. These identification methods are based on range. The modified probabilistic data association (MPDA) algorithm obtains a position estimate through different combinations of ranging by LS [15]. The NLOS detection is to identify the optimized position estimate which falls into the validation gate. Then the optimized position estimates are reserved and used to calculate the position of the MN by the corresponding association probability weighting. The method is based on location. A practical NLOS identification (PNI) technique based on support vector machine (SVM) is designed in [16], which can reduce the effects of transmission channel-dependent characteristics and optimize the classifier, so it’s based on channel. Our algorithm improves the hypothesis test in the MPDA to determine whether the position estimate is from LOS or NLOS.

2.3 NLOS identification and NLOS mitigation

Some positioning algorithms are divided into two steps: first identifying the NLOS error and then eliminating it. All of them play a certain role in improving the positioning accuracy. The Wylie method uses the time history of distance measurements from each BN, combined with the method of measuring noise standard deviation and residual analysis rank test, to determine whether there exists NLOS error in the distance measurements [13]. The second step is to eliminate the NLOS errors to achieve true TOA reconstruction by using the time history of TOA measurements. A distance measurement reconstruction mode was proposed in [17]. After identifying the NLOS error, Liu uses an orthogonal polynomial to optimize the distance measurements obtained by the biased Kalman filter (KF). In [18], an Bayesian sequential test is designed to identify whether the distance measurements are contaminated by NLOS errors. After identification, the original distance measurements, which are contaminated by NLOS errors, are optimized by a modified Kalman filter (MKF). In [19], according to the number of position estimates that fall into the validation gate, the environment at this moment is divided into mild NLOS and severe NLOS condition; then the average value of NLOS error is estimated by using the historical positioning results, and the distance measurements from each beacon node are reconstructed. Our algorithm reconstructs the distance measurements to reduce the NLOS errors and determines whether the position estimate is from LOS or NLOS, and finally combines the results.

3 Signal model

${\varvec{X}}(n) = [x(n) \, y{(}n{) }\hat{x}(n) \, \hat{y}{(}n{)}]^{T}$ is the state vector of the MN, where n is the time step and N is the whole time steps. $(x(n),y(n))$ is the coordinate of the MN and $(\hat{x}(n) \, \hat{y}{(}n{))}$ denotes the velocity of the MN. According to the force model, the state vector of the MN changes over time, which can be described as:

$${\varvec{X}}(n){ = }{\varvec{FX}}(n - 1) + {\varvec{Gu}}(n - 1),\quad n{ = }1,2, \ldots ,N.$$

(1)

$${\varvec{F}} = \left[ {\begin{array}{*{20}c} 1 & 0 & {\Delta t} & 0 \\ 0 & 1 & 0 & {\Delta t} \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{array} } \right],\;{\varvec{G}} = \left[ {\begin{array}{*{20}c} {\Delta t^{2} /2} & 0 \\ 0 & {\Delta t^{2} /2} \\ {\Delta t} & 0 \\ 0 & {\Delta t} \\ \end{array} } \right],$$

(2)

where, $\Delta t$ is the sampling period. ${\varvec{u}}(n)$ is the driving noise, which is assumed to be a Gaussian white noise with a mean of zero and covariance matrix of ${\varvec{Q}}(n)$. F is the state transition matrix of the MN, and G is the interference input matrix which is used to described the random acceleration of the MN causes by ${\varvec{u}}(n)$. Within the specific area, L beacon nodes are randomly deployed to receive the signal from the MN. ${\varvec{D}}(n)$ is the distance measurements based on TOA data between the BNs and MN at time step n, which can be defined as and calculated by follows:

$${\varvec{D}}(n) = \left[ {d_{1} (n),d_{2} (n), \cdots ,d_{L} (n)} \right]^{T}$$

(3)

$${\varvec{D}}(n) = {\varvec{h}}(X(n)) + b(n),\quad n = 1,2, \ldots ,N$$

(4)

where, ${\varvec{h}}({\varvec{X}}(n)) = \left[ {h_{1} (x(n)),h_{2} (x(n)),...,h_{L} (x(n))} \right]^{T}$ represents the actual distances between the BNs and the MN at time step n and ${\varvec{b}}(n) = \left[ {b_{1} (n),b_{2} (n),...,b_{l} (n),...,b_{L} (n)} \right]^{T}$ is the noise vector. The actual distances ${\varvec{h}}({\varvec{X}}(n))$ are measured by Euclidean distance and $h_{l} ({\varvec{X}}(n))$ is the l-th component of ${\varvec{h}}({\varvec{X}}(n))$.

$$h_{l} \left( {{\varvec{X}}(n)} \right) = \sqrt {\left( {x(n) - x_{B} (l)} \right)^{2} + \left( {y(n) - y_{B} (l)} \right)^{2} } ,\quad l = 1, \ldots ,L$$

(5)

where, $(x_{B} (l),y_{B} (l))^{T}$ denotes the coordinate of the l-th BN.

The noise vector ${\varvec{b}}(n)$ consists of sensor noise and NLOS error, and the sensor noise is also called LOS error. ${\varvec{b}}(n)$ contains random variables with a variance describing Gaussian noise due to the NLOS propagation, and it can be defined as:

$$b_{l} (n) = \left\{ {\begin{array}{*{20}l} {\sigma_{LOS} {\varvec{w}}(n)} \hfill & {{\text{LOS}}} \hfill \\ {\sqrt {\sigma_{LOS}^{2} + \sigma_{NLOS}^{2} } {\varvec{w}}(n) + b_{NLOS} } \hfill & {{\text{NLOS}}} \hfill \\ \end{array} } \right.$$

(6)

where, ${\varvec{w}}(n)$ is Gaussian noise $N(0,1^{2} )$. $b_{NLOS}$ denotes the NLOS error, which changes with the environment and could follow different distribution. $\sigma_{LOS}$ and $\sigma_{NLOS}$ are the standard deviation of the sensor noise and the NLOS error respectively. The measurement covariance matrix is:

$$R(n) = diag\left[ {\sigma_{ \, 1}^{2} ,\sigma_{ \, 2}^{2} , \ldots ,\sigma_{L}^{2} } \right],\quad l = 1,2, \ldots ,L$$

(7)

where $\sigma_{L}^{2}$ represents the variance of the noise.

In this paper, we assume that the standard deviation of the sensor noise $\sigma_{LOS}$ is known beforehand, while $\sigma_{NLOS}$ is unknown for us.

4 The proposed algorithm program

4.1 General concept

The flowchart of the proposed algorithm is illustrated in Fig. 1. First, we obtain a predicted position through Kalman prediction with real initial position information. Then we calculate the distances from the predicted position to all beacon nodes and make difference with the original distance measurements. In the first averaging, the distance differences from all beacon nodes are averaged, and the second averaging is the historical time average. The average value of the two times is used as the estimation of noise average, and then the mean reconstruction is performed: we subtract the estimation of noise average from the original distance measurements, and the results are approximated as the distances from all beacon nodes to the mobile node, thereby reducing the error caused by the noisy environment. However, reconstruction may produce a negative error when the noise is small, so we re-estimate the noise. After that, the robust regression technology is introduced to improve the EKF, which does not need to assume the prior knowledge about the NLOS errors. To further enhance the positioning accuracy, we divide the distance measurements into multiple groups, and each group produces a position estimate by least-squares. Then by processing these position estimates with EKF, the filtered position estimates can be obtained. LOS identification is to recognize whether the position estimate is in LOS through hypothesis testing. The position estimates whose test statistic is not greater than the validation threshold can fall into the validation region and are selected for residual weighting, and the other position estimates are discarded. Lastly, the result of REKF and the result of residual weighting are merged as the positioning result, which will be input into the next moment positioning.

Fig. 1

The flowchart of the proposed algorithm

4.2 Mean reconstruction method

In indoor localization, the sensors return observation value ${\varvec{D}}(n)$, which consisted of the actual distances and the noise ${\varvec{b}}(n)$ [20]. ${\varvec{b}}(n)$ is mixed with the LOS error and the NLOS error. The LOS error is generally smaller than NLOS error. So we can use the noise estimation to estimate NLOS error. The NLOS error has a certain rule and follows a specific distribution, which has different values with different probabilities. We take the method of averaging multiple distance measurements differences, the random NLOS errors tend to offset each other in the linear dynamic system in the process of averaging [21].

Assuming that the initial state information is known and the movement is entered from the door of the room, the position of the door can be used as the initial position. Based on this accurate data, the Kalman prediction position at the first moment is close to the real position. We calculate the distances from the predicted position to all beacon nodes, and the differences can be considered as the noise. The average of the differences from all beacon node can be approximated as the mean value of noise. On the basis of the above operations, the Kalman filter values of the subsequent time steps are also relatively accurate. Therefore, we can estimate the noise and average the differences in subsequent time steps.

4.2.1 Kalman prediction

The state vector ${\varvec{X}}(0|0)$ and covariance matrix ${\varvec{P}}(0|0)$ are initialized. The state vector and covariance matrix of current time step n can be predicted by the state and the covariance matrix at previous time step n − 1:

$${\varvec{X}}(n|n - 1) = {\varvec{FX}}(n - 1|n - 1)$$

(8)

$${\varvec{P}}(n|n - 1) = {\varvec{FP}}(n - 1|n - 1){\varvec{F}}^{T} + {\varvec{GQG}}^{T}$$

(9)

$D_{e,l} (n)$ is the distance between the predicted position and the l-th BN. We subtract $D_{e,l} (n)$ from the distance measurements $d_{l} (n)$ to get $d_{e,l} (n)$, $d_{e,l} (n)$ is the noise estimate of the l-th BN.

$$D_{e,l} (n) = \left\| {{\varvec{BX}}(n|n - 1) - [x_{B} (l),y_{B} (l)]} \right\|_{2}$$

(10)

$$d_{e,l} (n) = \left\| {d_{l} (n) - D_{e,l} (n)} \right\|_{1}$$

(11)

where, B is $\left[ \begin{gathered} 1{ 0 0 0} \hfill \\ 0 \, 1 \, 0 \, 0 \hfill \\ \end{gathered} \right]$, and $\left\| \bullet \right\|_{n}$ denotes n-norm.

4.2.2 First average

To overcome the randomness of the noise, $d_{e,l} (n)$ from the L BNs are averaged to obtain $M(n)$, which can be approximately considered of the noise at time step n.

$$A(n) = \sum\limits_{l = 1}^{L} {d_{e,l} (n)}$$

(12)

$$M(n) = A(n)/L$$

(13)

4.2.3 Second average

It is assumed that the NLOS errors follow the same distribution in a separate indoor environment with a small range and similar obstacles. So we perform the second average, historical data from initial time step to current time step n are averaged as the estimation of noise average:

$$m(n) = \sum\limits_{i = 1}^{n} {M(i)/n}$$

(14)

With the process of sampling, the average value can represent the digital characteristics of the noise in this environment.

4.2.4 Mean reconstruction

We mainly let the distance measurements directly subtract the noise average estimation. The noise is mixed with the sensor noise and the NLOS error, and the NLOS error is generally larger than sensor noise. Noise average estimation is mainly determined by the large NLOS error, therefore the distance measurement in NLOS minus the estimated mean value $m(n)$ will be closer to the actual distance. To mitigate the influence of NLOS error, we reconstruct the distance measurements by subtracting the recorded mean value:

$$d_{l} (n) = d_{l} (n) - m(n)$$

(15)

However, the distance measurement in LOS with a little noise error, subtracting the estimated mean value $m(n)$, may bring about a negative error. If the following conditions are met, it can be roughly considered as a little noise error.

$$d_{e,l} (n) < \sigma_{LOS} ,d_{e,l} (n) < M(n),\quad l = 1, \ldots ,L$$

(16)

For this case, similar to formula (12) and (13), re-estimate the noise to reduce the influence of negative error and make the reconstructed distances close to the actual distances:

$$\tilde{A}(n) = \sum\limits_{l = 1}^{T} {d_{e,l} (n)}$$

(17)

$$\tilde{M}(n) = \tilde{A}(n)/T$$

(18)

where, T is the number that $d_{e,l} (n)$ is judged to be a little error. For little noise error, we reconstruct the distance measurements by subtracting the re-estimated value of the noise:

$$d_{l} (n) = d_{l} (n) - \tilde{M}(n)$$

(19)

4.3 REKF algorithm

4.3.1 Deformation of the EKF formula

In the EKF, the Jacobian matrix ${\varvec{H}}(n)$, the innovation ${\varvec{v}}(n)$, the innovation covariance matrix ${\varvec{S}}(n)$ and the extended Kalman gain ${\varvec{K}}(n)$ at time step n can be calculated as follows:

$${\varvec{H}}(n) = \frac{{\partial {\varvec{h}}\left( {{\varvec{X}}(n|n - 1)} \right)}}{{\partial {\varvec{X}}(n|n - 1)}}$$

(20)

$${\varvec{v}}(n) = {\varvec{D}}(n) - {\varvec{h}}\left( {{\varvec{X}}(n|n - 1)} \right)$$

(21)

$${\varvec{S}}(n) = {\varvec{H}}(n){\varvec{P}}(n|n - 1){\varvec{H}}^{T} (n) + {\varvec{R}}(n)$$

(22)

$${\varvec{K}}(n) = {\varvec{P}}(n|n - 1){\varvec{H}}^{T} \left( {{\varvec{S}}(n)} \right)^{ - 1}$$

(23)

where ${\varvec{D}}(n)$ is the reconstructed distance measurements. The update of the state and covariance matrix are:

$${\varvec{X}}(n|n) = {\varvec{X}}(n|n - 1) + {\varvec{K}}(n){\varvec{v}}(n)$$

(24)

$${\varvec{P}}(n|n) = E\left[ {({\varvec{X}}(n) - {\varvec{X}}(n|n))({\varvec{X}}(n) - {\varvec{X}}(n|n))^{T} } \right]$$

(25)

In the next steps, we replace ${\varvec{X}}(n)$ with ${\varvec{X}}$. The EKF equation is rewritten and transformed into linear regression model by applying the robust technology. The state Eq. (1) and the measurement (4) are rewritten as follows:

$$\left[ {\begin{array}{*{20}c} {{\varvec{I}}_{4} } \\ {{\varvec{H}}(n)} \\ \end{array} } \right]{\varvec{X}}(n) = \left[ {\begin{array}{*{20}c} {{\varvec{FX}}(n - 1|n - 1)} \\ {{\varvec{D}}(n) - {\varvec{h}}({\varvec{X}}(n|n - 1)) + {\varvec{H}}(n){\varvec{X}}(n|n - 1)} \\ \end{array} } \right] + {\varvec{e}}(n)$$

(26)

$${\varvec{e}}(n) = \left[ \begin{gathered} {\varvec{F}}({\varvec{X}}(n - 1) - {\varvec{X}}(n - 1|n - 1)) + {\varvec{Gw}}(n - 1) \hfill \\ \, - {\varvec{v}}(n) \hfill \\ \end{gathered} \right]$$

(27)

$$E\left( {{\varvec{e}}(n){\varvec{e}}^{T} (n)} \right) = \left[ {\begin{array}{*{20}c} {{\varvec{P}}(n|n - 1)} & 0 \\ 0 & {{\varvec{R}}(n)} \\ \end{array} } \right] = {\varvec{C}}(n){\varvec{C}}^{T} (n)$$

(28)

where, $E\left( \cdot \right)$ represents the expectation and ${\varvec{C}}(n)$ is obtained by using Cholesky decomposition for $E\left( {{\varvec{e}}(n){\varvec{e}}^{T} (n)} \right)$. The linear regression model is constructed by multiplying (26) with ${\varvec{C}}^{ - 1} (n)$:

$$\tilde{\user2{y}} = {\varvec{AX}} + bf(\tilde{\user2{v}}) + \tilde{\user2{v}}$$

(29)

$$\tilde{\user2{y}} = {\varvec{C}}^{ - 1} (n)\left[ \begin{gathered} \, {\varvec{X}}(n|n - 1) \hfill \\ {\varvec{D}}(n) - {\varvec{h}}\left( {{\varvec{X}}(n|n - 1)} \right) + {\varvec{H}}(n){\varvec{X}}(n|n - 1) \hfill \\ \end{gathered} \right]$$

(30)

$${\varvec{A}} = {\varvec{C}}^{ - 1} (n)\left[ {\begin{array}{*{20}c} {{\varvec{I}}_{4} } \\ {{\varvec{H}}(n)} \\ \end{array} } \right]$$

(31)

$$\tilde{\user2{v}} = - {\varvec{C}}^{ - 1} (n){\varvec{e}}(n)$$

(32)

4.3.2 Robust regression

Referring to the linear regression model (29), the maximum likelihood estimation of X at time step n can be obtained by solving the following coupled equation:

$$\sum\limits_{i = 1}^{{M + \dim ({\varvec{X}})}} {\left[ {\varvec{A}} \right]_{ij} } \times \varphi \left( {\tilde{\user2{y}}_{i} - \sum\limits_{j^{\prime}}^{{\dim ({\varvec{X}})}} {\left[ {\varvec{A}} \right]_{ij^{\prime}} {\varvec{X}}_{j^{\prime}} } } \right) = 0$$

(33)

where, $\dim ( \cdot )$ is the operation to count the dimension of X and $j^{\prime } = 1,2, \ldots ,\dim ({\varvec{X}})$. However, the pdf $f(\tilde{\user2{v}})$ of the $\tilde{\user2{v}}$ is asymmetric because the mean of the NLOS error is positive. Therefore, we use the descending score function $\varphi \left( \cdot \right)$, which is referred to [22]:

$$\varphi (\theta ) = \left\{ {\begin{array}{*{20}l} \theta \hfill & {\left| \theta \right|{ < }\alpha_{1} } \hfill \\ {\beta_{1} {\text{tanh}}\left[ {0.5b_{1} (\alpha_{2} - \left| \theta \right|)} \right]{\text{sgn(}}\theta {)}} \hfill & {\alpha_{1} \le \left| \theta \right| \le \alpha_{2} } \hfill \\ 0 \hfill & {\left| \theta \right| > \alpha_{2} } \hfill \\ \end{array} } \right.$$

(34)

where, $\beta_{1}$ is chosen to be 1.739; according to [22], we set $\alpha_{1}$ and $\alpha_{2}$ as 1.5 and 3 respectively. The state estimate X obtained above still contains a large error, so we use Newton–Raphson method to optimize X:

At first, we need to obtain the initial state estimate ${\varvec{X}}_{0}$ by least squares (LS)s:

$${\varvec{X}}_{r} = \left( {{\varvec{A}}^{T} {\varvec{A}}} \right)^{ - 1} {\varvec{A}}^{T} \tilde{\user2{y}}\quad r{ = 0}$$

(35)

where, ${\varvec{X}}_{r}$ is the state estimate in r-th iteration.

Next, we determine the error residual $\user2{\hat{\tilde{v}}}$ through deformation by shifting (29) and estimate the error scale $\sigma_{{\text{v}}}$ by (37):

$$\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\tilde{v}} } = \tilde{\user2{y}} - {\varvec{AX}}_{r}$$

(36)

$$\sigma_{v} = 1.483mean\left\{ {\left| {\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\tilde{v}} } - mean(\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\tilde{v}} })} \right|} \right\}$$

(37)

Then, the update step of the robust Kalman filter is performed, in which the state estimate ${\varvec{X}}_{r}$ is updated iteratively by Newton–Raphson method. The iteration will continue until the preset convergence is met, otherwise the iteration will be broken, and the final result will be reserved.

$${\varvec{X}}_{r + 1} = {\varvec{X}}_{r} + \mu \left( {{\varvec{A}}^{T} {\varvec{A}}} \right)^{ - 1} {\varvec{A}}^{T} \varphi (\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\tilde{v}} }/\sigma_{v} )$$

(38)

$$\mu = {1 \mathord{\left/ {\vphantom {1 {\left( {1.25\max \left( {\left| {\varphi ^{\prime}(\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\tilde{v}} /\sigma_{v} )} \right|} \right)} \right)}}} \right. \kern-0pt} {\left( {1.25\max \left( {\left| {\varphi ^{\prime}(\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{\tilde{v}} /\sigma_{v} )} \right|} \right)} \right)}}$$

(39)

$$\left\| {{\varvec{X}}_{r + 1} - {\varvec{X}}_{r} } \right\|_{2} < \varepsilon$$

(40)

where, the preset threshold $\varepsilon$ is set to 0.5.

At last, ${\varvec{X}}_{r + 1}$ is determined as the state vector X at this time step n and the two-dimension coordinates of the position estimates ${\varvec{Z}}(n)$ can be obtained by X multiplying the matrix B:

$${\varvec{Z}}(n) = {\varvec{BX}}(n)$$

(41)

4.4 LOS identification strategy

If the noise is small, the reconstruction has little change to the original distance measurements and the reconstruction distance measurements have limited improvement to the positioning accuracy. The enhancement of the accuracy should be improved from the positioning method. We use multiple position estimates generated by distance measurements to eliminate outliers, and retain the position estimates under the condition of LOS with small errors.

We firstly divide the reconstructed distance measurements into K combinations and each combination generate a corresponding position estimates via least-squares. Then by processing these position estimates with extended Kalman filter (EKF), filtered positions $\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n)$ can be obtained which is expressed as follow:

$$\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n) = \left[ {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{x}_{i} (n),\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{y}_{i} (n)} \right]^{T} ,i = 1, \ldots ,K$$

(42)

_where

$$K = \sum\limits_{i = 3}^{L} {C_{L}^{i} }$$

(43)

Next, we obtain the coordinate ${\varvec{Z}}(n|n - 1)$ of the predicted position and calculate the innovation $\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{v} }_{i} (n)$ between the position estimate of the i-th combination and the predicted position:

$${\varvec{Z}}(n|n - 1) = {\varvec{BX}}(n|n - 1)$$

(44)

$${\varvec{a}}_{i} (n) = \user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n) - {\varvec{Z}}(n|n - 1)$$

(45)

LOS identification: If the NLOS error in position estimate of each combination has been mostly eliminated after the distance measurements reconstruction, the condition of the position estimate can be considered in LOS. Furthermore, there are:

$${\varvec{a}}_{i} (n) \sim N(0,\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{S} }_{i} (n)),\quad i = 1, \ldots ,K$$

(46)

where, $\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{S} }_{i} (n)$ is the innovation covariance matrix of $\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n)$.

$$\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{S} }_{i} (n) = {\varvec{BP}}(n|n - 1){\varvec{B}}^{T} + \sigma_{G}^{2} (\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{H} }_{i}^{T} (n)\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{H} }_{i} (n))^{ - 1}$$

(47)

$$\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{H} }_{i} (n) = \frac{{\partial {\varvec{h}}(\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n))}}{{\partial \user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n)}}$$

(48)

K hypotheses and alternatives are defined to validate (46). The hypothesis $\zeta_{0,i}$ holds true when the i-th position estimate is determined in LOS, otherwise the hypothesis $\zeta_{1,i}$ holds true [15].

$$\zeta_{0,i} {:}{\varvec{a}}_{i} (n) \sim N\left( {0,\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{S} }_{i} (n)} \right),\quad i = 1, \ldots ,K$$

(49)

$$\zeta_{1,i} {:}\;{\text{not}}\;\zeta_{0,i} ,\quad i = 1, \ldots ,K$$

(50)

$${\varvec{T}}_{i} (n) = {\varvec{a}}_{i}^{T} (n)\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{S} }_{i}^{ - 1} (n){\varvec{a}}_{i} (n)$$

(51)

where, ${\varvec{T}}_{i} (n)$ is the test statistic, which is used to compare with the threshold value $\gamma$ of the validation gate to judge whether the position estimate $\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n)$ is in the validation region. If ${\varvec{T}}_{i} (n)$ isn’t larger than $\gamma$, the hypothesis $\zeta_{0,i}$ holds; otherwise $\zeta_{0,i}$ is rejected. The threshold value $\gamma$ is related to tracking threshold probability $P_{g}$, which represents the probability that the position estimate that falls into the verification area can be correctly detected, and it can be obtained from the following formula:

$$\int\limits_{0}^{\gamma } {f_{{\chi^{2} (2)}} (x)dx} = P_{g} = 1 - P_{Fa}$$

(52)

where, $f_{{\chi^{2} (2)}} ( \cdot )$ is the probability density function of chi-square distribution, which has two degrees of freedom. $P_{Fa}$ is the false alarm rate, while the sum of $P_{g}$ and $P_{Fa}$ is 1.

We record the position estimates that passed the hypothesis test and the number of them, which are denoted as $\overline{\user2{Z}}_{j} (n)$ and $k(n)$ respectively. $\overline{\user2{Z}}_{j} (n)$ is expressed as follows:

$$\overline{{\mathbf{Z}}}_{j} (n) = \left[ {\overline{x}_{j} (n),\overline{y}_{j} (n)} \right]^{T} ,\quad j = 1, \ldots ,k(n)$$

(53)

where, $k(n)$ is a non-negative integer no greater than K.

4.5 Residual weighting method

When $k(n)$ is more than zero, the position estimates fall into the validation region in the previous procedure are selected and used for residual weighting in this procedure. While the other position estimates are discarded. It may also occur that $k(n)$ is equal to zero, which indicates that all position estimates do not meet the hypothesis and are not identified in LOS, then we use the original position estimates $\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n)$.

Residual weighting method is performed to prevent reconstruction failure. Assuming that the previous steps did not introduce large errors, the predicted point is relatively accurate and is near the real position of mobile node. Therefore, we take the predicted position ${\varvec{X}}(n|n - 1)$ as the center, and the definition of residual [4] is modified as follows:

When $k(n) = 0$:

$$res_{i} (n) = \left\| {BX(n|n - 1) - \left[ {\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{x}_{i} (n),\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{y}_{i} (n)} \right]} \right\|_{2} ,\quad i = 1, \ldots ,K$$

(54)

When $k(n) > 0$:

$$res_{j} (n) = \left\| {BX(n|n - 1) - \left[ {\overline{x}_{j} (n),\overline{y}_{j} (n)} \right]} \right\|_{2} ,\quad j = 1, \ldots ,k(n)$$

(55)

The position estimate which is close to the predicted position is given a larger weight by taking the reciprocal of the corresponding residual. After calculating the residual of each position estimate, the residual weighted results of them are combined as the positioning result of this time step n:

When $k(n) = 0$:

$${\varvec{W}}(n) = \frac{{\sum\limits_{i = 1}^{K} {\user2{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{Z} }_{i} (n)(res_{i} (n))^{ - 1} } }}{{\sum\limits_{i = 1}^{K} {(res_{i} (n))^{ - 1} } }}$$

(56)

When $k(n) > 0$:

$${\varvec{W}}(n) = \frac{{\sum\nolimits_{j = 1}^{k(n)} {\overline{Z}_{j} (n)(res_{j} (n))^{ - 1} } }}{{\sum\nolimits_{j = 1}^{k(n)} {(res_{j} (n))^{ - 1} } }}$$

(57)

4.6 Results merging

In this subsection, the positioning results of the above two methods are merged by weighting. Due to the distance measurements contaminated by noise, the filtered distance measurements are closer to the actual distances. We compute the distances from the BNs to the positioning results, which are expressed as ${\varvec{h}}({\varvec{Z}}(n))$ and ${\varvec{h}}({\varvec{W}}(n))$. The greater the differences between them and the distance measurements ${\varvec{D}}(n)$ are, the more effective of this method can be considered. Therefore, we utilize the ideas in [4], and revise the definitions:

$$g_{1} (n) = {\varvec{D}}(n) - h({\varvec{Z}}(n))$$

(58)

$$g_{2} (n) = {\varvec{D}}(n) - h({\varvec{W}}(n))$$

(59)

Then the weighted sum of the two results at time step n can be obtained after computing the weighting coefficient $\beta_{i} (n)$, which is defined as ${\varvec{O}}(n)$:

$$\beta_{i} (n) = \frac{{g_{i} (n)}}{{\sum\nolimits_{i = 1}^{2} {g_{i} (n)} }}$$

(60)

$${\varvec{O}}(n) = \beta_{1} (n){\varvec{Z}}(n) + \beta_{2} (n){\varvec{W}}(n)$$

(61)

At the same time, we substitute ${\varvec{O}}(n)$ into the first and second components of the state vector ${\varvec{X}}(n|n)$:

$$\left[ {X(n|n)(1),X(n|n)(2)} \right] = O(n)$$

(62)

5 Simulation and experimental results

5.1 Simulation scene and parameter settings

In this subsection, we mainly introduce the simulation scene and various parameters. The simulation was carried out on the MATLAB platform. And the MN had a fixed trajectory in a $80 \times 80\;{\text{m}}$ simulation area, where six beacon nodes were randomly distributed in Fig. 2. We sampled 80 times at a sampling interval of 0.5 s. The initial state of the MN was $[1m{ 78}m{\text{ 1m/s 2}}{\text{.5m/s}}]^{T}$ and the initial covariance matrix of MN was ${\varvec{P}}(0) = {\varvec{I}}_{4}$. ${\varvec{u}}(n)$ was the Gaussian white noise $N(0,1^{2} )$, and ${\varvec{R}}(0) = \sigma_{l}^{2} {\varvec{I}}_{6}$ was the covariance matrix of measurement noise. In this paper, the threshold probability was $P_{g} = 0.99$, so the false alarm rate was $P_{Fa} = 0.01$. 1000 Monte Carlo simulations were conducted in each environment.

Fig. 2

A Monte Carlo simulation scene

We use root mean square error (RMSE) and cumulative distribution function (CDF) of the localization error to characterize the localization performance of these algorithms, and RMSE is calculated as follows:

$$RMSE = \sqrt {\frac{1}{MC}\frac{1}{N - 1}\sum\limits_{k = 1}^{MC} {\sum\limits_{n = 2}^{N} {\left\| {O_{k} (n) - A(n)} \right\|^{2} } } }$$

(63)

where, ${\varvec{A}}(n)$ is the actual location, and ${\varvec{O}}_{k} (n)$ is the estimated position at time step n in the k-th Monte Carlo simulation, which is obtained from (61). $MC = 1000$ is the total Monte Carlo runs times, and $N = 80$ denotes the number of whole samples in a Monte Carlo run.

To illustrate the general applicability of the proposed algorithm, we complicate the motion trajectory and set it as a cubic curve $y = - 0.00063x^{3} + 40.63, \, x \in [1,80][1,80]$, where the inflection point is ${\varvec{A}}(n)(1) = 40$. Figure 2 shows the present route of the MN and the random distribution of six BNs in a Monte Carlo simulation.

The NLOS error may follow different distributions according to the environment, and the distribution of NLOS error may be related to the type and distribution of indoor obstacles. We did simulations when the NLOS error follow Gaussian, uniform and exponential distribution respectively. We mainly compared our algorithm with the conventional positioning algorithm: EKF [23], REKF [24], IMM-EKF [25] and probabilistic data association IMM (PIMM) [26]. The specific positioning performances of these algorithms in simulation are shown in the following section.

5.2 Simulation results and discussion

5.2.1 Gaussian distribution

On the condition that NLOS error and sensor noise followed Gaussian distribution $N(\mu_{NLOS} ,\sigma_{NLOS}^{2} )$ and $N\left( {0,\sigma_{LOS}^{2} } \right)$ respectively, we set the default parameters as shown in the Table 1. The NLOS error probability is defined as the proportion of NLOS error to the noise composed of LOS error and NLOS error.

Table 1 The parameters in Gaussian distribution

Figure 3 demonstrates the change of RMSE under different probabilities of the NLOS error $P_{NLOS}$ which varied from 0.1 to 1. The RMSE curve of the EKF, REKF and IMM-EKF had the same trend, and the RMSE value of them from largest to smallest were the EKF, REKF and IMM-EKF. The PIMM had the largest RMSE value and poorest positioning performance when $P_{NLOS}$ was above 0.6. The proposed algorithm had the lowest average value of RMSE, which was 2.1047 m and at least 39.90% lower than that of the other four algorithms. Figure 4 displays the change in positioning performance when $\mu_{NLOS}$ varied from 2 to 11 m. The RMSE of the EKF, REKF and IMM-EKF had the same uptrend and they were nearly close, but their RMSE value were not small. No matter how many $\mu_{NLOS}$ was, the proposed algorithm always had the smallest RMSE. The average values of the RMSE were the EKF, the REKF, the IMM-EKF, the PIMM and the proposed algorithm in descending order, and the specific values of them were 4.3656 m, 4.2116 m, 4.1703 m, 4.1438 m and 2.5052 m respectively. Figure 5 displays the change in positioning performance when $\sigma_{NLOS}$ varied from 2 to 11 m. The RMSE of the EKF, REKF and IMM-EKF had the same uptrend, but their RMSE value were not smaller than the other two algorithms. The proposed algorithm had the smallest RMSE among these algorithms. The average values of the RMSE were the EKF, the REKF, the IMM-EKF, the PIMM and the proposed algorithm in descending order, and the specific values of them were 4.9315 m, 4.7162 m, 4.7013 m, 4.5468 m and 2.9892 m respectively. As shown in Fig. 6, the RMSE curve of the proposed algorithm is steeper than other algorithms, which is far from the other curves. The 50% localization errors of the proposed algorithm was about 1.771 m, which was at least 11.5% lower than that of the other four algorithms. And the 90% localization errors of the proposed algorithm were about 3.383 m, which was at least 31.81% lower than that of the other algorithms.

Fig. 3

The RMSE comparison of probability $P_{NLOS}$ under Gaussian distribution

Fig. 4

The RMSE comparison of mean value $\mu_{NLOS}$ under Gaussian distribution

Fig. 5

The RMSE comparison of standard deviation $\sigma_{NLOS}$ under Gaussian distribution

Fig. 6

The RMSE comparison of standard deviation $\sigma_{NLOS}$ under Gaussian distribution

With the increase of the probability, mean or standard deviation of the NLOS error, the true value of NLOS error at a certain time will increase, and the interference to the measured value will intensify. The localization errors of the position estimates processed by the location algorithm will be greater. Therefore, in Figs. 3, 4 and 5, the RMSE of each algorithm has an upward trend. Since the proposed algorithm adopted mean reconstruction method, the greater the probability of NLOS error was, the more times NLOS situation occurred, the more the mean value can reflect the actual situation. The proposed algorithm in Fig. 3 had small RMSE and good positioning performance in the case of high probability of NLOS error. When the probability of the NLOS error $P_{NLOS}$ was small, the NLOS interference was little, so the RMSE was also small. The PIMM algorithm associated multiple position estimates with Kalman predicted position. These position estimates directly affected the results, and they were generated by LS. The effectiveness of the PIMM algorithm largely depends on the number of excellent measurements, so in the case of high NLOS probability, the PIMM performance was not outstanding. The CDF reflects the distribution of overall localization errors, which can be seen from Fig. 6. Most of the localization errors of the proposed algorithm were less than 3 m, and only few outliers appeared in the positioning.

5.2.2 Uniform distribution

On the condition that NLOS error followed uniform distribution $U(0,\max )$, we set the default parameters as shown in the Table 2.

Table 2 The parameters in uniform distribution

We studied the positioning effect of different localization algorithms when the NLOS error probability was increasing. Figure 7 is similar to in Fig. 4, in which the RMSE of the proposed algorithm changed slightly. Especially when $P_{NLOS}$ was above 0.3, its value was stable at around 2 m. The RMSE of the EKF, REKF and IMM-EKF were rising linearly with $P_{NLOS}$. The proposed algorithm had the lowest average value of RMSE, which was 1.9576 m and at least 40.44% lower than that of the other four algorithms. Then, we let the parameter max change from 6 to 15 m. Figure 8 shows that the RMSE were positively correlated to the parameter max. The EKF, REKF and IMM-EKF had the largest RMSE and the largest growth rate. The average values of the RMSE were the EKF, the REKF, the IMM-EKF, the PIMM and the proposed algorithm in descending order, and the specific values of them were 3.5689 m, 3.4480 m, 3.4335 m, 3.3649 m and 2.9892 m respectively. The influence of the parameter L on the positioning effect of different algorithms was shown in Fig. 9. No matter how many BNs there were, our algorithm always worked best, improving accuracy by more than 2 m over other algorithms. The proposed algorithm had the lowest average value of RMSE, which was 2.0197 m and at least 37.36% lower than that of the four other algorithms.

Fig. 7

The RMSE comparison of the probability $P_{NLOS}$ under uniform distribution

Fig. 8

The RMSE comparison of parameter max under uniform distribution

Fig. 9

The RMSE comparison of the number of BNs under uniform distribution

In the case of uniform distribution, the larger the upper limit of NLOS error was, the greater the probability of NLOS error appeared with a larger value, so the interference to positioning became stronger. As shown in the Fig. 8 the RMSE of each algorithm increased with parameter max. The proposed algorithm determined the position estimates and discarded the position estimates that did not fall into the verification gate. Therefore, the proposed algorithm was not susceptible to NLOS errors and had good robustness with smallest RMSE curve growth rate. The more the number of beacon nodes were, the more information about the mobile node can be measured, and the more equations in the maximum likelihood location method can be established, so the the position estimates obtained by LS will be more accurate. As shown in the Fig. 9, the more BNs were, the better these algorithms performed.

5.2.3 Exponential distribution

On the condition that NLOS error followed exponential distribution $E(1/\lambda )$, we set the default parameters as shown in the Table 3.

Table 3 The parameters in exponential distribution

Figure 10 demonstrates the RMSE values of each algorithm under different probabilities $P_{NLOS}$. When $P_{NLOS}$ was 0.3, the performance of the proposed algorithm was better than the EKF, the REKF and the IMM-EKF, but it was not as good as the PIMM. When the probability P_NLOS was above 0.3, the proposed algorithm performed well in localization, and the PIMM was sub-optimal. The proposed algorithm had the lowest average value of RMSE, which was 2.2132 m and at least 28.46% lower than that of the other four algorithms. Figure 11 demonstrates the RMSE values of each algorithm under different parameters $\lambda$. The average values of the RMSE were the EKF, the IMM-EKF, the REKF, the PIMM and the proposed algorithm in descending order, and the specific values of them were 3.1050 m. 2.9599 m, 2.9554 m, 2.7823 m and 2.1185 m respectively. The CDF of localization error of different algorithms are shown in Fig. 12. The 90% localization errors of these algorithms were the EKF, the IMM-EKF, the REKF, the PIMM and the proposed algorithm in descending order, and the specific values of them were 4.683 m, 4.579 m 4.463 m, 3.611 m and 3.374 m respectively.

Fig. 10

The RMSE under the probability $P_{NLOS}$ under exponential distribution

Fig. 11

The RMSE comparison of parameter $\lambda$ under exponential distribution

Fig. 12

The CDF of localization error under exponential distribution

In the case of uniform distribution, with the increase of parameter $\lambda$, the maximum value of NLOS error increases exponentially. Compared with Gaussian and uniform distribution of Figs. 4, 5 and 8, as shown in the Fig. 11, the growth rate of RMSE curve is larger and the curve is steeper. Although there are three different NLOS error distributions, the proposed algorithm showed good positioning performance and robustness.

5.3 Experimental site and equipment

The real environment is often complex and changeable, so on the basis of simulation, we conducted two experiments in real indoor environments. Ultra-wide band (UWB) technique is popular for researchers to use in indoor positioning, due to its high accuracy and low energy consumption [27]. In the experiment, the UWB nodes were used as radio frequency (RF) devices and sensor nodes in WSN, and Fig. 13 displays a UWB node. The MN was connected with the power supply and it was strapped to the walker's shoes; to maintain the same height with MN, the BNs were placed on the ground. At the same time, the BNs transmitted the distance measurements to the laptop through the universal serial bus (USB) cable and the laptop displayed the distance measurements on the screen.

Fig. 13

The physical image of UWB node

As shown in Figs. 14 and 15, the experimental site was a university classroom with a large number of tables, seats and students, which can be considered as obstacles. Therefore, the measured data obtained from the UWB nodes were easily contaminated by NLOS errors, containing large errors. We randomly placed six UWB nodes and moved at a constant speed of 1.2 m/s, and sampled 30 times in total. We took the corner of the lower left corner as the origin; took the long side of the classroom as the x-axis, and the short side as the y-axis, to establish a two-dimensional coordinate system, and the coordinate of the initial position was $[0.6m \, 1.2m]^{T}$. The motion trajectory was roughly a rectangle. We first moved in the vertical direction, so the horizontal speed was $0m/s$. The entire positioning process was sampled 30 times in total, during the sampling time $(\Delta t = 1s)$, each beacon node collected more than 20 distance measurements. After sampling, the average value of them was obtained as the input of each localization algorithm.

Fig. 14

The actual scene of the university classroom

Fig. 15

The schematic diagram of the university classroom

Figures 16 and 17 show the actual scenario and schematic diagram of the research group office. The difference was that the scope of this scene was smaller, so there were only four BNs, and the track distance was relatively short, so only 15 times were sampled. As shown in Fig. 17, we set an origin at the bottom right of the room, extending the y-axis in the left direction of the plan view and the x-axis in the upper direction of the plan view to establish a coordinate system, and the coordinate of the initial position was $[7.2\;{\text{m }}1.8\;{\text{m}}]^{T}$ (Figs. 18, 19, 20, and 21).

Fig. 16

The actual scene of the research group office

Fig. 17

The schematic diagram of the research group office

Fig. 18

The data before and after the algorithms processing in the university classroom

Fig. 19

The trajectories in the university classroom

Fig. 20

The CDF versus the localization error in the university classroom

Fig. 21

The data before and after the algorithms processing in the research group office

5.4 Experimental results and discussion

To illustrate the filtering effect of the algorithms, the data before and after the algorithm processing were shown in Figs. 18 and 21. Because the proposed algorithm subtracted a value during reconstruction, the processed results may be smaller than the actual distances, but they were close to the actual distances, so the position estimates obtained were still more accurate than that in other algorithms. The positioned trajectories in the university classroom and the research group office are shown in Figs. 19 and 22, and the track of the proposed algorithm was close to the real track. In addition, the CDF of the localization errors of the proposed algorithm were smaller than other algorithms in Figs. 20 and 23. Because the interference of NLOS errors caused by lots of obstacles in the experimental site were large, the localization performance of the EKF, REKF, IMM-EKF and PIMM were relatively poor. Since the classroom was not as large as the simulation scene, and the noise here was not as complicated as expected, so the final positioning results were slightly different from the simulation.

Fig. 22

The trajectories in the research group office

Fig. 23

The CDF versus the localization error in the research group office

The algorithm and data processing were conducted in MATLAB2018b platform of the laptop whose computer system is a 64-bit operating system and the processor is Intel (R) Core (TM) i5-7300HQ CPU @ 2.50 GHz. Tables 4 and 5 show the running time that each algorithm estimated the position at a sampling time in the university classroom and the research group office. In order to ensure the high precision of positioning, it is inevitable to increase the amount of calculation. The sampling frequency of this experiment was set to 1 Hz. Although the proposed algorithm consumed more time, the processing time for each sampling point was still less than one second, which equalled to the interval between single sample. Therefore, the algorithm can achieve real-time tracking and positioning.

Table 4 Running time of each sampling point in the university classroom

Table 5 Running time of each sampling point in the research group office

6 Conclusion

In this paper, we proposed a mean reconstruction method to improve the REKF, which can detect and mitigate the NLOS errors to enhance the positioning accuracy. The specific operation of the algorithm is to use the Kalman prediction position to calculate the differences from the distance measurements, which are nearly equivalent to the noise. After averaging the data to estimate the mean value of the noise, the distance measurements are reconstructed by subtracting the estimation of the noise average. At the same time, the results of the residual weighting can ensure the effectiveness of the reconstruction method. The algorithm can significantly reduce the NLOS errors, especially in the environment with severe NLOS errors. The simulation and experimental results both demonstrated that the proposed algorithm had higher localization accuracy with strong robustness, which had a prominent advantage over traditional localization algorithms. While, there are few shortcomings in the proposed algorithm.

• 1. The mean reconstruction method is required to be used in a small range of area where obstacles are similar, and the NLOS errors follow the same distribution.

• 2. The reconstruction may bring negative errors, which make the reconstructed distances smaller than the actual distances, and the position estimates appear on both sides of the actual trajectory.

• 3. The proposed algorithm is more computationally intensive. First, in the RWGH step, the number of combinations selected is larger than other algorithms; second, the Newton–Raphson iteration will also consume a certain amount of computation.