The flowchart of our algorithm is depicted as
Figure 1. The distance value
is measured by the sensor nodes and the position estimation
is calculated through this filter. Firstly, in order to mitigate the driving noise and initially correct NLOS errors, we utilize GKF to pre-process the measurement. Secondly, we calculate the residual value, which is the difference between the filtered result and the measurement, and save it in the storage module. Thirdly, the historical residuals over a period of time and current residual are utilized to train GMM. Double Gaussian functions are used in this paper to fit these residuals and two clustering centers can be obtained. The degree of membership (probability of propagation condition) is easily obtained through calculating the Euclidean distance from the current residual to the cluster centers. Fourthly, UKF with a relatively large covariance matrix is used to further process the weak NLOS errors after the strong NLOS errors are partially eliminated by GKF. Finally, the filtering results of multiple filters (GKF and UKF) are mixed according to the degree of membership (probability of propagation condition) calculated in a mixture Gaussian fitting step. The maximization likelihood algorithm (ML) is used to estimate the coordinate.
5.2. Grey Kalman Filter (GKF)
GKF is based on the KF framework. The difference from KF is that GM(1,1) is used in the prediction step, where, ‘G’ means grey, ‘M’ means model and ‘(1,1)’ denotes the established differential equation is first-order and has only one variable. The generation value of GM (1,1) is directly used as the predicted value of the next time step in KF. Furthermore, the state transition matrix is updated continuously based on the motion characteristic of targets. The new state transition matrix is applied to the updating process of the covariance matrix and state estimation at the current time step. Therefore, GKF could avoid the random direction and speed of the targets which cause the process noise in the state equation. Due to the prediction step being dependent on the historical filtered values, GKF could also mitigate NLOS errors partially.
The entire algorithm is elaborated as following steps:
Step 1: Building the original distance series.
We define the
n-th element of the original series of
i-th beacon node at time step
k as follows:
Initial the original time series as follows:
where
is the original distance series,
denotes the filtered distance at time step
k,
is the constant series length of predicting points. The predicting points are the historical GKF filtered distances and we can choose different series length applying to prediction step.
For example, if we set
, we obtain original distance series as follows:
where the
are the prediction points. We would use these points to predict the value of the next time step. The number of prediction points must be chosen reasonably. Too many prediction points will bring in lots of historical values that are too far for the present moment so that the premature value can be regarded as noise. However, too few prediction points also cannot be accepted because they fail to provide enough information for prediction [
33]. The paper [
33] has verified that the appropriate number of prediction points is about 8 when the sampling period is 1 s. In actual projects, engineers need to set a reasonable prediction points number according to the sampling frequency. In the experimental part of this paper, the sampling interval is 1 s so 8 prediction points are set. At the first 8 points, because the prediction points are too few for grey prediction (
), KF is used to initial the time series. Thereafter, the number of historical values is enough for adopting GKF to filter the measured distance.
Step 2: Calculate the first-order accumulative series as Equation (15) through adding all the past prediction points for every element in the series .
We define the element of accumulative series as Equation (14):
Calculate the first-order accumulative series as follows:
where
denotes the accumulative series that will be utilized to predict the distance value at the next time step.
Step 3: Assume that the
n-th element
in series
meets the first-order ordinary differential equation as (16).
where
is development coefficient,
is grey effect coefficient in the grey theory. As long as
and
are obtained, the predicted value
can be solved.
Step 4: Calculate the mean of the accumulated series between every two prediction points to obtain matrix
and generate the constant term vector
as follows.
Step 5: Calculate
and
through the least-square method.
Step 6: Substitute
and
to the differential Equation (16) and calculate the solution
.
We can use this model to estimate
by substituting
to Equation (19) and obtaining:
Step 7: Build the predicted series
as (22) through cumulative subtraction of the above results as Equation (21) and the prediction of the next time step
can be calculated as Equation (23).
is directly used as the value of the next time in the prediction step in KF. It plays a significant role in eliminating NLOS errors. In the case of NLOS, the measured distance is much longer than the real distance. Therefore, in the continuous time series of distance, severe NLOS errors will cause the mutation point. But the predicted value of GM (1,1) is highly correlated with previous filtered values, and large fluctuations will be weakened, which can effectively solve this issue.
In order to simplify the expression, set parameter
and
and rewrite the predicted value as Equations (23) and (24) [
31].
Similarly, for speed series we can also get its prediction as Equation (26) using the same method of distance prediction.
where
is the accumulative series value of speed.
and
are used to distinguish the parameter
and
in distance prediction.
Step 8: Obtain the predicted state at the next time step.
where
is the distance predicted in (25) and
can similarly be estimated.
Step 9: Update the state transition matrix
as Equation (28) [
33].
This step has an important effect on the mitigation of process noise caused by random direction and speed, since the state transition matrix is updated at every time step according to the target movement characteristics.
Step 10: Kalman Filter.
Calculate measurement residual
and Kalman gain
:
Obtain the position estimation through estimation Equations (32) and (33).
In order to show our algorithm clearly, we give the pseudo code in this paper as Algorithm 1.
Algorithm 1. The pre-processing for the measurements based on grey Kalman filter (GKF) |
Input: |
Output: |
begin |
fori = 1: M do |
for do |
|
|
for j= do |
|
|
|
end for |
|
|
|
|
|
|
|
|
|
end for |
for do |
|
|
|
|
|
|
end for |
end for |
end |
In order to follow the pseudocode in Algorithm 1 and provide a much clearer exposition of GKF, a more detailed flowchart (
Figure 2) is prepared corresponding to the GKF of
Figure 1.
5.3. Mixture Gaussian Fitting (MGF)
A soft NLOS identification method is proposed based on GMM clustering algorithm. By contrast with from the hard decision method such as Wylie, Rwgh [
4] and the hypothesis testing discriminant method [
11], our method give the probability of each condition, which we regard this method as a soft NLOS identification way.
The effectiveness of mixture Gaussian fitting (MGF) is dependent on the corrected distance in pre-processing step. GKF can mitigate the process noise and alleviate NLOS errors partially. Therefore, the filtered distance of GKF can be used to calculate the residual which is the difference from the measurement. So the residual value here is essentially a NLOS error estimated. In general, it is impractical to obtain the NLOS error distribution parameters, but in this paper, GMM is used to approximate the PDF of NLOS errors and gives the probability of each condition.
Step 1 (Calculate the residual):
We firstly calculate the distance residual between the measured value and the filtered value of GKF and store it to the memory module.
Step 2 (Fit the mixture Gaussian distribution):
We use the historical residuals and the current residual to fit the mixture Gaussian distribution. The distribution of mixture Gaussian is as follows:
where
is the unknown Gaussian parameter vector, the distribution parameter of each distribution
.
and
are the mean and the standard variance,
,
is the probability of the
j-th Gaussian function. In this paper, two Gaussian functions are used to fit these residuals for every beacon node. So
G is set to 2. One has a relatively small mean value, which can represent the LOS condition or the weak NLOS condition, the other has a relatively large mean value which indicates the strong NLOS condition.
Take the logarithm to (35) to obtain the likelihood function
. [
11]
We use expectation maximization (EM) algorithm [
11] to estimate the unknown parameter
through maximizing the log-likelihood function
. The EM algorithm continuously iterates and adjusts the parameter
to achieve the maximum value of the likelihood function. In practice, a termination condition is usually set as Equation (37).
where the parameter
is a small threshold which is 1 × 10
−5 in this paper and the parameter
r is the iteration times. The basic process of the EM algorithm consists of E-step and M-step.
E-step. Calculate the probability that the residual value
belongs to the
j-th Gaussian distribution as Equation (38).
M-step. Calculate the partial derivative of the Equation (36) and let the derivative function equal to zero. Solve unknown parameters of Gaussian distribution as Equations (39)–(41).
Step 3 (Calculate the degree of membership in each condition)
After using two Gaussian distribution functions to fit all the residual value, we utilize the Euclidean distance between current residual and the clustering centre
to calculate the degree of membership in each condition. We define the membership function as (44) and (45):
To illustrate the meaning of the membership function clearly, we will provide an example when NLOS errors obey
and we choose the fitting result at one time step as
Figure 3.
We can observe that the residual value at this time step is 3.3 m (blue dot) and the means of Gaussian functions are 1.0 m (red dot) and 4.3 m (green dot) respectively. We have successfully fitted the NLOS errors distribution with two Gaussian functions through the EM algorithm. It can be observed that the fitted result is approximately in line with the real condition. The sensor noise obeys zero-mean white Gaussian distribution which is very close to the red curve while the NLOS errors obey
which is approximately with consistent with the green curve. The Euclidean distance between the residual and the red dot is (3.3 − 1.0) m = 2.3 m. Similarly, the Euclidean distance between the residual and the green dot is (4.3 − 3.3) m = 1.0 m. It is not difficult to understand that the farther away from the mean of a specific Gaussian distribution, the lower the probability that it belongs to this Gaussian distribution. In this example, the membership of the residual value to the red Gaussian distribution is 1.0/(2.3 + 1.0) m = 0.3 and the membership of green Gaussian is 0.7. In order to display our identification algorithm clearly we give the description of MGF as Algorithm 2.
Algorithm 2. Pseudocode of mixture Gaussian fitting (MGF) |
Input: |
Output: |
begin |
fordo |
for do |
|
|
|
|
|
|
|
|
|
end for |
end for |
end |
5.4. Unscented Kalman Filter (UKF)
To simplify the subsequent analysis, we rewrite the system Equations (8) and (9) using symbol
and
:
Step 1 (Initialization)
Calculate the expectation of the processed state and the covariance by GKF.
where
denotes the state vector after being pre-processed by GKF.
Step 2 (Generate the sigma points)
The dimension of sigma points is 2
L + 1 where
L is the dimension of the state vector [
4]. In this paper,
L = 2. The generated sigma points are expressed as follows:
where
is the distribution parameter of UKF.
Step 3 (Prediction)
Transform the sigma points as Equation (51) based on the non-linear state Equation (46). And calculate the weighted average of these sigma points as Equation (52).
where
is the weighting factor.
Thereafter, predict the observation value using nonlinear measurement equation [
4].
Use the weighting factor to obtain the average value of
.
Step 4 (Update)
Update the matrix of state covariance as Equation (57) and the measurement covariance as Equation (58) [
4].
Calculate the Kalman gain as Equation (59), the updated state vector as Equation (60) and the covariance matrix as Equation (61).
The final filtered distance of UKF is denoted as .