A Double Extended Kalman Filter Algorithm for Weakening Non-Line-of-Sight Errors in Complex Indoor Environments Based on Ultra-Wideband Technology

Abstract

In complex indoor environments, target tracking performance is impacted by non-line-of sight (NLOS) noises and other measurement errors. In order to fix NLOS errors, a double extended Kalman filter (DEKF) algorithm is proposed, which refers to a kind of cascaded structure composed of two Kalman filters. In the proposed algorithm, the first filter is a classic Kalman filter (KF) and the second is an extended Kalman filter (EKF). Time of arrival (TOA) measurements collected by multiple stationary ultra-wideband (UWB) sensors are used. The residual errors between the measured TOA and that of the first KF are predicted, and the covariance of the first KF is adjusted correspondingly. Then, we use the estimated distance state of the first KF as a measurement vector for the second EKF in order to obtain a smoother observation. One of the advantages of the proposed algorithm is that it is able to perform target tracking with good accuracy even without or with only one LOS TOA measurement for a period of time without prior information about the NLOS noise, which may be difficult to obtain in practical applications. Another advantage is that the accuracy does not greatly decrease when NLOS noises exist for a long period of time. Finally, the proposed DEKF can maintain the high-precision positioning characteristics in both the constant velocity (CV) model and the constant acceleration (CA) model in the LOS/NLOS environment. Our simulation and experimental results show that the proposed algorithm performs much better than other algorithms in SOTA, particularly in severe mixed LOS/NLOS environments.

1. Introduction

With the development of wireless communication technology, the demand for location-based services (LBSs) is rapidly increasing [1,2,3,4,5]. Currently, there are several technologies for indoor positioning, such as Bluetooth Low Energy (BLE) [6,7,8], WiFi [9,10,11], Radio Frequency Identification (RFID) [12,13,14], and Impulse Radio–Ultra Wideband (IR-UWB) [15,16,17,18]; IR-UWB is superior to the others with its centimeter positioning accuracy in complex indoor environments.

The UWB signals are impacted by multipath effects, which may lead to great measurement errors, especially in NLOS environments. The algorithm proposed in [19] keeps the LOS pseudo-measurement results, discarding the NLOS ones, and calculates the average of all the selected LOS measurements. The limitation of this tracking algorithm is that, when there is no LOS BaseStation within a period of time, the estimation of states can only rely on predictions. The Kalman filter and its enhancements, such as EKF, cubature Kalman filter (CKF), unscented Kalman filter (UKF), adaptive Kalman filter (AKF), or particle filter (PF), are used [20,21,22,23,24,25,26,27] to reduce NLOS noise-introduced errors in complex indoor environments [28,29]. To reduce measurement errors, forms of extended KF (EKF) with robust estimation, such as M-estimation and fuzzy-tuning M-estimation EKF, were proposed in [30]. However, the above filtering technologies are still limited by their accuracy, especially in complex LOS and NLOS mixture conditions. A time-of-flight (ToF)-based NLOS indoor tracking method incorporating adaptive ranging error mitigation techniques is presented in [31]. However, due to its high computational complexity, it is difficult to appropriately apply this method to scenarios with high real-time requirements. The classification of LOS and NLOS conditions using deep learning (DL) models was mentioned in [32]. However, the study does not discuss the performance of the model in complex indoor environments in detail. Deep-learning-based NLOS/LOS classification methods for indoor positioning systems were also mentioned in [33]. However, the training of deep learning models requires a large amount of data support, and it may be a challenge to obtain enough labeled data in some indoor environments.

In order to further improve target tracking performance in complex LOS and NLOS mixture conditions, a double Kalman filter with a residual classification and covariance adjustment (RCCA) algorithm is proposed to mitigate NLOS-introduced errors. In this algorithm, two Kalman filters are cascaded, where the first filter is a KF and the second is an EKF. The RCCA process is applied on the KF system, whose states are distances and velocities. Residual classification means that the residuals between the first KF’s observed measurements and its system predictions are calculated, and then, the covariance of the first KF system is adjusted for the Kalman gain according to the error ranges. After that, the distance vectors are used as measurements of the second EKF, and the estimated variance of the first KF is input as the measurement covariance. This completes the RCCA process.

The remainder of this article is organized as follows. In Section 2, we introduce the system framework of this methodology. In Section 3, we discuss the residual classification and variance adjustment process. Section 4 presents the details of the DEKF algorithm. In Section 5, the simulation results are shown and experimental verification is carried out. Finally, Section 6 presents the conclusions.

2. System Model

2.1. Extended Kalman Filter Modeling

A system containing one target and M stationary UWB anchors is considered in this section. For generality, the constant acceleration (CA) model is used, so the state of target can be defined as follows:

$${\mathbf{X}}_k={[x_k,y_k,z_k,{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k,{\ddot{x}}_k,{\ddot{y}}_k,{\ddot{z}}_k]}^T,$$

(1)

where ${[x_k,y_k,z_k]}^T$ is the coordinate of the target, ${[{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k]}^T$ is the velocity of the target, ${[{\ddot{x}}_k,{\ddot{y}}_k,{\ddot{z}}_k]}^T$ is the acceleration of the target, and k stands for the present moment. The stationary UWB anchors are located at ${\mathbf{X}}_i^U={[x_i^U,y_i^U,z_i^U]}^T,i=1,\dots ,M$. In order to simplify the analysis, a constant velocity (CV) model is used for formula derivation, which is essentially equivalent to the CA model with acceleration equal to zero. Thus, the state of the target is defined as follows:

$${\mathbf{X}}_k={[x_k,y_k,z_k,{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k]}^T,$$

(2)

The measured distances between the target and the anchors are calculated using the received time of flight (TOF), which is expressed as ${\mathbf{Z}}_k=\mathrm{c}·{\mathrm{TOF}}_k$, $\mathrm{c}=3\times {10}^8$ (m/s). The state evolution model of the EKF system is as follows:

$${\mathbf{X}}_k=\mathbf{A}{\mathbf{X}}_{k-1}+{\mathbf{w}}_k,$$

(3)

$${\mathbf{Z}}_k=\mathbf{h}\left({\mathbf{X}}_k\right)+{\mathbf{v}}_k,$$

(4)

where $\mathbf{A}$ is the transition matrix of the states in the CV model:

$$\mathbf{A}=\left[\begin{array}{cccccc}1& 0& 0& T_s& 0& 0\\ 0& 1& 0& 0& T_s& 0\\ 0& 0& 1& 0& 0& T_s\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],$$

(5)

where $T_s$ is the observation time of the system and $\mathbf{h}\left({\mathbf{X}}_k\right)={[h_1\left({\mathbf{X}}_k\right),\dots ,h_M\left({\mathbf{X}}_k\right)]}^T$ is a non-linear transfer function of state and measurement, where $h_i\left({\mathbf{X}}_k\right)$ = $\sqrt{{(x_k-x_i^U)}^2+{(y_k-y_i^U)}^2+{(z_k-z_i^U)}^2}$. The essence of EKF is to transform the non-linear case into a locally linear case. For non-linear functions, the properties of derivatives can be exploited for local linearization by a Jacobian matrix. The Jacobian can be obtained by taking the partial derivative with respect to each dimension of $h_i\left({\mathbf{X}}_k\right)$. It is shown below:

$${\mathbf{H}}_k=\left[\begin{array}{cccccc}{\displaystyle \frac{x_k-x_1^U}{h_1\left({\mathbf{X}}_k\right)}}& {\displaystyle \frac{y_k-y_1^U}{h_1\left({\mathbf{X}}_k\right)}}& {\displaystyle \frac{z_k-z_1^U}{h_1\left({\mathbf{X}}_k\right)}}& 0& 0& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ {\displaystyle \frac{x_k-x_M^U}{h_M\left({\mathbf{X}}_k\right)}}& {\displaystyle \frac{y_k-y_M^U}{h_M\left({\mathbf{X}}_k\right)}}& {\displaystyle \frac{z_k-z_M^U}{h_M\left({\mathbf{X}}_k\right)}}& 0& 0& 0\end{array}\right]$$

(6)

The non-linear function $h_i\left({\mathbf{X}}_k\right)$ does not contain ${[{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k]}^T$, and thus, the right-hand side of the matrix is an M × 3 zero matrix.

In (3), ${\mathbf{w}}_k$ is a process noise vector with a covariance matrix ${\mathbf{Q}}_k^x$. In (4), ${\mathbf{v}}_k$ is a measurement noise vector with a covariance matrix ${\mathbf{R}}_k^x$, which is determined by the measurement accuracy of the anchors. As for the value of ${\mathbf{Q}}^x$, which is mentioned in [34], it takes different values in different models. In the CV model, it is defined as follows according to [34]:

$${\mathbf{Q}}^x={\mathbf{\Phi }}_{s1}\left[\begin{array}{cc}{\displaystyle \frac{T_s^3}{3}}{\mathbf{I}}_3& {\displaystyle \frac{T_s^2}{2}}{\mathbf{I}}_3\\ {\displaystyle \frac{T_s^2}{2}}{\mathbf{I}}_3& T_s{\mathbf{I}}_3\end{array}\right].$$

(7)

where ${\mathbf{\Phi }}_{s1}$ is the spectrum density of the process noise for the KF system and ${\mathbf{I}}_3$ is a 3 × 3 unit matrix.

The iterative computation process for the EKF system is as follows [20,35,36]:

$$\begin{array}{c}{\widehat{\mathbf{X}}}_{k|k-1}=\mathbf{A}{\widehat{\mathbf{X}}}_{k-1},\end{array}$$

(8)

$$\begin{array}{c}{\widehat{\mathbf{P}}}_{k|k-1}^x=\mathbf{A}{\widehat{\mathbf{P}}}_{k-1}^x{\mathbf{A}}^T+{\mathbf{Q}}_k^x,\end{array}$$

(9)

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

(10)

$$\begin{array}{c}{\widehat{\mathbf{X}}}_k={\widehat{\mathbf{X}}}_{k|k-1}+{\mathbf{K}}_k^x({\mathbf{Z}}_k-\mathbf{h}\left({\widehat{\mathbf{X}}}_{k|k-1}\right)),\end{array}$$

(11)

$$\begin{array}{c}{\widehat{\mathbf{P}}}_k^x=(\mathbf{I}-{\mathbf{K}}_k^x{\mathbf{H}}_k){\widehat{\mathbf{P}}}_{k|k-1}^x.\end{array}$$

(12)

Equations (8) and (9) are update equations from the $k-1$ state to the k state, and (10), (11), and (12) are measurement update equations. ${\widehat{\mathbf{P}}}_{k-1}^x$ is the error covariance matrix of ${\widehat{\mathbf{X}}}_{k-1}$, $\mathbf{A}$ is the state transition matrix, ${\mathbf{Q}}_k^x$ is the process noise covariance matrix, ${\mathbf{R}}_k^x$ is the measurement noise covariance matrix, ${\mathbf{H}}_k$ is the measurement transition matrix, ${\mathbf{Z}}_k$ is the measurement matrix, and the weighting matrix ${\mathbf{K}}_k^x$ is generally referred to as the Kalman gain matrix.

2.2. Measurement Error Modeling

Generally, measurement errors are made of measurement noise and NLOS noise, where measurement noise is modeled by Gaussian white noise determined by measured anchors. NLOS noise is caused by reflected, refracted, or multipath transmission signals. The measurement error modeling is as follows:

$${\rho }_{k,i}={\dot{\rho }}_{k,i}+n_{k,i}+w_{k,i},i=1,\cdots ,M.$$

(13)

where ${\rho }_{k,i}$ is the measured distance between the target and anchor i, ${\dot{\rho }}_{k,i}$ is the real distance, $n_{k,i}$ is the measurement noise, and $w_{k,i}$ is the NLOS noise. The NLOS noise is usually modeled with a uniform distribution, mean-shifted Gaussian distribution, or exponential distribution [19,30,36]. In the modeling, we assume a uniformly distributed NLOS noise in the following constraints for simplicity, although there are other methods for describing the NLOS noise, such as the Markov process. For a mixed LOS/NLOS environment, the probability density function of ${\eta }_i$ is defined as:

$$p\left({\eta }_i\right)=(1-{\epsilon }_i)p_{\mathrm{LOS}}\left(n_i\right)+{\epsilon }_i{p}_{\mathrm{NLOS}}\left(w_i\right),$$

(14)

where ${\eta }_i=n_i+w_i$ is defined as in (13), ${\epsilon }_i$ is the proportion of NLOS errors at anchor i, $p_{\mathrm{LOS}}\left(n_i\right)$ is zero mean Gaussian distributed, and $p_{\mathrm{NLOS}}\left(w_i\right)$ is mean-shifted Gaussian distributed. The mixed LOS/NLOS model is implemented using a two-state Markov process, which is the same as that mentioned in [19,36,37,38]. The state transition is shown in Figure 1.

The transition probability matrix of the Markov process is:

$$\left[\begin{array}{cc}1-\alpha & \alpha \\ \beta & 1-\beta \end{array}\right],$$

(15)

where $\alpha$ is the probability of transition from state LOS to NLOS and $\beta$ is the probability of transition from state NLOS to LOS. ${\epsilon }_i$ is related to $\alpha$ and $\beta$ by:

$${\epsilon }_i={\displaystyle \frac{\alpha }{\alpha +\beta }}.$$

(16)

The proportion of LOS and NLOS noise can be adjusted by the values of $\alpha$ and $\beta$, and there are several sets of values that satisfy one ${\epsilon }_i$.

Table 1. Scaling factor of NLOS noise.

${\mathit{\epsilon }}_{\mathit{i}}$	$\mathit{\alpha }$	$\mathit{\beta }$
0.1	0.01	0.09
0.25	0.02	0.06
0.5	0.05	0.05
0.75	0.06	0.02

shows one possible combination.

3. Residual Classification and Covariance Adjustment

For the first Kalman filter of the two cascaded filters, the distances between the target and the anchors as well as their variation rates of measurement are state variables of KF:

$$\begin{array}{c}\hfill {\mathbf{Y}}_k={[{\rho }_{1,k},\cdots ,{\rho }_{M,k},{\dot{\rho }}_{1,k},\cdots ,{\dot{\rho }}_{M,k}]}^T.\end{array}$$

(17)

where ${\mathbf{Y}}_k$ is the KF state vector in the k-th iteration, ${\rho }_{i,k}$ is the measured distance between the i-th anchor and the target in the k-th iteration, and ${\dot{\rho }}_{i,k}$ is the rate of change in the measured distance.

After calculating the prediction of the states, a residual obtained between measurement and prediction is shown below:

$$\begin{array}{c}\hfill {\mathbf{d}}_k=|{\mathbf{Z}}_k-\mathbf{T}{\widehat{\mathbf{Y}}}_{k|k-1}|,\end{array}$$

(18)

where ${\mathbf{Z}}_k$ is a set of measurements collected by several stationary UWB anchors and $\mathbf{T}$ is a transition matrix between state and observation.

$$\mathbf{T}=\left[\begin{array}{cc}{\mathbf{I}}_M& {\mathbf{0}}_M\end{array}\right].$$

(19)

Then, the range of the obtained residual is classified as follows:

$${\mathbf{d}}_k\{\begin{array}{cc}0<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<{\mu }_1\hfill & \mathrm{(20a)}\\ {\mu }_1\le \mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<{\mu }_2\hfill & \mathrm{(20b)}\\ {\mu }_2\le \mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<{\mu }_3\hfill & \mathrm{(20c)}\\ ⋮\hfill & \mathrm{(20d)}\\ {\mu }_{N-3}\le \mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<{\mu }_{N-2}\hfill & \mathrm{(20e)}\\ {\mu }_{N-2}\le \mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<{\mu }_{N-1}\hfill & \mathrm{(20f)}\\ {\mu }_{N-1}\le \mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<{\mu }_N\hfill & \mathrm{(20g)}\end{array}$$

where ${\mu }_N>{\mu }_N-1>\cdots >{\mu }_3>{\mu }_2>{\mu }_1$ and N is the number of range groups. The classification criteria are based on the order of $\mu$’s magnitude, and the choice of the first parameter ${\mu }_1$ in (20a) is based on the LOS measured noise covariance, which is determined by the UWB ranging accuracy. When the LOS measured noise is Gaussian distributed with $(\mu ,\sigma )=(0\mathrm{m},0.1\mathrm{m})$, the cumulative distribution function (CDF) is defined as [39]:

$$\begin{array}{c}\hfill F\left(a\right)=P(\mathrm{X}\le a).\end{array}$$

(21)

When a = 0.375, F(a) = 0.9999. This value is already very close to 1. When a is greater than 0.7, the sensitivity to noise is reduced. Therefore, 0.5 is selected as the value of ${\mu }_1$ after many tests. The choice of this value can be adjusted accordingly. Other classifications are based on the fixed value region. Therefore, the region in (20) can be set:

$${\mathbf{d}}_k\{\begin{array}{c}0<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<0.5\hfill \\ 0.5<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<1\hfill \\ 1<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<10\hfill \\ 10<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<20\hfill \\ 20<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<30\hfill \\ 30<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<40\hfill \\ 40<\mathrm{m}\mathrm{a}\mathrm{x}\left({\mathbf{d}}_k\right)<50\hfill \\ \hfill ⋮\hfill \end{array}$$

(22)

After obtaining the residual range, the expectation of the squared residuals is defined as:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill {\mathbf{D}}_k& =E\left[{\mathbf{d}}_k{\mathbf{d}}_k^T\right]\hfill \\ \hfill & =\mathbf{T}\mathbf{G}{\widehat{\mathbf{P}}}_{k-1}^y{\mathbf{G}}^T{\mathbf{T}}^T+\mathbf{T}{\mathbf{Q}}_k^y{\mathbf{T}}^T+{\mathbf{R}}_k^y,\hfill \end{array}\end{array}$$

(23)

where $\mathbf{G}$ is a transition matrix of states defined as:

$$\mathbf{G}=\left[\begin{array}{cc}{\mathbf{I}}_M& T_s{\mathbf{I}}_M\\ {\mathbf{0}}_M& {\mathbf{I}}_M\end{array}\right],$$

(24)

According to the characteristic of expectations, when the random variable $\mathrm{X}$ is in a uniform distribution, the expectation of the squared variable is:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill E\left({\mathrm{X}}^2\right)& ={\int }_a^b{\displaystyle \frac{{\mathrm{X}}^2}{b-a}}\mathrm{dX}\hfill \\ \hfill & ={\displaystyle \frac{b^2+ab+b^2}{3}}.\hfill \end{array}\end{array}$$

(25)

where a and b are the lower and upper bound of $\mathrm{X}$, respectively.

Therefore, when the residual ${\mathbf{d}}_k$ is in a uniform distribution, the expectation of the squared residual ${\mathbf{D}}_k=diag\left(\left[{\displaystyle \frac{b^2+ab+a^2}{3}}\right]\right)$. In (20), the range of ${\mathbf{d}}_k$ has been already classified, and thus, the corresponding ${\mathbf{D}}_k$ is calculated by (25). Then, according to (23), the following equation is obtained:

$$\begin{array}{c}\hfill \mathbf{T}{\mathbf{Q}}_k^y{\mathbf{T}}^T+{\mathbf{R}}_k^y={\mathbf{D}}_k-\mathbf{T}\mathbf{G}{\widehat{\mathbf{P}}}_{k-1}^y{\mathbf{G}}^T{\mathbf{T}}^T,\end{array}$$

(26)

where $\mathbf{T}\mathbf{G}{\widehat{\mathbf{P}}}_{k-1}^y{\mathbf{G}}^T{\mathbf{T}}^T$ can be calculated through the last-time estimation. ${\mathbf{Q}}_k^y$ is generally not adjusted greatly, and the adjustment range is $0\sim 1$. The parameter $\lambda$ is defined as the adjustment factor of ${\mathbf{Q}}_k^y$, which is defined as:

$$\begin{array}{c}\hfill {\lambda }_{k,j}={\displaystyle \frac{N-j}{N}}.\end{array}$$

(27)

where N is the number of range groups and j is the group to which it belongs.

When the residual ${\mathbf{d}}_k$ is large, which means that the observed value contains a large noise component, the final estimate is biased toward the predicted value, so ${\mathbf{Q}}_k^y$ needs to be reduced. Similarly, when the residual ${\mathbf{d}}_k$ is small, the final estimate is biased towards the observed value, and ${\mathbf{Q}}_k^y$ needs to be increased. The adjustment for ${\mathbf{Q}}_k^y$ is shown below:

$$\begin{array}{cc}\hfill & {\mathbf{Q}}_k^y={\lambda }_{k,j}·{\mathbf{Q}}_0^y({\lambda }_{k,j}={\displaystyle \frac{N-j}{N}}).\hfill \end{array}$$

(28)

After the adjustment of ${\mathbf{Q}}_k^y$ in (28), ${\mathbf{R}}_k^y$ can be adjusted according to (26). Thus, residual classification and covariance adjustment are accomplished. One of the advantages of RCCA is that it does not require the detection and discrimination of LOS/NLOS measurements. The other advantage is that NLOS measurements can be retained, which is an indicator of the presence of NLOS interference for a long time. The flowchart of RCCA is shown in Figure 2.

4. Double Extended Kalman Filter Based on RCCA

In this section, residual classification and covariance adjustment (RCCA) is applied to the double extended Kalman filter, which is constructed by concatenating a KF and an EKF.

The main principle of RCCA is that the predicted value is obtained via a conventional filtering process through KF, and then, the residual between the predicted value and value (from the anchors) is obtained. The interval is classified according to the size of the residual, and the corresponding covariance matrix and observation noise covariance matrix are adjusted. Then, the results filtered by the KF are used as the input to the second EKF, which is used as the observation value and observation noise matrix of the EKF. The first-stage KF adjusts the covariance matrix through residual classification, to obtain more accurate observations for the second-stage EKF for further processing. The architecture of the overall system is shown in Figure 3.

First, for the pre-stage KF, the iteration calculation process of the KF is shown below:

1. Prediction:

$$\begin{array}{c}{\widehat{\mathbf{Y}}}_{k|k-1}=\mathbf{G}{\widehat{\mathrm{Y}}}_{k-1},\end{array}$$

(29)

$$\begin{array}{c}{\mathbf{d}}_k=|{\mathbf{Z}}_k-\mathbf{T}{\widehat{\mathbf{Y}}}_{k|k-1}|,\end{array}$$

(30)

$$\begin{array}{c}{\mathbf{d}}_k\mathrm{classification},\end{array}$$

(31)

$$\begin{array}{c}{\mathbf{Q}}_k^y={\lambda }_{k,j}·{\mathbf{Q}}_0^y({\lambda }_{k,j}={\displaystyle \frac{N-j}{N}}),\end{array}$$

(32)

$$\begin{array}{c}{\mathbf{P}}_k^y=\mathbf{G}{\mathbf{P}}_{k-1}^y{\mathbf{G}}^T+{\mathbf{Q}}_k^y,\end{array}$$

(33)

2. RCCA:

$$\begin{array}{c}{\mathbf{R}}_k^y={\mathbf{D}}_k-\mathbf{T}\mathbf{G}{\widehat{\mathbf{P}}}_{k-1}^y{\mathbf{G}}^T{\mathbf{T}}^T-\mathbf{T}{\mathbf{Q}}_k^y{\mathbf{T}}^T,\end{array}$$

(34)

3. Estimation:

$$\begin{array}{c}{\mathbf{K}}_k^y={\widehat{\mathbf{P}}}_{k|k-1}^y{\mathbf{T}}^T{(\mathbf{T}{\widehat{\mathbf{P}}}_{k|k-1}^y{\mathbf{T}}^T+{\mathbf{R}}_k^y)}^{-1},\end{array}$$

(35)

$$\begin{array}{c}{\widehat{\mathbf{Y}}}_k={\widehat{\mathbf{Y}}}_{k|k-1}+{\mathbf{K}}_k^y{\mathbf{d}}_k,\end{array}$$

(36)

$$\begin{array}{c}{\widehat{\mathbf{P}}}_k^y=(\mathbf{I}-{\mathbf{K}}_k^y\mathbf{T}){\widehat{\mathbf{P}}}_{k|k-1}^y.\end{array}$$

(37)

Then, for the latter EKF, the iteration process of the EKF is shown below:

1. Prediction

$$\begin{array}{c}{\widehat{\mathbf{X}}}_{k|k-1}=\mathbf{A}{\widehat{\mathbf{X}}}_{k-1},\end{array}$$

(38)

$$\begin{array}{c}{\widehat{\mathbf{P}}}_{k|k-1}^x=\mathbf{A}{\widehat{\mathbf{P}}}_{k-1}^x{\mathbf{A}}^T+{\mathbf{Q}}_k^x,\end{array}$$

(39)

2. Estimation

$$\begin{array}{c}{\mathbf{R}}_k^x=\mathbf{T}{\widehat{\mathbf{P}}}_k^y{\mathbf{T}}^T,\end{array}$$

(40)

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

(41)

$$\begin{array}{c}{\mathbf{Z}}_k^{-}=\mathbf{T}{\widehat{\mathbf{Y}}}_k,\end{array}$$

(42)

$$\begin{array}{c}{\widehat{\mathbf{X}}}_k={\widehat{\mathbf{X}}}_{k|k-1}+{\mathbf{K}}_k^x({\mathbf{Z}}_k^{-}-\mathbf{h}\left({\widehat{\mathbf{X}}}_{k|k-1}\right)),\end{array}$$

(43)

$$\begin{array}{c}{\widehat{\mathbf{P}}}_k^x=(\mathbf{I}-{\mathbf{K}}_k^x{\mathbf{H}}_k){\widehat{\mathbf{P}}}_{k|k-1}^x.\end{array}$$

(44)

where the measurements ${\mathbf{Z}}_k^{-}$ and the measured covariance matrix ${\mathbf{R}}_k^x$ in the EKF system are based on the KF, and are calculated in (42) and (40). ${\mathbf{Z}}_k^{-}$ is not the measurement from the anchors; instead, it is the estimation from the KF. The estimation error variance of the KF is selected as the corresponding EKF measurement covariance matrix ${\mathbf{R}}_k^x$. The design uses more accurate distance values as the observations of the EKF, and the estimated state error variance decreases with iterations, which is in line with the trend of decreasing measurement error covariance. The double extended Kalman filter based on RCCA is summarized in Algorithm 1, and the flowchart is shown in Figure 4.

Algorithm 1 Double-layer extended-Kalman filter algorithm.

Input: KF: The initial state (distance, rate of change for distance) of target ${\mathbf{Y}}_0$ and the corresponding estimation error variance matrix ${\mathbf{P}}_0^y$, the initial covariance ${\mathbf{Q}}_0^y$ for Kalman filter process noise ensured by experiments, the measurements ${\mathbf{Z}}_k$ of all distances between M stationary anchors and target. EKF: The initial state (position, velocity, acceleration) of the target ${\mathbf{X}}_0$ and the corresponding estimation error variance matrix ${\mathbf{P}}_0^x$, the initial covariance ${\mathbf{Q}}_0^x$ for Extended Kalman filter process noise. The pre-set threshold ${\mu }_1$ for the first group classification in (20a). For  $k=1:\mathrm{times}$ First Kalman filter with RCCA: - Calculate the prediction state ${\widehat{\mathbf{Y}}}_{k|k-1}$ - Calculate the residual of prediction and measurements ${\mathbf{d}}_k$ using (18) - Judge the range of ${\mathbf{d}}_k$ using (22) - Adjust the ${\mathbf{Q}}_k^y$ using (28) - Calculate the prediction variance ${\widehat{\mathbf{P}}}_{k|k-1}^y$ using (33) - Calculate the ${\mathbf{R}}_k^y$ using (26) - Bring the adjusted ${\mathbf{Q}}_k^y$ and ${\mathbf{R}}_k^y$ into (35) to obtain Kalman gain ${\mathbf{K}}_k^y$ of first KF - Output the distance state $\mathbf{T}{\widehat{\mathbf{Y}}}_k$ and the estimation error variance $\mathbf{T}{\widehat{\mathbf{P}}}_k^y{\mathbf{T}}^T$. Second Extended Kalman filter: - Calculate the prediction state and covariance ${\widehat{\mathbf{X}}}_{k|k-1}$ and ${\widehat{\mathbf{P}}}_{k|k-1}^x$ - Obtain the first KF’s output distance state as observation of second EKF ${\mathbf{Z}}_k^{-}=\mathbf{T}{\widehat{\mathbf{Y}}}_k$, and the estimation error variance as measurement covariance ${\mathbf{R}}_k^x=\mathbf{T}{\widehat{\mathbf{P}}}_k^y{\mathbf{T}}^T$ - Calculate the Kalman gain ${\mathbf{K}}_k^x$ of second EKF - Update the state ${\widehat{\mathbf{X}}}_k$ and output - Update the estimation error variance ${\widehat{\mathbf{P}}}_k^x$ and output. End For Output: ${\widehat{\mathbf{X}}}_k$, ${\widehat{\mathbf{P}}}_k^x$

5. Simulation Results and Experimental Verification

5.1. Simulation Environments and Settings

The simulation was carried out with MATLAB R2021b on a PC. The EKF, IMED-KF [19], M-REKF [30], and proposed DEKF algorithms were tested and compared under the CV and CA models. There are five stationary UWB anchors located at $[x_1^U,y_1^U,z_1^U]=[2\mathrm{m},7\mathrm{m},1\mathrm{m}]$, $[x_2^U,y_2^U,z_2^U]=[12\mathrm{m},7\mathrm{m},2\mathrm{m}],[x_3^U,y_3^U,z_3^U]=[7\mathrm{m},12\mathrm{m},3\mathrm{m}],[x_4^U,y_4^U,z_4^U]=[7\mathrm{m},2\mathrm{m},5\mathrm{m}]$, and $[x_5^U,y_5^U,z_5^U]=[7\mathrm{m},7\mathrm{m},7\mathrm{m}]$. The simulation was performed with 100 Monte Carlo experiments with a time step of 0.01 s. The duration of each experiment was 10 s. The measurement noise was set to have a Gaussian distribution with $({\mu }_m,{\sigma }_m)=(0\mathrm{m},0.1\mathrm{m})$, and the threshold of the first interval was defined as ${\mu }_{1,i}=0.5,$ $i=1,\dots ,M$, where $M=5$ is the number of anchors. The NLOS noise was modeled with a uniform distribution. The initial state of the second EKF was ${\mathbf{X}}_0=[2\mathrm{m},2\mathrm{m},2\mathrm{m},0.4\mathrm{m}/\mathrm{s},0.4\mathrm{m}/\mathrm{s},0.4\mathrm{m}/\mathrm{s},0.02\mathrm{m}/\mathrm{s},0.02\mathrm{m}/\mathrm{s},$${0.02\mathrm{m}/\mathrm{s}]}^T$, and the initial state error variance ${\mathbf{P}}_0^x$ for the EKF was $diag\left(\right[0.1,0.1,0.1,0.01,0.01,0.01,0.005,0.005,0.005\left]\right)$. The distances in the initial state of the first KF were calculated by the initial target position and the anchor positions with $h_i\left({\mathbf{X}}_k\right)=\sqrt{{\left(x_k-x_i^U\right)}^2+{\left(y_k-y_i^U\right)}^2+{\left(z_k-z_i^U\right)}^2}$, and the variation rate of the distances $v_{d,0}$ was set to be $0.1\mathrm{m}/\mathrm{s}$. The initial state error variance for the KF was defined as ${\mathbf{P}}_0^y=diag\left([0.1,\cdots ,0.1,0.01,\cdots ,0.01]\right)$.

As mentioned in Section 2.2, uniformly distributed noise was chosen as the NLOS noise model. The NLOS noise model is described in Figure 5, where the black blocks represent the occupied time of the NLOS noise with $\mathrm{U}(0,10\mathrm{m})$ uniformly distributed noise, and the white blocks represent the occupied time in the LOS environment. Both the NLOS and LOS environments also have Gaussian measurement noise with $({\mu }_m,{\sigma }_m)=(0\mathrm{m},0.1\mathrm{m})$. The LOS/NLOS noise distribution for the five anchors in 1000 iteration times is shown in Figure 5.

5.2. Performance Metrics

The root mean squared error (RMSE) [39] is used to evaluate the accuracy of positioning, and it is defined as:

$$RMS{E}_k=\sqrt{{\displaystyle \frac{1}{L}}\sum _{l=1}^L{({\mathbf{X}}_{l,k}-{\widehat{\mathbf{X}}}_{l,k})}^T({\mathbf{X}}_{l,k}-{\widehat{\mathbf{X}}}_{l,k})}.$$

(45)

where L is the number of Monte Carlo trials.

In addition, the Cramer–Rao lower bound (CRLB) [40,41,42] is also used as a metric for algorithm performance. Here, the Posterior CRLB (PCRLB) is used as the metric for ${\mathbf{X}}_k$ estimation; it is defined as:

$$E\left[({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_k){({\mathbf{X}}_k-{\widehat{\mathbf{X}}}_k)}^T\right]\ge {\mathbf{J}}_k^{-1}={\mathrm{PCRLB}}_k,$$

(46)

where ${\mathbf{J}}_k$ is the Fisher information matrix:

$${\mathbf{J}}_k={[\mathbf{A}{\mathbf{J}}_{k-1}^{-1}{\mathbf{A}}^T+{\mathbf{Q}}_{k-1}]}^{-1}+{\mathbf{H}}_k^T{\mathbf{R}}_k^{-1}{\mathbf{H}}_k.$$

(47)

The cumulative distribution function (CDF), which is defined in (21), is also taken into account in the evaluation of target tracking accuracy.

5.3. Simulation Results

For generality, the CA model was used in the simulations, as shown in Figure 6, in which the RMSE and CDF of different filtering results are presented in the environment where the noise of all five anchors contains LOS only. In the LOS environment, EKF and the proposed DEKF can have a steady-ranging accuracy within 10 cm, while IMED-KF and M-REKF diverge during iteration. Using the two-state Markov chain described in Section 2.2 as the NLOS/LOS environment simulation model, four different scenarios, S1–S4, were set, and the NLOS noise distribution of the five anchors in different scenarios is as follows:

S1:

$\epsilon =[0.1,0,0,0,0],$

S2:

$\epsilon =[0,0.25,0,0.25,0],$

S3:

$\epsilon =[0,0.25,0.1,0.75,0],$

S4:

$\epsilon =[0.25,0.25,0.25,0.25,0.25].$

Figure 6. CA model: (a) RMSE comparison in LOS environment; (b) CDF comparison in LOS environment.

Figure 6. CA model: (a) RMSE comparison in LOS environment; (b) CDF comparison in LOS environment.

In scenario S4, the results in the CA model present different outcomes, as shown in Figure 7. In the CA model, the RMSE of IMED-KF grows fast during iteration, and M-REKF also tends to increase with a smaller slope. On the contrary, the proposed DEKF is stable within 5 cm, and EKF is stable with a meter-level RMSE. The simulation results illustrate that IMED-KF and M-REKF in the CA motion model diverge much more easily, leading to an inaccurate result. However, the proposed DEKF is stable and highly accurate in the CA model. The tracking RMSE results in the LOS and S1–S4 environments for the CA model are illustrated in

Table 2. CA model: positioning result comparison in LOS and S1–S4.

Noise Situation	Tracking Results: Average RMSE/m
	DEKF	EKF	IMED-KF	M-REKF
LOS	0.022	0.017	0.181	2.227
S1	0.023	0.228	0.232	2.245
S2	0.027	0.926	3.385	2.327
S3	0.052	1.453	8.647	2.408
S4	0.054	1.868	11.934	2.467

. It can be seen that DEKF can remain stable and have a much more accurate range when the environment changes, while EKF, IMED-KF, and M-REKF are far from accurate.

5.4. Experimental Verification of the Algorithm

In order to test the performance of the proposed algorithm in a complex indoor environment, an experimental environment was built in our office using a UWB module, including four anchors and one target. The office area is about $46{\mathrm{m}}^2 (4.76\times 9.63\mathrm{m})$, and there were many people and some equipment in the office when these experiments were carried out, as shown in Figure 8.

The DEKF, EKF, and IMED-KF algorithms are used for performance comparison. As shown in Figure 9a, red lines and red dots represent the position and trajectory of the target, and green dots represent stationary points. Blue points represent the anchors, and their coordinates are $B{S}_1(0.20,1.68)\mathrm{m},B{S}_2(4.45,1.68)\mathrm{m},B{S}_3(0.20,7.15)\mathrm{m}$, and $B{S}_4(4.45,6.18)\mathrm{m}$. The anchor height was set to $1.7\mathrm{m}$ to aid us in building the experimental environment in the lab, such as adding metal sheets to block the anchors for NLOS occurrence in tracking experiments. Figure 9b shows the test track in the indoor office environment.

The dashed line in Figure 9a shows the motion of the target. The target performs a $3\mathrm{m}$ reciprocating motion. Its x-axis remains the same, while the y-axis is displaced up to $6\mathrm{m}$.

Figure 10 illustrates the RMSE values of several algorithms mentioned in the previous section for the LOS environment and the four NLOS cases. The RMSEs of these algorithms in the LOS environment are very close, under 10 cm, which means that the three algorithms are all accurate enough to achieve real-time tracking. In the four NLOS environment cases, the RMSEs of the three algorithms are relatively small, and the RMSE of DEKF is the smallest.

Figure 11 shows the RMSEs of each algorithm when changing the number of anchors subject to NLOS interference from one to two. When the target returns from the other side of the track, an NLOS anchor is added for a duration of about half the experiment time. The RMSE of EKF and IMD-KF increases rapidly at half time, while DEKF remains stable when the number of NLOS anchors increases.

For the CA model, the results in the LOS and NLOS situations are shown in

Table 3. Positioning test result comparison in CA model.

NLOS Number	Tracking Results/m
	DEKF	EKF
LOS	tracking	tracking
	${\mathrm{RMSE}}_{max}=0.33$	${\mathrm{RMSE}}_{max}=0.53$
	${\mathrm{RMSE}}_{avg}=0.17$	${\mathrm{RMSE}}_{avg}=0.18$
1-NLOS	tracking	tracking
	${\mathrm{RMSE}}_{max}=0.37$	${\mathrm{RMSE}}_{max}=0.69$
	${\mathrm{RMSE}}_{avg}=0.18$	${\mathrm{RMSE}}_{avg}=0.27$
2-NLOS	tracking	tracking
	${\mathrm{RMSE}}_{max}=0.39$	${\mathrm{RMSE}}_{max}=0.71$
	${\mathrm{RMSE}}_{avg}=0.18$	${\mathrm{RMSE}}_{avg}=0.34$
3-NLOS	tracking	tracking
	${\mathrm{RMSE}}_{max}=0.54$	${\mathrm{RMSE}}_{max}=0.89$
	${\mathrm{RMSE}}_{avg}=0.24$	${\mathrm{RMSE}}_{avg}=0.41$
4-NLOS	tracking	tracking
	${\mathrm{RMSE}}_{max}=0.61$	${\mathrm{RMSE}}_{max}=255.48$
	${\mathrm{RMSE}}_{avg}=0.26$	${\mathrm{RMSE}}_{avg}=53.56$

. Among the three algorithms, IMED-KF cannot obtain a stable and accurate result in this motion model, whereas the proposed DEKF and EKF can track the target in a relatively accurate way. In the LOS environment, DEKF and EKF can reach high accuracy with values close to each other. When the number of NLOS anchors increases, EKF tracks the target more and more inaccurately, especially in the 4-NLOS situation. However, the RMSE of DEKF can remain stable when the environment become severe, with an average RMSE from 17 cm to 26 cm.

6. Conclusions

This article proposes a double extended Kalman filter algorithm in order to fix the weakening effect on tracking accuracy when the measurements have NLOS and measuring noises. The preceding Kalman filter with a residual classification and covariance adjustment (RCCA) module was designed to smooth the distance input and can adjust the Kalman gain in time as the observations change. Then, the states of the preceding KF into the next EKF as the distance observations change. The experimental results demonstrate that the algorithm presented in this paper exhibits high accuracy under both CV and CA models. It can achieve centimeter-level accuracy in LOS/NLOS environments, specifically, an accuracy within 4 cm in LOS and within 10 cm in NLOS, and it converges at a faster speed than its counterparts according to the results of numerous simulations. Using the proposed algorithm, target tracking will be performed quite accurately when the LOS measurements are diminished near to zero. In addition, the accuracy will not decrease significantly when severe NLOS errors lasting a long time occur, as verified by the simulation and experimental results.