A Novel Non-Line-of-Sight Error Mitigation Algorithm Using Double Extended Kalman Filter for Ultra-Wide Band Ranging 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). The time of arrival (TOA) measurements collected by multiple stationary ultra-wide band (UWB) sensors are used. Residual errors between the measured TOA and the prediction from the first KF are used to adjust the covariance of the first KF accordingly. Then, we use the estimated distance state of the first KF as a measurement vector of 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 a good accuracy even without or with only one line-of-sight(LOS) TOA measurement for a period of time without prior information of the NLOS noise, which may be difficult to obtain in practical applications. Another advantage is that the accuracy does not significantly decrease when NLOS noises persist for a long period of time. Finally, the proposed DEKF can maintain high-precision positioning characteristics in both the constant velocity (CV) model and the constant acceleration (CA) model for LOS/NLOS environments. In the case of mixed LOS/NLOS environments, the RMSE of the proposed algorithm can be kept within 5 cm, while the RMSEs of other compared algorithms are easily beyond tens of centimeters. At the same time, when the confidence of RMSE is set to 95% for 1000 MC simulations, the confidence interval of the proposed algorithm is the smallest, and the mean value is 3–5 times closer to the true value compared to other algorithms. Simulation and experimental results show that the proposed algorithm performs much better than other state-of-the-art algorithms, particularly in severe mixed LOS/NLOS environments.

1. Introduction

With the development of technology, the demand for location-based service (LBS) is increasing rapidly [1,2,3,4,5]. Although various localization techniques are used in different systems, such as hybrid systems that combine kinematic modeling with sporadic measurements [6] and Bayesian filtering techniques in real-time location systems [7], wireless localization systems are widely employed for their great advantages of achieving low cost and high accuracy simultaneously. Currently, there are several technologies for indoor positioning, such as Bluetooth Low Energy (BLE) [8,9,10], WIFI [11,12,13], Radio Frequency Identification (RFID) [14,15,16], Impulse Radio–Ultra Wideband (IR-UWB) [17,18,19,20], of which IR-UWB is superior to others with centimeter positioning accuracy in complex indoor environments.

UWB signals are impacted by multipath effects, which may lead to great measurement errors, especially in NLOS environments. The algorithm proposed in [21] maintains the LOS pseudo-measurement results while discarding the NLOS ones, and calculates the average of all selected LOS measurements. The limitation of this tracking algorithm is that when there is no LOS BaseStation for a period of time, the estimation of states can only rely on the prediction. The Kalman filter and its enhancements, like EKF, cubature Kalman filter (CKF), unscented Kalman filter (UKF), adaptive Kalman filter (AKF), or particle filter (PF), are used [22,23,24,25,26,27,28,29] to reduce the errors introduced by NLOS noises for complex indoor environments [30,31]. Although Kalman filters are suitable for systems with white Gaussian noise, their accuracy will be greatly affected in the case of LOS/NLOS noise mixture environments. To reduce measurement errors, extended KFs (EKFs) with robust estimation, such as M-estimation and fuzzy-tuning M-estimation EKF, are proposed in [32]. In this algorithm, the CV model is used as the motion model. If only measurement and NLOS errors are taken into account, these algorithms finally tend to converge to final values with reasonable RMSEs. If the CA model is used, they are likely to diverge with very large RMSEs, which is unacceptable in most scenarios. That is, most existing filtering algorithms have limited 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 [33]. However, due to its high computational complexity, it cannot easily meet the application scenarios with high real-time requirements. Classification of LOS and NLOS conditions using deep learning (DL) models has been mentioned in [34]. However, it 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 have also been mentioned in [35]. But 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 the 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 calculating the residuals between the first KF’s observed measurements and its system predictions, and then according to the error ranges, the covariance of the first KF system is adjusted for the Kalman gain. After that, the distance vectors will be used as measurements for the second EKF, and the estimated variance of the first KF will be input as measurement covariance. This completes the RCCA process. The proposed DEKF algorithm can perform target tracking with a stable centimeter accuracy even without or with just one LOS TOA measurement for a period of time without prior information of the NLOS noise. In addition, it is able to adjust the covariance of the system before estimation, allowing the system to perform timely parameter adjustments without a significant reduction in the precision of the previous estimate. Finally, the proposed DEKF can maintain high precision in both the CV model and the CA model for LOS/NLOS mixture environments.

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 details of the DEKF algorithm. In Section 5, simulation results and experimental verification are shown. 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 that 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$ refers to the coordinates of the target, ${[{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k]}^T$ is the velocity of the target, and ${[{\ddot{x}}_k,{\ddot{y}}_k,{\ddot{z}}_k]}^T$ is the acceleration of the target. 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, the 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)

And the measured distances between target and anchors are calculated by 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}$ in (3) is the transition matrix of states in 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)

and $T_s$ in (5) 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 nonlinear 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 nonlinear case into a locally linear case. For nonlinear functions, the properties of derivatives can be exploited for local linearization by the 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 as 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 nonlinear function $h_i\left({\mathbf{X}}_k\right)$ does not contain ${[{\dot{x}}_k,{\dot{y}}_k,{\dot{z}}_k]}^T$; 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 of ${\mathbf{Q}}_k^x$. In (4), ${\mathbf{v}}_k$ is a measurement noise vector with a covariance matrix of ${\mathbf{R}}_k^x$, which is determined by the measurement accuracy of the anchors. ${\mathbf{Q}}^x$ in the CV model is defined as follows [36]:

$${\mathbf{Q}}^x={\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)

${\Phi }_{s1}$ in (7) is the spectrum density of 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 shown as follows [22,37,38]:

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

(8)

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

(9)

$$\begin{array}{cc}\hfill {\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},\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\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)),\hfill \end{array}$$

(11)

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

(12)

Equations (8) and (9) are update equations from the $k-1$ state to the k state, and (10)–(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 transition matrix of the state, ${\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 transition matrix of measurement, ${\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 Errors 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. Measurement error modeling is shown 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. NLOS noise is usually modeled by a uniform distribution, mean-shifted Gaussian distribution, or exponential distribution [21,32,38]. In the modeling, we assume a uniformly distributed NLOS noise in the following constraints for simplicity, although there are other methods to describe NLOS noise, such as the Markov process. For a mixed LOS/NLOS environment, the probability density function of ${\eta }_i$ is defined as follows:

$$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 mentioned in [21,38,39,40]. The state transition is shown in Figure 1, and the transition probability matrix of the Markov process is as follows:

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

(15)

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

$${\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 the 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, distances between the target and 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 the 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 the state and the observation.

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

(19)

Then, classify the range of the obtained residual:

$$\begin{array}{cc}\hfill d_k& \{\begin{array}{}\mathrm{(20a)}& 0<\max \left(d_k\right)<{\mu }_1\hfill \mathrm{(20b)}& {\mu }_1\le \max \left(d_k\right)<{\mu }_2\hfill \mathrm{(20c)}& {\mu }_2\le \max \left(d_k\right)<{\mu }_3\hfill \mathrm{(20d)}& ⋮\hfill \mathrm{(20e)}& {\mu }_{N-3}\le \max \left(d_k\right)<{\mu }_{N-2}\hfill \mathrm{(20f)}& {\mu }_{N-2}\le \max \left(d_k\right)<{\mu }_{N-1}\hfill \mathrm{(20g)}& {\mu }_{N-1}\le \max \left(d_k\right)<{\mu }_N\hfill \end{array}\hfill \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 follows [41]:

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

(21)

When a = 0.375, $F\left(a\right)$ = $0.9999$. This value is already very close to 1. When a is greater than 0.7, the sensitivity to noise will be 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=\left\{\begin{array}{c}0<max\left({\mathbf{d}}_k\right)<0.5\hfill \\ 0.5\le max\left({\mathbf{d}}_k\right)<1\hfill \\ 1\le max\left({\mathbf{d}}_k\right)<10\hfill \\ 10\le max\left({\mathbf{d}}_k\right)<20\hfill \\ 20\le max\left({\mathbf{d}}_k\right)<30\hfill \\ 30\le max\left({\mathbf{d}}_k\right)<40\hfill \\ 40\le max\left({\mathbf{d}}_k\right)<50\hfill \\ ⋮\hfill \end{array}\right.$$

(22)

After obtaining the residual range, the expectation of the squared residuals will be defined as follows:

$$\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{T}$ is given in (19) and where $\mathbf{G}$ is a transition matrix of states defined as follows:

$$\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}$ follows a uniformed 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 bounds of $\mathrm{X}$, respectively.

Therefore, when the residual ${\mathbf{d}}_k$ is 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 already been classified; 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 previous estimation. ${\mathbf{Q}}_k^y$ is generally not adjusted significantly, and the adjustment range is between 0 and 1. The parameter $\lambda$ is defined as the adjustment factor of ${\mathbf{Q}}_k^y$, which is defined as follows:

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

(27)

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

When the residual ${\mathbf{d}}_k$ is large, it means that the observed value contains a large noise component, and 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, the residual’s 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 for the presence of NLOS interference over an extended period. The flowchart of RCCA is shown in Figure 2.

4. Double Extended Kalman Filter Based on RCCA

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

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

KF:

• 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)
• 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)
• 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 filter, the iteration process of EKF filter is shown below:

EKF:

• 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)
• 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 filter, which are calculated in (40) and (42). ${\mathbf{Z}}_k^{-}$ is not the measurements from anchors; instead, it is the estimation from the KF filter. The estimation error variance of the KF filter 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 filter, 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.

The major computational complexity of the proposed DEKF algorithm lies in the matrix operation, and the complexity of each step of the algorithm is shown in

Table 2. Complexity evaluation.

Steps of the Algorithm	MAC Computational Complexity
Calculate the prediction state ${\widehat{\mathbf{Y}}}_{k|k-1}$	$\mathcal{O}\left(M^2\right)$
Calculate the residual of prediction and measurements ${\mathbf{d}}_k$	$\mathcal{O}\left(M^2\right)$ + $\mathcal{O}\left(M\right)$
Judge the range of ${\mathbf{d}}_k$	$\mathcal{O}(log{M}^2)$  +  $\mathcal{O}\left(N\right)$
Adjust the ${\mathbf{Q}}_k^y$	$\mathcal{O}\left(M^2\right)$ + $\mathcal{O}\left(1\right)$
Calculate the prediction variance ${\widehat{\mathbf{P}}}_{k|k-1}^y$	$\mathcal{O}\left(M^3\right)$ + $\mathcal{O}\left(M^2\right)$ + $\mathcal{O}\left(M\right)$
Calculate the ${\mathbf{R}}_k^y$	$\mathcal{O}\left(M^3\right)$ + $\mathcal{O}\left(M^2\right)$ + $\mathcal{O}\left(M\right)$
Bring the adjusted ${\mathbf{Q}}_k^y$ and ${\mathbf{R}}_k^y$ into (35) to obtain the Kalman gain ${\mathbf{K}}_k^y$ of the first KF	$\mathcal{O}\left(M^3\right)$
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$	$\mathcal{O}\left(M^3\right)$ + $\mathcal{O}\left(M^2\right)$
Calculate the prediction state and covariance ${\widehat{\mathbf{X}}}_{k|k-1}$ and ${\widehat{\mathbf{P}}}_{k|k-1}^x$	$\mathcal{O}\left(L^3\right)$ + $\mathcal{O}\left(L^2\right)$
Calculate the Kalman gain ${\mathbf{K}}_k^x$ of the second EKF	$\mathcal{O}\left(M^3\right)$ + $\mathcal{O}\left(M^2\right)$ + $\mathcal{O}\left(M\right)$ + $\mathcal{O}\left(M^{2}L\right)$ + $\mathcal{O}\left(M{L}^2\right)$
Update the state ${\widehat{\mathbf{X}}}_k$ and output	$\mathcal{O}\left(M\right)$ + $\mathcal{O}\left(ML\right)$ + $\mathcal{O}\left(L\right)$
Update the estimation error variance ${\widehat{\mathbf{P}}}_k^x$ and output	$\mathcal{O}\left(L^3\right)$ + $\mathcal{O}\left(L^2\right)$ + $\mathcal{O}\left(M{L}^2\right)$

, where M is the number of anchors, N is the number of residual classification partitions, and L is the dimension of the state vector of the EKF, which is equal to 6 in the CV model and 9 in the CA model. In general, the total DEKF algorithm complexity is $\mathcal{O}\left(M^3\right)$ + $\mathcal{O}\left(M^2\right)$ + $\mathcal{O}\left(M\right)$ + $\mathcal{O}(log{M}^2)$ + $\mathcal{O}\left(L^3\right)$ + $\mathcal{O}\left(L^2\right)$ + $\mathcal{O}\left(L\right)$ + $\mathcal{O}\left(M^{2}L\right)$ + $\mathcal{O}\left(M{L}^2\right)$ + $\mathcal{O}\left(NL\right)$ + $\mathcal{O}\left(N\right)$ + $\mathcal{O}\left(1\right)$. As M is no more than 5 in most scenarios, the computational complexity of DEKF can be approximately equal to $\mathcal{O}\left(L^3\right)$, which is $\mathcal{O}\left(6^3\right)$ for the CV model and $\mathcal{O}\left(9^3\right)$ for the CA model. This complexity can be easily implemented in most low-cost hosts with limited resources.

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 the Kalman filter process noise as determined by experiments, and the measurements ${\mathbf{Z}}_k$ of all distances between M stationary anchors and the 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$, and the initial covariance ${\mathbf{Q}}_0^x$ for the extended Kalman filter process noise. The preset 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 the Kalman gain ${\mathbf{K}}_k^y$ of the 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 distances state as the observation of the second EKF ${\mathbf{Z}}_k^{-}=\mathbf{T}{\widehat{\mathbf{Y}}}_k$, and the estimation error variance as the 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 the 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 [21], M-REKF [32], and the proposed DEKF algorithms were tested and compared under the CV and CA models. There were five stationary UWB anchors located at $[x_1^U,y_1^U,z_1^U]=\left[2\mathrm{m},7\mathrm{m},1\mathrm{m}\right]$, $[x_2^U,y_2^U,z_2^U]=\left[12\mathrm{m},7\mathrm{m},2\mathrm{m}\right],[x_3^U,y_3^U,z_3^U]=\left[7\mathrm{m},12\mathrm{m},3\mathrm{m}\right],[x_4^U,y_4^U,z_4^U]=\left[7\mathrm{m},2\mathrm{m},5\mathrm{m}\right]$, and $[x_5^U,y_5^U,z_5^U]=\left[7\mathrm{m},7\mathrm{m},7\mathrm{m}\right]$. 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 be a Gaussian distribution with $({\mu }_m,{\sigma }_m)=\left(0\mathrm{m},0.1\mathrm{m}\right)$, and the threshold of the first interval was defined as ${\mu }_{1,i}=0.5,i=1,...,M$, where $M=5$ was the number of anchors. NLOS noise was modeled by 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 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 initial state of the first KF was 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 rates of distances $v_{d,0}$ were set to be 0.1 m/s. The initial state error variance for 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, the 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 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 NLOS and LOS environments also have Gaussian measurement noise with $({\mu }_m,{\sigma }_m)=\left(0\mathrm{m},0.1\mathrm{m}\right)$. The LOS/NLOS noise distribution for five anchors over 1000 iterations is shown in Figure 5.

5.2. Performance Metrics

The root mean squared error (RMSE) [41] was used to evaluate the accuracy of positioning, which is defined as follows:

$$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. After performing L Monte Carlo experiments, the RMSE was calculated according to the formula of Equation (45) for subsequent performance comparison and analysis.

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

$$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)

Cumulative distribution function (CDF) was also taken into account during evaluation for target tracking accuracy, which is defined in (21).

5.3. Simulation Result

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 the five anchors all 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 will 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 shown 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].$

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 grew fast during iteration, and M-REKF also tended to increase with a smaller slope. In contrast, the proposed DEKF was stable within 5 cm, and EKF was 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 inaccurate results. However, the proposed DEKF is stable and highly accurate in CA models. The tracking RMSE results in LOS and the S1–S4 environments for the CA model are illustrated in

Table 3. CA model: comparison of positioning results 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

, which means DEKF can maintain stable and much more accurate ranging characteristic when the environment changes, while EKF, IMED-KF, and M-REKF are far from accurate. A total of 1000 Monte Carlo experiments were carried out for various algorithms to obtain the results and mean values. The confidence intervals for various algorithms were derived from the MC experiments with the confidence level set to 95%. As all experiments were carried out with the CA model, in which error accumulation effects become significant over time. We use 1/3, 2/3, and the final time point within the experimental time interval as the time line for MC experimental comparison for various algorithms. The result is shown in

Table 4. Comparison of confidence levels and confidence intervals across different algorithms.

Alogrithm	Mean Value/m			Confidence Interval
	Comparison Time Line			Comparison Time Line
	1/3	2/3	Final	1/3	2/3	Final
DEKF	3.2628	4.7504	6.4073	[3.2166,3.3091]	[4.6926,4.8082]	[6.3506,6.4585]
EKF	1.9671	3.1457	5.8684	[1.4389,2.4953]	[2.7987,3.4928]	[5.3568,6.3800]
IMED-KF	3.1209	2.3843	12.3286	[1.3761,4.8657]	[−4.6303,9.3989]	[−1.1058,25.7630]
M-REKF	3.1209	2.3843	12.3286	[1.3761,4.8657]	[−4.6303,9.3989]	[−1.1058,25.7630]

.

5.4. Experimental Verification of Algorithm

To evaluate the efficacy of the proposed algorithm within a sophisticated indoor setting, we constructed an experimental environment in our office, leveraging a UWB module that comprises four anchors and one target. For UWB communication and localization, we utilized DW3000 chips sourced from American company Qorvo, Greensboro, NC, USA. The office area was about 46 m² (4.76 × 9.63 m), and there were many people and equipment in the office when the experiments were carried out, as shown in Figure 8.

The DEKF, EKF, and IMED-KF algorithms were used for performance comparison. As shown in Figure 9a, the red lines and red dots represent the position and trajectory of the target, and the green dots represent stationary points. The blue points in it 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)$ m. The heights of the anchors were set to be 1.70 m. Figure 9b describes the test track in the indoor office environment.

The dashed line shown in Figure 9a shows the motion of the target. The target performed a 3 m reciprocating motion. Its x-axis remained the same, while the y-axis was displaced up to 6 m.

Figure 10 illustrates the RMSE values of several algorithms mentioned in the previous section for LOS and four NLOS cases. The RMSEs of these algorithms in the LOS environment were very close, under 10 cm, which means the three algorithms were all accurate enough to achieve real-time tracking. In the four NLOS environment cases, the RMSEs of the three algorithms were relatively small, and the RMSE of DEKF was 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 returned from the other side of the track, an NLOS anchor was added for a duration of about half the experiment time. The RMSE of EKF and IMD-KF increased rapidly at half time, while DEKF remained stable when the number of NLOS anchors increased.

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

Table 5. Comparison of positioning test results in the 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 could not obtain a stable and accurate result in this motion model, and the proposed DEKF and EKF could track the target in a relatively accurate way. In the LOS environment, DEKF and EKF both achieved high accuracy, with values close to each other. When the number of NLOS anchor increased, EKF tracked the target more and more inaccurately, especially in the 4-NLOS situation. However, the RMSE of DEKF kept stable when the environment became severe, whose average RMSE ranged from 17 cm to 26 cm.

5.5. Comparative Analysis of Algorithms

The limitation of the algorithm mainly lies in its computational complexity. Because the two Kalman filters are concatenated and the residual is calculated and classified at the same time, the matrix operations involved are increased, resulting in a high overall algorithm complexity. The specific algorithm complexity analysis is mentioned in Table 2. But despite this complexity, the algorithm can be easily implemented in most low-cost hosts with limited resources. According to the experimental results, the advantages and disadvantages of each algorithm can be analyzed, and the analysis results are shown in

Table 6. Algorithm difference comparison results.

Algorithm	Accuracy	Complexity	LOS/NLOS Noise Support	CV/CA Model Scalability
DEKF	High	High	Best Support	Best Scalability
EKF	Moderate	Low	Poorly Support	Good Scalability
IMED-KF	Moderate	Moderate	Limited Support	Limited Scalability
M-REKF	High	Moderate	Limited Supprot	Limited Scalability

. Compared with other comparison algorithms, the algorithm proposed in this paper has advantages in application scenarios and anti-interference ability, at the cost of relatively high complexity.

6. Conclusions

This article proposed a Double Extended Kalman filter algorithm in order to address the degradation in tracking accuracy caused by NLOS and measurement noise. The preceding Kalman filter with a residual classification and covariance adjustment (RCCA) module is designed to smooth the distance inputs, which can adjust the Kalman gain in time as the observations change. Then, the states for the preceding KF will feed into the next EKF as distance observations. In order to validate the feasibility of the proposed algorithm, both simulation experiments and real-world scenario tests were conducted. During the simulation experiments, physical modeling was performed for both NLOS and LOS noise environments, and various scenarios involving mixed noise were set up. The motion models of objects were categorized into two types: the CV model and the CA model. The experimental results demonstrate that the algorithm presented in this paper exhibits high accuracy under both CV and CA models. It can achieve centimeter accuracy in LOS/NLOS environments, and the accuracy is within 4 cm in LOS and within 10 cm in NLOS, and it converges at a faster speed. At the same time, when the confidence of RMSE was set to 95% for 1000 MC simulations, the confidence interval of the proposed algorithm was the smallest, and the mean value was 3–5 times closer to the true value compared to other algorithms. With the proposed algorithm, target tracking can still be performed fairly accurately when the line-of-sight measurements diminish to near zero. In addition, even with severe NLOS errors persisting for a long time, the accuracy does not degrade substantially, as verified by the simulation and experimental results.