MCC-CKF: A Distance Constrained Kalman Filter Method for Indoor TOA Localization Applications

Abstract

Non-Gaussian noise may have a negative impact on the performance of the Kalman filter (KF), due to its adoption of only second-order statistical information. Thus, KF is not first priority in applications with non-Gaussian noises. The indoor positioning based on arrival of time (TOA) has large errors caused by multipath and non-line of sight (NLOS). This paper introduces the inequality state constraint to enhance the ranging performance. Based on these considerations, we propose a constrained Kalman filter based on the maximum correntropy criterion (MCC-CKF) to enhance the TOA performance in the extreme environment of multipath and non-line of sight. Pratical experimental results indicate that MCC-CKF outperforms other estimators, such as Kalman filter and Kalman filter based on maximum entropy.

1. Introduction

In human civilization, the exact location of the target plays an important role in many vital fields, like transportation, military, logistics, exploration, and environment protection, in which we mightily demand for accurate location information. Usually, when we are in outside environment, the global position system (GPS) nearly could meet our needs. Global Navigation Satellite Systems (GNSSs), such as GPS, Galileo and Beidou, represent the satellite-based location technique and have been committed to promoting location-based services. However, satellite-based location techniques cannot function properly indoors, because the satellite signal strength is quickly undermined below the receiving threshold by construction materials or the ground [1]. Therefore, the indoor target localization demands us to find another positioning method that no longer uses the satellite.

As an effective method for indoor positioning, the wireless indoor location techniques have been commonly used in the military, fire search and rescue, intelligent home and many other fields. Among these methods, TOA-based ultra-wideband (UWB) ranging technology has been attracting much attention for its excellent resolution and accuracy.

However, TOA-based ranging and positioning accuracy are affected by multipath and non-line-of-sight (NLOS) scenarios. The error from NLOS [2,3,4] is considered as the most important element affecting the accuracy. Therefore, the research on how to reduce TOA ranging error and improving positioning accuracy has been a long-term concern of the academic world and the industry. LOS (line-of-sight) and NLOS conditions could be distinguished by employing classification methods [5,6], which can typically achieve, in previous studies, up to 92% accuracy in LOS/NLOS identification [4,5,6,7]. In industry, the TOA-based measurement mostly uses RF chips that are DW1000 and NanoLOC released by Decwave [8] and Nanotron [9] respectively. Above two RF chips, both offer several RF signal parameters for LOS/NLOS identification, RSS and PFP, for instance. In addition, these suppliers recommend some algorithms to help their chips distinguish LOS/NLOS scenes more accurately. In summary, the NLOS identification can use its result to mitigate the measured error, as well as enhance the localization accuracy. There have also existed other means to mitigate NLOS influence like Kalman filter [10], redundant range estimations [11], and environmental geometrical constrains [12]. In [13], we can find detailed overviews of the NLOS identification and mitigation.

Kalman filter (KF) is one of the most useful tools to observe linear dynamic systems. The TOA ranging accuracy can be improved by smoothing TOA ranging results to mitigate the impact of the NLOS propagation [14]. However, in practice, TOA ranging is more affected by non-Gaussian noises rather than Gaussian ones, whose errors could achieve as high as 10 m [15,16]. If the system is disturbed by non-Gaussian noise, such as this mismatch, the performance of Kalman filter will be degraded, which has no ability to limit the large ranging error. Because when the noise is non-Gaussian, Kalman filter cannot take advantage of the high-order information of process and measurement noise.

Besides, it is generally considered that KF [17] take advantage of minimum mean square error (MMSE) as the optimal criterion in linear systems with white Gaussian noise. The standard is so sensitive to large outliers and non-Gaussian noise that Kalman filters may not show good performance when the systems are mixed with colored noise [16,18] appearing in many actual application scenarios. In order to effectively work out the estimation problem when the system has colored Gaussian noise, several optimization criteria have been proposed to replace the MMSE. It is worth noting that the maximum correntropy criterion (MCC) has been successfully used in some applications mixed together with colored Gaussian noises [19,20,21].

To enhance the performance of the Kalman filter for TOA ranging and localization applications that have colored Gaussian noise [16,18], maximum correntropy criterion Kalman filtering (MCC-KF) has used the MCC instead of the MMSE as the optimization criterion for LOS/NLOS TOA ranging. The MCC-KF can get much better performance than traditional KF in many cases. Furthermore, the measured distance and its variance can be adopted to constrain the ranging error space, which are regulated to eliminate the effects of colored Gaussian noises. Then, the proposed method can smooth the adjusted distance, which minimizes the error according to the adjusted variance. By combining the MCC with constrained estimation technology, the proposed MCC-based constrained Kalman Filter algorithm (MCC-CKF) can perform better.

In this paper, we proposed a Maximum Correntropy Criterion Constrained Kalman Filter (MCC-CKF) to solve the above mismatch in TOA ranging and localization applications with colored Gaussian noises. By LOS/NLOS identification, Maximum correlation entropy kernel mapping and constrained optimization to TOA ranging/localization process, the localization accuracy can be significantly improved. The results of practical experiment shows that ranging errors fit the Gaussian distribution, and MCC-CKF can dramatically increased the positioning accuracy in practical applications. The rest of this paper is organized as follows: Section 2 analyzes the characteristics of indoor TOA ranging error and its boundaries. Section 3 introduces Maximum Correntropy Criterion (MCC) and MCC based Kalman filtering method for TOA ranging/localization applications. Section 4 elaborates the overall solution of constrained Kalman filter under MCC with considering TOA ranging boundaries. Section 5 introduces performance evaluation based on practical use case. Section 6 comes to the conclusion.

2. Indoor TOA Ranging Error Analysis

Firstly, we give a brief description about TOA distance measuring principle and then, analyze the multipath channel conditions in the applications of TOA ranging, and finally, calculate the TOA ranging error boundaries.

2.1. Time of Arrival

As shown in Figure 1, under most circumstances, we make use of the two-way ranging method to avoid high precision synchronization between two nodes, which are defined as target node and reference node, respectively. In this algorithm, for the first step, the target node gives off an wide-band impulse, which would be obtained by the reference node. At last, the reference node sends back the ranging data in the acknowledgment (Ack) package containing a time stamp. As the Ack signal arrives at the target, we would get the round trip flight time between above mentioned two nodes. Then, the estimated distance could be formulated as follows:

$$\widehat{d}=T_p\times c=\frac{T_{round}-T_{relay}}{2}\times c$$

(1)

where $T_p$ denotes the one-way signal propagation time between the target node and the reference node, $T_{relay}$ represents the ranging data processing time of the reference node and $T_{round}$ denotes the two-way spread time of the impulse. The round trip ranging algorithm is adopted in IEEE 802.15.4a standard, which is an international standard of high accuracy localization technology for wireless sensor network applications.

2.2. Characteristics of TOA Ranging Error

The TOA ranging error mainly comes from the hardware (${ϵ}_d$) and the multipath channel condition. Furthermore, in an indoor environment, the main error source is pulse shift error caused by multipath condition. As shown in Figure 2, the line of sight (LOS) transmission and non-line of sight (NLOS) transmission are two typical scenarios for indoor TOA ranging applications. Therefore, the signal error for TOA ranging can be considered as the multipath error and UDP (undetectable direct path [19]) error:

• Multipath error (${ϵ}_m$) is derived from the peak shift resulted from combining the first detectable path with its adjacent paths.
• UDP error (${ϵ}_u$) is derived from that FDP(first detected path) is not DP, as indicated in Figure 3.

According to the above analysis, We divide the ranging error ($e_{TOA}$) into two parts:

$$e_{TOA}=\left\{\begin{array}{cc}{e}_{LOS}\sim \mathcal{N}({\mu }_{LOS},{\sigma }_{LOS}^2)\hfill & ,s=LOS\hfill \\ e_{NLOS}\sim \mathcal{N}({\mu }_{NLOS},{\sigma }_{NLOS}^2)\hfill & ,s=NLOS\hfill \end{array}\right.$$

(2)

where s indicates the channel state.

In the literature, such as in [3,10], is is shown that $e_{LOS}$ and $e_{NLOS}$ obey normal distribution, namely $e_{LOS}\sim \mathcal{N}({\mu }_{LOS},{\sigma }_{LOS}^2)$ and $e_{NLOS}\sim \mathcal{N}({\mu }_{NLOS},{\sigma }_{NLOS}^2)$.

We use d to denote the true distance between the target node and the reference one. In addition, $\widehat{d}$ represent the estimated distance via TOA. Define random variable:

$$\begin{array}{cc}\hfill {\widehat{d}}_{sub-mean}& =\left\{\begin{array}{cc}\widehat{d}-{\mu }_{LOS},\hfill & s=LOS\hfill \\ \widehat{d}-{\mu }_{NLOS},\hfill & s=NLOS\hfill \end{array}\right.\hfill \end{array}$$

(3)

where s is condition indicator which can be indicated by specific chip register [8] or LOS/NLOS identification method, such as [5,22]. The error of ${\widehat{d}}_{sub-mean}$ obeys ${\widehat{e}}_{sub-mean}={\widehat{d}}_{sub-mean}-d$, namely $e_{sub-mean}\in \mathcal{N}(0,{\sigma }_{sub-mean}^2)$, in which

$$e_{sub-mean}\in \mathcal{N}(0,{\sigma }_{sub-mean}^2)$$

(4)

where p stands for the probability of LOS condition, whereas $1-p$ stands for that of NLOS condition.

2.3. TOA Ranging Error Boundaries

Based on the above considerations, the ranging distance error in LOS and NLOS could be confirmed to fit for a zero-mean Gaussian distribution, as shown in Equation (8). Thus, the $3\sigma$ law of the normal distribution [23] could be employed to set the distance error interval.

Based on the $3\sigma$ law, the ranging error can be divided into the following:

$$\left\{\begin{array}{c}{\mu }_{LOS}-3{\sigma }_{LOS}\le e_{LOS}\le {\mu }_{LOS}+3{\sigma }_{LOS}\hfill \\ {\mu }_{NLOS}-3{\sigma }_{NLOS}\le e_{NLOS}\le {\mu }_{NLOS}+3{\sigma }_{NLOS}\hfill \end{array}\right.$$

(5)

Therefore, the range of estimated distance d can be represented as:

$$\left\{\begin{array}{cc}\widehat{d}-{\mu }_{LOS}-3{\sigma }_{LOS}\le \widehat{d}\le \widehat{d}-{\mu }_{LOS}+3{\sigma }_{LOS},\hfill & ifs=LOS\hfill \\ \widehat{d}-{\mu }_{NLOS}-3{\sigma }_{NLOS}\le \widehat{d}\le \widehat{d}-{\mu }_{NLOS}+3{\sigma }_{NLOS},\hfill & ifs=NLOS\hfill \end{array}\right.$$

(6)

furthermore, which could be described as:

$$\left\{\begin{array}{cc}{\widehat{d}}_{sub-mean}-3{\sigma }_{LOS}\le \widehat{d}\le {\widehat{d}}_{sub-mean}+3{\sigma }_{LOS},\hfill & ifs=LOS\hfill \\ {\widehat{d}}_{sub-mean}-3{\sigma }_{NLOS}\le \widehat{d}\le {\widehat{d}}_{sub-mean}+3{\sigma }_{NLOS},\hfill & ifs=NLOS\hfill \end{array}\right.$$

(7)

We define this constraint as $C(\ast )$ for the convenience of the following description.

3. Maximum-Correntropy-Criterion-Based Kalman Filter (MCC-KF)

Maximum correntropy criterion (MCC) has played a significant role in machine learning, pattern recognition, and signal processing with the existence of non-White Gaussian noise, especially the large outliers [24,25,26]. In this part, we introduce briefly to MCC and Kalman Filter (KF), and then present a MCC-based Kalman filter method for non-White Gaussian noise applications.

3.1. Maximum Correntropy Criterion (MCC)

From the definition of the information theoretic and kernel methods, correntropy denotes the metric similarity between two variables [24,27]. Cross-correntropy between two scalar variables measures the second-order information as well as higher-order statistical information in the joint probability density function [28,29]. We define the cross-correntropy of two random scalar variables X and Y as

$$V(X,Y)=E\left[\kappa (X,Y)\right]=\int \kappa (x,y)d{F}_{XY}(x,y)$$

(8)

where E represents the expectation, and $\kappa (·,·)$ is a positive definite kernel function that satisfies the Mercer theory. In this paper, we use the Gaussian kernel as a kernel function

$$\kappa (x,y)=G_{\sigma }\left(e\right)=exp(-\frac{e^2}{2{\sigma }^2})$$

(9)

where $e=x-y$, and $\sigma \ge 0$ denotes the kernel bandwidth.

In most practical applications, the number of data we could access is scant and the joint distribution $F_{XY}$ is usually unavailable. For solving this problem, one can use the sample mean square to estimate the correntropy:

$$\widehat{V}(X,Y)=\frac{1}{N}\sum _{i=1}^N{G}_{\sigma }\left(e\left(i\right)\right)$$

(10)

where $e\left(i\right)=X_i-Y_i$, ${\{X_i,Y_i\}}_{i=1}^N$ are N samples drawn from $F_{XY}$.

The Gaussian kernel can use Taylor series expansion, we have

$$\widehat{V}(X,Y)=\sum _{n=0}^{\infty }\frac{{(-1)}^n}{2^n{\sigma }^{2n}n!}E\left[{(X-Y)}^{2n}\right]$$

(11)

Given a sequence of error data $e{\left(i\right)}_{i=1}^N$, the expected cost function could be described as:

$$J_{MCC}=\frac{1}{N}\sum _{i=1}^N{G}_{\sigma }\left(e\left(i\right)\right)$$

(12)

Our goal is to get a parameter vector W for an adaptive model. $X_i$ and $Y_i$ term the output of the model and the desired response. The W in MCC can be denoted as the following optimization problem:

$$\widehat{W}=\underset{W\in \Omega }{argmax}\frac{1}{N}\sum _{i=1}^N{G}_{\sigma }\left(e\left(i\right)\right)$$

(13)

where $\widehat{W}$ means the optimal solution, and $\Omega$ represents a feasible set of parameter.

3.2. MCC-KF

A general Kalman Filter [17] makes all attempts to find the best running estimation for a recursive system, whose state and measurement equation can be respectively described as:

$$\begin{array}{cc}{x}_k=F_{k,k-1}{x}_{k-1}+u_{k,k-1},\hfill & u_{k,k-1}\sim \mathcal{N}(0,Q_{k,k-1})\hfill \\ z_k=H_k{x}_k+v_k,\hfill & v_k\sim \mathcal{N}(0,R_k)\hfill \end{array}$$

(14)

Here $x_k$ is an n-vector that represents the true state of the underlying system. The state matrix $x_k$ for TOA-based distance measurement can be defined as:

$$x_k={\left[d_k{v}_k\right]}^T$$

(15)

where $d_k$ is the distance between target and reference node. Besides, $v_k$ stands for target node’s velocity. The covariance matrices in the Kalman Filter provide us with the uncertainty in the state estimation. For a more detailed discussion of Kalman Filtering, please consult [30].

Therefore, we formulate a a new cost function to better describe the condition with the existence of colored noises, in consideration of the weighting matrices of least squared (LS) method [3,31]. It could be described as follows:

$$\begin{array}{c}\hfill J_m=G_{\sigma }\left(\right||{\widehat{x}}_{k|k}-F_{k,k-1}{\widehat{x}}_{k|k-1}{\left|\right|}_{P_{k|k-1}^{-1}})+\frac{1}{m}\sum _{j=1}^m{G}_{\sigma }\left(\right||z_{jk}-H_j{\widehat{x}}_{k|k}{\left|\right|}_{R_{jjk}^{-1}})\end{array}$$

(16)

where $G_{\sigma }\left(\right||·\left|\right|)=exp(\frac{-\left|\right|·{\left|\right|}^2}{2{\sigma }^2})$, $\left|\right|·{\left|\right|}_M$ denotes the M-weighted two-norm of a vector, $\sigma$ is the defined bandwidth. $R_{jjk}$ is the jth element of diagonal matrix $R_k$. $H_j$ is the jth row of matrix H.

In order to minimize this objective function, we find its derivative with respect to ${\widehat{x}}_k$ by solving

$$\frac{\partial J_m}{\partial {\widehat{x}}_{k|k}}=0$$

(17)

Then, the new estimator could be derived as follows:

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(z_{k|k}-H_k{\widehat{x}}_{k|k-1})$$

(18)

$$K_k={(P_{k|k-1}^{-1}+{\gamma }_k{H}^T{R}_k^{-1})}^{-1}{\gamma }_k{H}^T{R}_k^{-1}$$

(19)

where

$${\gamma }_k=\frac{G_{\sigma }\left(\right||z_{k|k}-H_k\widehat{x_{k|k}}{\left|\right|}_{P_{k|k-1}^{-1}})}{G_{\sigma }\left(\right||{\widehat{x}}_{k|k}-F_{k,k-1}\widehat{x_{k|k-1}}{\left|\right|}_{R_{jjk}^{-1}})}$$

(20)

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(z_{k|k-H_k{\widehat{x}}_{k|k-1}})$$

(21)

$$P_{k|k}=(I-K_{k}H)P_{k|k-1}{(I-K_{k}H)}^T+K_k{R}_k{K}_k^T$$

(22)

The Kalman filter based on maximum correntropy criterion (MCC-KF) make use of correntropy to conquer colored Gaussian noise and also take advantage of the covariance in gain matrix computation, as described in Algorithm 1.

Algorithm 1 Maximum correntropy criterion Kalman filter (MCC-KF)

0: Initialization: 1: ${\widehat{x}}_0=E\left[x_0\right]$, $P_0=E[(x_0-{\widehat{x}}_0){(x_0-{\widehat{x}}_0)}^T]$ Prior estimation: % Compute the prior distance estimation 2: ${\widehat{x}}_{k|k-1}=F_{k,k-1}{\widehat{x}}_{k-1}$ % Compute the prior covariance matrix 3: $P_{k|k-1}=F_{k,k-1}{F}_{k,k-1}^T+Q_k$ Posteriori estimation: % Compute the Gaussian kernel parameter for update 4: ${\gamma }_k=\frac{G_{\sigma }\left(\right||z_{k|k}-H_k\widehat{x_{k|k}}{\left|\right|}_{P_{k|k-1}^{-1}})}{G_{\sigma }\left(\right||{\widehat{x}}_{k|k}-F_{k,k-1}\widehat{x_{k|k-1}}{\left|\right|}_{R_{jjk}^{-1}})}$ % Update the posterior Kalman gain 5: $K_k={(P_{k|k-1}^{-1}+{\gamma }_k{H}^T{R}_k^{-1})}^{-1}{\gamma }_k{H}^T{R}_k^{-1}$ % Updata the posterior distance estimation 6: ${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k(z_{k|k-H_k{\widehat{x}}_{k|k-1}})$ % Update the posterior covariance matrix 7: $P_{k|k}=(I-K_{k}H)P_{k|k-1}{(I-K_{k}H)}^T+K_k{R}_k{K}_k^T$ 8: $k+1\to k$, go to step 2.

4. Constrained Kalman Filter under MCC

In order to improve the performance of the Kalman filter in TOA ranging and localization applications with heavy-tailed Colored-Gaussian noise, MCC-KF is detailed in the above section. Furthermore, the measured distance and its variance can be adopted to constrain the ranging error space. In this section, a constrained Kalman filter method under MCC will be proposed to combine the MCC with the constrained estimation technology [32,33]. The unconstrained MCC-KF solution is projected onto the constrained state surface (as shown in Equation $C(\ast )$).

In order to combine the constrained ranging information into the filter, we define the following cost function:

$$\begin{array}{cc}{J}_m=\hfill & G_{\sigma }\left(\right||{\widehat{x}}_{k|k}-{\widehat{x}}_{k|k-1}{\left|\right|}_{P_{k|k-1}^{-1}})+\frac{1}{m}\sum _{j=1}^m{G}_{\sigma }\left(\right||z_{jk}-H_j{\widehat{x}}_{k|k}{\left|\right|}_{R_{jjk}^{-1}})+\frac{1}{2}\left|\right|{\widehat{x}}_{k|k}-d{\left|\right|}_{W^{-1}}\hfill \\ \hfill & s.t.C{\widehat{x}}_{k|k}-D\le 0\hfill \end{array}$$

(23)

where $G_{\sigma }\left(\right||·\left|\right|)=exp(\frac{-\left|\right|·{\left|\right|}^2}{2{\sigma }^2})$, $\left|\right|·{\left|\right|}_M$ denotes the M-weighted two-norm of a vector, $\sigma$ is the defined bandwidth. $R_{jjk}$ is the jth element of diagonal matrix $R_k$. $H_j$ is the jth row of matrix H. Furthermore,

$$C={\left[\begin{array}{cc}1& 0\\ -1& 0\end{array}\right]}^T$$

(24)

$$D=\left\{\begin{array}{cc}\hfill \left[\begin{array}{c}{\widehat{d}}_k+3{\sigma }_{LOS}\\ -({\widehat{d}}_k-3{\sigma }_{LOS})\end{array}\right],& ifs=LOS\hfill \\ \hfill \left[\begin{array}{c}{\widehat{d}}_k+3{\sigma }_{NLOS}\\ -({\widehat{d}}_k-3{\sigma }_{NLOS})\end{array}\right],& ifs=NLOS\hfill \end{array}\right.$$

(25)

We could get the unconstrained estimate by solving the following minimization problem. Given on a specific time-step k, ${\widehat{x}}_{k|k}^P$ is the inequality constrained estimate and $W_k$ is any positive definite symmetric weighting matrix, thus we could obtain:

$$\begin{array}{cc}{\widehat{x}}_{k|k}^P=\hfill & \underset{x}{argmin}{(x-{\widehat{x}}_{k|k})}^{\prime }{W}_k(x-{\widehat{x}}_{k|k})\hfill \\ \hfill & s.t.C{\widehat{x}}_{k|k}-D\le 0\hfill \end{array}$$

(26)

With use of fmincon in Matlab [34], above mentioned inequality constrained optimization could be easily solved. Here, we adopt a method of active set to solve the problem of inequality constraints, where we convert inequality constraints into equality constraints by treating the active set as additional equality constraints [35].

In the first step, we find the optimal estimation without consideration of inequality constraints, and we call the estimation ${\widehat{x}}_{k|k,j}^{P\ast }$, in which j denotes the number of iterations. In order to find out if the solution lies in the inequality constraint space, we ought to find the vector which we moved along to obtain ${\widehat{x}}_{k|k,j}^{P\ast }$. The equation is shown as follows:

$$\eta ={\widehat{x}}_{k|k,j}^{P\ast }-{\widehat{x}}_{k|k,j-1}^P$$

(27)

Now, let us check out that if each of our inequality constraints are satisfied. If so, we set ${\tau }_{max}=1$. Otherwise, we would tend to make the choice of maximum value of ${\tau }_{max}$ to make sure that ${\widehat{x}}_{k|k,j-1}+{\tau }_{max}\eta$ lies in the inequality constraints space. We choose our estimation as

$${\widehat{x}}_{k|k,j}^{P\ast }={\widehat{x}}_{k|k,j-1}^P+{\tau }_{max}\eta$$

(28)

Restricting the optimal Kalman gain is the solution of this problem. We find out the optimal $K_k$ for a specific time step k, which is written below

$$\begin{array}{cc}{\widehat{K}}_k^R=\hfill & \underset{K\in {\mathbb{R}}^{n\times m}}{argmin}Tr\{(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^{\prime }+K_k{R}_k{K}_k^{\prime }\}\hfill \\ \hfill & s.t.C{\widehat{x}}_{k|k-1}+K_k{v}_k\le D\hfill \end{array}$$

(29)

where $Tr(·)$ stands for the trace of given matrix.

We could make use of a means of inequality constraints optimization to work out this problem. In this paper, we solve the optimization problem by using CVX toolbox [36]. The restricted Kalman Gain is applied to get the covariance matrix of the inequality constrained estimation. We can reduce the convergence error, by taking the difference between the updated state estimations from the last two iterations calculated by the restricted Kalman Gain. The detailed algorithm flow can be referred to Algorithm 2.

Algorithm 2 Maximum Correntropy Criterion-Based Constrained Kalman Filter (MCC-CKF)

0: Initialization: 1: ${\widehat{x}}_0=E\left[x_0\right]$, $P_0=E[(x_0-{\widehat{x}}_0){(x_0-{\widehat{x}}_0)}^T]$ Prior estimation: % Compute the prior distance estimation 2: ${\widehat{x}}_{k|k-1}=F_{k,k-1}{\widehat{x}}_{k-1}$ % Compute the prior covariance matrix 3: $P_{k|k-1}=F_{k,k-1}{F}_{k,k-1}^T+Q_k$ Posteriori estimation: % Compute the best unconstrained estimate, if indicated channel state is LOS 4: if$s=LOS$then 5:   ${\widehat{x}}_{k|k}^P=\underset{x}{argmin}{(x-{\widehat{x}}_{k|k})}^{\prime }{W}_k(x-{\widehat{x}}_{k|k})$, s.t. $C{\widehat{x}}_{k|k}-\left[\begin{array}{c}{\widehat{d}}_k+3{\sigma }_{LOS}\\ -({\widehat{d}}_k-3{\sigma }_{LOS})\end{array}\right]\le 0$ 6: end if % Compute the best unconstrained estimate, if indicated channel state is NLOS 7: if$s=NLOS$then 8:   ${\widehat{x}}_{k|k}^P=\underset{x}{argmin}{(x-{\widehat{x}}_{k|k})}^{\prime }{W}_k(x-{\widehat{x}}_{k|k})$, s.t. $C{\widehat{x}}_{k|k}-\left[\begin{array}{c}{\widehat{d}}_k+3{\sigma }_{LOS}\\ -({\widehat{d}}_k-3{\sigma }_{LOS})\end{array}\right]\le 0$ 9: end if % Project above solution to the inequality constrained space 10: ${\widehat{x}}_{k|k,j}^{P\ast }={\widehat{x}}_{k|k,j-1}^P+{\tau }_{max}\eta$ % Update the posterior Kalman gain 11: ${\widehat{K}}_k^R=\underset{K\in {\mathbb{R}}^{n\times m}}{argmin}tr\{(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^{\prime }+K_k{R}_k{K}_k^{\prime }\}$, s.t. $C{\widehat{x}}_{k|k-1}+K_k{v}_k\le D$ % Update the posterior covariance matrix 12: $P_{k|k}=(I-K_{k}H)P_{k|k-1}{(I-K_{k}H)}^T+K_k{R}_k{K}_k^T$ 13: $k+1\to k$, go to step 2.

5. Experiments and Results Analysis

The performance of our proposed algorithm could be evaluated by employing the TOA-based positioning system. Practical experiment is carried out in a typical indoor scenario to verify the effectiveness of our proposed method. In this section, we firstly give a brief introduction about the experiment setup, and then present a detailed analysis and discussion on corresponding experiment results.

5.1. Settings

A practical indoor positioning system based on the UWB-TOA, developed based on DW1000, have been applied to perform the field testing. The experiment scene was shown in Figure 4, which is located in the hall (10 m × 17 m × 4 m) inside the building of School of Computer and Communication Engineering, University of Science and Technology Beijing (USTB). Four reference nodes are respectively located as $(1,1)$, $(9,1)$, $(1,16)$ and $(9,16)$. The experimenter is asked to periodically walk along a rectangular trajectory within the region. In the experiment, we have used the TOA node in our localization system, which consists of a 32-bit RISC processor (STM32F103) and an on-board UWB module DWM1000. The experimenter holds the TOA node in hand. Thus, the NLOS condition was mainly resulted from the human body and the obstacle between the target node and the base stations. Empirical measurements have been conducted in the test filed, and totally 1549 times of localization were conducted. During the whole experiment process, about 6196 items of estimated distances are collected, and 1549 items of coordinates are derived to satisfy the ranging and localization accuracy.

5.2. Ranging Error Optimization Analysis

The statistical results of ranging errors in the experiment are firstly considered in this section. We compared the original distance ranging errors, the errors filtered by KF, MCC-KF and MCC-CKF, respectively. The mean and variance of ranging error in practical use case are summarized in

Table 1. The quantized distribution parameters, as well as means and variances, of TOA estimated distances in practical use case.

Initialization:	Measured Error	KF	MCC-KF	MCC-CKF
Mean (m):	1.13	0.86	0.45	0.04
Variance (m2):	0.67	0.37	0.58	0.35

.

The ranging distance error’s distribution could be seen in Figure 5. Figure 5a shows the original distance measurement results, from which we can see two peaks of Gaussian distribution, respectively belonging to LOS and NLOS distance errors. Correspondingly, Figure 5b presents the ranging error distribution when processed by proposed MCC-CKF method, which basically accords with White Gaussian noise distribution, which could also be refered to Table 1. Thus, we can conclude that with the use of proposed MCC-KF method, the Colored Gaussian noise casued by LOS/NLOS conditions could be significantly mitigated.

Figure 6 shows the cumulative distribution of original distance errors and the results filtered by KF, MCC-KF and MCC-CKF, from which we can see that proposed MCC-CKF shows much better performance than the others. The quantized distribution parameters, as well as means and variances, of above mentioned TOA estimated distances, are detailed in Table 1.

5.3. Positioning Error Optimization Analysis

Similar to the distance ranging error results, the proposed MCC-CKF still works toward localization of moving targets. With the use of least squared localization method, the localization errors vary with the distance ranging filtering method. KF and MCC-KF show some superiorities on the whole, but there is still some disturbance in the small error range. It is possibly because KF does not work well with non-White Gaussian noise and MCC-KF diverges due to the lack of constraints. Proposed MCC-CKF method shows the best performance in localization experiments, whose mean and variance are also the smallest among the comparison methods, as shown in

Table 2. The quantized distribution parameters, as well as means and variances, of TOA estimated distances in practical use case.

Initialization:	Measured Error	KF	MCC-KF	MCC-CKF
Mean (m):	5.41	4.16	3.22	1.28
Variance (m2):	45.44	11.17	0.96	1.25

.

6. Conclusions

This paper presents a new distance mitigation algorithm based on constrained Kalman filter under maximum correntropy criterion (MCC-CKF) to enhance the performance of TOA distance ranging. Proposed method are proved to be able to decrease the ranging errors resulting from non-White Gaussian noises in the indoor scenarios. The performance of our algorithm was compared with the other traditional and state-of-the-art TOA distance mitigation algorithms, such as Kalman filter and maximum correntropy based Kalman filter (MCC-KF), with implementation of practical experiments in typical indoor scenarios. Significantly, results show that our method has significant advantages in improving distance ranging and localization accuracy.

It is worth mentioning that, our current research is limited to the localization and tracking of a single target. In future studies, we intend to put more effort in sequential and cooperative targets tracking using modified MCC-CKF methods.