Abstract
Ultrawideband (UWB) is well-suited for indoor positioning due to its high resolution and good penetration through objects. The observation model of UWB positioning is nonlinear. As one of nonlinear filter algorithms, extended Kalman filter (EKF) is widely used to estimate the position. In practical applications, the dynamic estimation is subject to the outliers caused by gross errors. However, the EKF cannot resist the effect of gross errors. The innovation will become abnormally large and the performance and the reliability of the filter algorithm are inevitably influenced. In this study, a robust EKF (REKF) method accompanied by hypothesis test and robust estimation is proposed. To judge the validity of model, the global test based on Mahalanobis distance is implemented to assess whether the test statistical term exceeds the threshold for outlier detection. To reduce and eliminate the effects of the individual outlier, the robust estimation using scheme III of the Institute of Geodesy and Geophysics of China (IGGIII) based on local test of the normalized residual is performed. Meanwhile, three kinds of stochastic models for outliers are expressed by modeling the contaminated distributions. Furthermore, the simulation and measurement experiments are performed to verify the effectiveness and feasibility of the proposed REKF for resisting the outliers. Simulation experiment results are given to demonstrate that the outliers following all the three kinds of contaminated distributions can be detected. The proposed REKF can effectively control the influences of the outliers being treated as systematic errors and large variance random errors. When the outliers come from the thick-tailed distribution, the robust estimation does not play a role, and the REKF are equivalent to the EKF method. The measured experiment results show that the outliers will be generated in the nonline-of-sight environment whose impact is abnormally serious. The robust estimation can provide relatively reliable optimized residuals and control the influences of the outliers caused by gross errors. We can believe that the proposed REKF is effective to resist the effects of outliers and improves the positioning accuracy compared with least-squares (LS) and EKF method. Moreover, the adaptive filter and ranging error model should be considered to compensate the state model errors and ranging systematic errors respectively. Then, the measurement outliers will be detected more correctly, and the robust estimation will be used effectively.
1. Introduction
High accuracy position information is of great importance in location-based service (LBS). Due to a large bandwidth, ultrawideband (UWB) can obtain high-resolution distance estimation and enables reliable distance estimation [1]. Therefore, UWB is well-suited for indoor positioning applications. The observation model of UWB positioning is nonlinear. The approximate solutions can be obtained iteratively based on Taylor's expansion of nonlinear distance equations [2, 3]. As a standard method for solving general nonlinear equations, the Gauss–Newton iteration is efficient and has a linear convergence rate for points close to the solution [4]. However, in this procedure, only the measurements at the discrete epoch are employed to estimate the positions. This approach wastes the useful state model information which describes the dynamic process. The Kalman filter (KF) has been applied in the area of dynamic positioning. It makes full use of the information of the state model. When the process and measurement noises are Gaussian distribution, it can be proven that KF is unbiased and consistent, and it is optimal in the linear system [5]. In practical applications, most of the dynamic state is nonlinear system, and many nonlinear filter algorithms are developed, such as extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Particle Filter (PF) [6]. As a standard filter algorithm of nonlinear estimation, EKF simply linearizes the nonlinear observation equation. The distribution is propagated through the first-order expression of the nonlinear system. Because of neglecting the high order items of the expansion, the linearization error is introduced [7].
In practical applications, an unbiased estimation of the positioning result will be expected. On one hand, the estimation of filter algorithms is affected by the model errors. When the dynamic system model is not accurate, the performance and the reliability of the filter algorithms are inevitably influenced. To compensate the model errors, one common way is to introduce the corresponding parameters into the state and observation functional model [8]. However, this approach may introduce wrong or insufficient parameters. Besides, too many input parameters will lead to the high dimension state vector, and this may result in the increasing computation load and rank defect model [9]. An adaptive fitting method for systematic errors of the observations and kinematic model errors is presented to resist the influences of systematic errors on the estimated states of navigation. The systematic errors are fitted with a mean or a weighted mean by using the residuals of observations and residuals of predicted states within a chosen time window [10, 11]. Another way for compensating the model errors is to introduce suitable covariance matrix in the stochastic model. However, good priori knowledge of the process and measurement information is difficult to obtain, and an ineffective covariance matrix will cause greater error or even filtering divergence. An innovation-based adaptive Kalman filter for integrated navigation is developed, and the adaptive Kalman filter is based on the maximum likelihood criterion for the proper choice of the filter weight [12]. An adaptive fading Kalman filter based on Mahalanobis distance is proposed, and this method has a stronger tracking ability to the true state than the standard Kalman filter in the presence of modeling errors [13].
On the contrary, the positioning estimation is subject to different environmental error factors, including signal blockage, multipath, and thermal noise. According to the cause of the error, it is divided into gross error, systematic error, and random error. The gross error is very different from the assumed stochastic model and is a small probability event. The systematic error which is exact value or changes with the law mainly affects the accuracy of the measurement. The random error occurs randomly and obeys the statistical model. It has a certain influence on the precision. The data is contaminated by the outliers, which is non-Gaussian distribution and heavy-tailed distribution [14]. Two categories of advanced techniques have been developed for treating the observations contaminated by outliers, one is the outlier detection method based on the statistical test and the other is the robust estimation method [15]. Statistical test or model errors, outliers, and biases usually consists of detection, identification, and adaptation (DIA) step which is an important diagnostic tool for data quality control [16]. The DIA method combines parameter estimation with hypothesis test, and parameter estimation is conducted to find estimates of the parameters one is interested in and testing is conducted to remove any biases that may be present [17]. Under the assumption with Gaussian distribution, the weighted sum of squared residuals follows the noncentral Chi-squared distribution for the global model test [18, 19]. To screen each individual observation for an outlier, the data snooping based on the local model test is implemented [20, 21]. A robust Kalman filter scheme based on the Chi-square test is proposed to resist effectively the influence of observation error including the outliers in the actual observations and the heavy-tailed distribution of the observation noise, so robustness can be achieved [22]. Different robust filter approaches are adopted for solving the measurement outliers. A robust Kalman filter based on the m-interval polynomial approximation (MIPA) method for unknown non-Gaussian noise is proposed, and the MIPA Kalman filter is computationally feasible, unbiased, more efficient, and robust [23]. The robust Kalman filter is obtained by Bayesian statistics and by applying a robust M-estimate for rank deficient observation models. The outliers are down-weighted not only in the observations but also in the updated parameters [24]. A general estimator for an adaptively robust filter is developed. This method can not only resist the influence of outlying kinematic model errors but also controls the effect of measurement outliers [25]. An adaptive method with fading memory and a robust method with enhancing memory is proposed in the Kalman filter. The method has the ability of strongly tracking the variation of the state and is insensitive to gross errors in observation [26]. A robust version of the Kalman filter to address process modeling errors in the linear system with rank deficient measurement models is developed using the generalized maximum likelihood estimator (M-estimator) [27]. A robust unscented Kalman filter based on the generalized M-estimation is proposed to improve the robustness of the integrated navigation system. The filter has the ability to suppress the effects of outliers from both the dynamic model and measurements on dynamic state estimates [28].
In this study, the discussion will be restricted to the problem of resisting the observations contaminated by outliers. A robust EKF (REKF) method is proposed accompanied by hypothesis test and robust estimation. The main feature of this proposed method consists of two parts. One is the global test based on Mahalanobis distance for outlier detection. The hypothesis test is carried out for testing the model. The other is the robust estimation using scheme III of the Institute of Geodesy and Geophysics of China (IGGIII) based on the local test of the normalized residual. The outliers are resisted by the equivalent weights according to the discrepancy between the model and the measurements.
The remainder of this paper is organized as follows. In Section 2, the nonlinear least-squares (LS) solution of overdetermined distance equations and the extended Kalman filter are introduced. In Section 3, the REKF method is proposed. Three kinds of contaminated distributions are illustrated. In Section 4, the effectiveness of proposed REKF is verified and analyzed by the numerical examples. In Section 5, the conclusions are summarized.
2. The Extended Kalman Filter
The distance equation of UWB positioning is given as [29]where denotes the observation distance between the tag and anchor; denotes the observation vector; is the Euclidean distance; represents the corresponding random error; and is the known coordinate of anchor. is the coordinate of tag.
When the state model is not considered and the nonlinear equations are overdetermined, the nonlinear solution of distance equation is to find , where , in which represents the residual vector [30].
The approximate solutions can be obtained iteratively based on the linearized distance equations. With a rough initial value, we can obtain the LS solution by the way of Gauss–Newton method with iterations, and the LS solution can be written aswhere is the Jacobian matrix of distance equations and is the direction cosine vector from the tag to the anchor.
The Gauss–Newton method only includes the first-order Taylor expansion of distance equations. The linearization of the positioning observation model results in biased LS estimators. The bias comes from neglected higher order terms, which can be regarded as a systematic error. It will be realized that the parameter estimator tends to be unbiased with a sufficiently small relative ranging error or a good positioning configuration. This makes the bias totally negligible.
When the state model is considered, the state and observation models of the nonlinear system are expressed aswhere and represent the state vector of system at epoch k − 1 and k; is the observation vector of system at epoch k; and is the nonlinear state transition function and observation function; and are process noise and observation noise, both of them are uncorrelated Gaussian white noise, where and are corresponding covariance.
By linearizing the state model and observation model, we combine the linear dynamic system and first-order observation equations, and the model of EKF can be expressed as [9]where and are the state transition and observation matrices, respectively.
The EKF implementation can be written as follows.
The predicted state and covariance matrix can be calculated as
The Kalman filter gain can be written as
The estimated state vector and posterior covariance matrix can be expressed as
3. The Robust Extended Kalman Filter Accompanied by Hypothesis Test and Robust Estimation
The UWB positioning data influenced by outliers do not fulfill the assumed stochastic model of extended Kalman filter, and it can be a potential problem for parameter estimation. A robust extended Kalman filter should be applied to resist the effects of measurement outliers. Firstly, we perform the global test based on Mahalanobis distance for outlier detection. Then, the robust estimation using the IGGIII scheme based on local test of the normalized residual is implemented. Furthermore, the stochastic model for outliers in the measurement process is expressed.
3.1. The Global Test Based on Mahalanobis Distance for Outlier Detection
The global model test is used to detect discrepancies between the measurements and the functional and stochastic models [31]. For outlier detection, a judging index is defined as the square of the Mahalanobis distance from the observation to its prediction, and the hypothesis test is performed by treating the judging index as the test statistic [22]. The test statistic term can be written aswhere denotes the Mahalanobis distance, is the test statistic term, and is the innovation vector.
The hypothesis test can be carried out for testing the model. The null hypothesis is that the stochastic model of parameter estimation is correct, and the observations do not contain gross error and obey Gaussian distribution. The alternative hypothesis is that there is at least one measurement outlier caused by gross error in the data. Then, the probability of null hypothesis being rejected satisfieswhere is predetermined score by the chosen level of significance based on Chi-square distribution table. The small significance level is chosen, the more frequently we will incline to accept the null hypothesis. A good hypothesis test for outlier detection would minimize the probabilities of decision error. The test statistic term can be a measure of significance.
When the innovation is normal, the test statistic term is smaller than the threshold of the test statistic, and should be Chi-square distributed with the dimension of the observation vector as the degree of freedom (DOF). The null hypothesis is confirmative. If is larger than , the value of test statistic will fall in the right tail area of the distribution, and the null hypothesis will be rejected. We can believe that the outliers in the observation do occur and the stochastic model of observation error is not correct. The test statistic term will follow a noncentral Chi-square distribution with noncentral parameter.
3.2. The Robust Estimation Using the IGGIII Scheme Based on Local Test of the Normalized Residual
If the global test rejects the null hypothesis, it shows that the model does not conform with the specifications. We should find and eliminate the individual outlier, and the local test is performed. For the outlier detection, the null hypothesis is that there is no observation affected by outliers. The alternative hypothesis is that there is an outlier in one known observation [32]. The normalized residual of observation in the observation vector constructed as a test statistic term is given aswhere is the prediction residual, is standard deviation of residual, is standard deviation of observation, and is the cofactor matrix corresponding to the prediction residual.
If the observations are contaminated by the outliers, the covariance should be inflated. To control the influence of the outliers, the equivalent weight elements based on the IGGIII which are established based on the M-estimation are applied [33]. In fact, some existing equivalent weight functions can be used to calculate the equivalent weight element. The robust gain matrix factor of Kalman filter can be written as [34]where and are two constants, which are usually determined based on the objective requirement, and represents the th element in the state vector.
3.3. The Stochastic Model for Outliers
The stochastic model is expressed by modeling the distribution and introducing a prior covariance matrix of the observations. We assume the measurement errors obey the normal distribution which is an independent Gaussian variable with zero mean and equal variance. The probability density function (PDF) readswhere is the random variable, is the expectation of the observation which reflects the average value of random variables, and is the standard deviation which indicates the dispersion degree of random variables.
In general, outliers require special attention in data analysis, the outliers are most often caused by gross errors and gross errors are most often caused outliers. The outliers are the result of two mechanisms. One mechanism is that the observations errors obey the normal distribution, but the gross errors follow a different distribution. The gross errors which contribute to the outliers can be treated as systematic errors and large variance random errors. Another one is that both the observations’ random errors and gross errors come from the thick-tailed distribution. The outliers come from the tails of the distribution [31]. Note that an extreme observation may not be an outlier, and it may instead be an indication of skewness of PDF. Three kinds of contaminated distributions can be expressed as follows.
If the observations are contaminated by the gross errors, the outliers are treated as systematic errors, and the PDF can be obtained as location-contaminated normal distribution [35]:where is the gross errors, which is a kind of gross error generated by adding a bias to the random variable, and is the probability of gross errors in the total observations.
If the outliers are treated as random variables with large variance, the PDF is given as scale-contaminated normal distribution [36]:where is the variance of gross errors, and this kind of gross error is obtained by increasing the variance in the random variable.
The Laplace distribution is introduced to express the nonnormal distributed gross error. The Laplace distribution has the heavier tail than the normal distribution. It is also called the double exponential distribution. We can consider it as a result of variance inflation. The PDF can be expressed as [37]where is the scale parameter of the model, and the kurtosis of this distribution is 3.
4. Numerical Examples
In this section, the simulation and measurement experiments are carried out for evaluating the performance of proposed REKF method for UWB Positioning. The LS procedure is performed first to provide the initial state estimates and then the state estimates are updated by the filter. Meanwhile, the individual positioning results of the three methods LS, EKF, and REKF are counted and compared.
4.1. Simulation Verification
In order to realize positioning application, the anchors are distributed in four upper corners of space. The performance of the REKF is analyzed based on three sets of tests with the three kinds of stochastic models for outliers. The positioning accuracy are obtained by calculating the RMS (root mean square). The simulation experimental scene and the test trajectory are shown in Figure 1.
(a)
(b)
For the first set of test, the outliers for this stochastic model are biased, and we can simulate the observations with the systematic errors and random errors. The systematic errors are regarded as gross errors, and they are randomly added to certain epochs. The significance level for testing the model is predetermined 0.01. The probability of gross errors in the total observations is set to 0.05.
To check the model error for outlier detection, the hypothesis test based on Mahalanobis distance is implemented. It is used to assess whether the outliers are in the observation. The result of test and the threshold are compared in Figure 2. We can see that the test statistics are relatively small in most epochs, but some epochs still exceed the threshold. The outliers are outstanding and can be easily detected. It indicates that the outliers in these observations do occur. The statistical results of positioning error for all epochs are shown in Figure 3. The statistical results of mean positioning error are given in Figure 4. It can be observed that the LS and EKF estimation are not effective and the observations are contaminated by the gross errors. Besides, EKF estimates are better than LS due to considering dynamic model information. The REKF proposed in this paper consists of the hypothesis test and robust estimation. The hypothesis test detects the outliers in the observation. The robust estimation can provide reliable residuals and control the influences of the outliers. We can believe that the REKF is effective to resist the effects of outliers being treated as systematic errors.
For the second set of test, the outliers for this stochastic model are due to large variance random errors which can be seen as gross errors. We can simulate a certain percentage of large variance random errors in the total observations. Similarly, the significance level for testing the model is predetermined 0.01. Meanwhile, four sets of scale-contaminated normal distributions for outliers are conducted: (Case 1), (Case 2), (Case 3), and (Case 4).
As shown in Figure 5, the hypothesis test is carried out to compare the test statistic term of four cases. We can see that the number of epochs exceeding the threshold in Case 4 is the most and the number of epochs in Case 1 is the least. It makes us known that the larger the covariance of the gross errors, the more the number of outliers. The larger the proportion of gross errors in the total observations is, the more the number of outliers will be.
The statistical results of positioning error for all epochs in four cases are shown in Figure 6. The statistical results of mean positioning error in four cases are given in Figure 7. The positioning performance of Cases 1 to 4 can be ranked from good to poor. It is shown that the gross errors with larger covariance greatly affect the positioning results of a few epochs when the proportion of gross errors in the total observations is same. Comparing Case 1 and Case 3 or compare Case 2 and Case 4, it can be observed that those gross errors with larger covariance have a great influence on the mean positioning error of LS and EKF, but the mean positioning error of REKF in two cases are very close. It indicates that the frequency that robust estimation is called is consistent. REKF can resist the influence of gross errors equally and effectively in despite of different covariance. Furthermore, the larger proportion of gross errors in the total observations will affect the positioning results of more epochs when the covariance of the gross errors is equal. Comparing Case 1 and Case 2 or compare Case 3 and Case 4, it can be seen that those gross errors that account for a larger proportion of the observations have corresponding influence on the mean positioning error of LS, EKF, and REKF. It can be known that the robust estimation is called more times in REKF in Cases 2 and 4. This also increases the positioning error of some epochs and leads to the larger mean positioning error of REKF in Cases 2 and 4.
(a)
(b)
(c)
(d)
For the third set of test, the outliers for this stochastic model come from the tails of the distribution. We can simulate the observations with Laplace distribution. Both the random errors and gross errors come from the distribution. In order to detect more outliers, the significance level for testing the model is predetermined 0.05.
Figure 8 represents the hypothesis test for outlier detection based on Mahalanobis distance. We can see that the test statistics of some epochs exceed the threshold and the outliers exist in the observations. Figure 9 shows the statistical results of positioning error for all epochs. Figure 10 illustrates the statistical results of mean positioning error. It can be seen that the positioning error of LS is also the largest, and the REKF works as well as EKF. It demonstrates that the robust estimation does not play a role in resisting gross errors and the REKF does not reduce the effects of the thick-tailed distributed outliers.
Through the above analysis, we can verify that the performance of the proposed REKF is superior or not inferior to EKF under the condition that the outliers exist. Moreover, we note that the test statistics of initial epoch of the three sets of tests exceed the threshold. It can be known that it is the condition that reaches asymptotic stability of Kalman filter algorithm.
4.2. Measurement Verification
Figure 11 represents the UWB sensor network positioning system based on the DecaWave Mini2016 suite, including the anchor and tag nodes. By determining the time of flight (TOF) of signals travelling between the anchor and tag, the ranging measurements in these transceivers is performed based on the two way ranging (TWR).
(a)
(b)
In the measurement experiment, the test trajectory and the experimental scene are shown in Figure 12. The experimental scene is an empty hall of 10 meters by 18 meters. The anchor nodes are placed on a tripod of the same antenna height. The tester walked along the predetermined trajectory.
(a)
(b)
(c)
Three sets of tests are conducted as follows. In Test 1, the tag node is mounted on the top of the helmet, which is worn on the head of tester in a dynamic situation. Compared with Test 1, during the testing process, the relevant external personnel enter the experiment field and walk randomly for interfering signal in Test 2. In Test 3, the tag node is on the waist of tester, and there are no relevant personnel entering the experiment field.
As shown in Figure 13, the hypothesis test is implemented for outlier detection. In Test 1, we find that although there is no external interference, and there are still some epochs whose test statistic terms exceed the threshold. Combined with the statistical position results which are shown in Figure 14, we can see that the test statistic terms at the corners are relatively large. The main cause is that the state model is not reasonable to describe the dynamic process. This leads to larger prediction residuals, and the robust estimation of the proposed REKF method will be invoked to resist the mistaken gross errors. This not only does not have the robust effect but also increases the positioning error. In this case, the adaptive filter is generally used to compensate the state model errors. Moreover, during the positioning process, the positioning estimation is subject to different error sources, including gross errors, systematic errors, and random errors. Although the hardware devices adopt the principle of TWR without a common time reference, the clock drift and offset still affect the ranging systematic errors. Meanwhile, the positioning system is prone to become ill-posed. The performance and the reliability of the positioning algorithms are inevitably influenced. It causes the residuals of the filtering algorithm increasing, and the test statistic terms of a few epochs exceed the threshold. In practical applications, the systematic errors should be modeled, and its influence should be weakened as far as possible.
(a)
(b)
(c)
(a)
(b)
(c)
In Test 2, the test statistic terms of some epochs have exceeded the threshold similar to Test 1, two of which are particularly large and outstanding. It makes us know that the signal transmission between the anchor and the tag is significantly affected by external personnel in these two epochs. Then, the gross errors are generated due to the influence of the nonline-of-sight (NLOS) propagation. Meanwhile, the position statistical results indicate that the proposed REKF improve the positioning accuracy compared with EKF and LS. The robust estimation can provide relatively reliable optimized residuals and control the influences of the outliers caused by gross errors. In Test 3, although there is no external influence, the tag wearing position will influence the signal propagation throughout. Compared with Test 2, its impact is abnormally serious and the generated gross errors are very large. The outliers are outstanding and can be easily detected. Similarly, it can be observed that the robust estimation is carried out to reduce the prediction residual. We can believe that the REKF is effective to resist the effects of outliers.
5. Conclusions
The main goal of this paper is to verify the effectiveness of proposed REKF. It provides a reference to realize the reliability of filter algorithm applied to UWB positioning. The LS estimator has been widely applied in kinematic positioning. This method can resist the dynamic model error. However, it only determines the discrete position based on the observation model, but wastes the dynamic model information. As a generalization of the LS estimator, EKF obtains the better positioning performance by solving the linear dynamic system and observation equations. However, both LS and EKF which minimize the sum of the residuals are sensitive to the outliers. The performance and the reliability of the filtering algorithm are inevitably influenced when the measurements are contaminated by the outliers. The proposed REKF accompanied by hypothesis test and robust estimation is applied to process the observations being contaminated by the outliers. In the procedure, the outliers are detected by the hypothesis test, and then the outliers are controlled by the robust estimation based on the robust equivalent weights. It will be realized that the proposed REKF has better performance when the outliers are the systematic errors and large variance random errors. The REKF works as well as the EKF when the Laplace distribution accounts for the outliers. Although the outliers existing in the observations are effectively detected, the robust estimation does not play a role. The REKF performs not well in the thick-tailed distributed outliers. For the thick-tailed distributed outliers, LS adjustment is not suitable. Furthermore, in practical positioning services, the measurements are complex due to the different error sources. It is necessary to perform quality analysis and preprocessing on the raw observations. The better performance can be obtained.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare no conflicts of interest.
Acknowledgments
This work was supported by Youth Program of National Natural Science Foundation of China (41904029); Scientific Research Project of Beijing Educational Committee (KM202010016009); and National Key Research and Development Program of China (2016YFC0803103).