Fault-tolerant relative navigation based on Kullback–Leibler divergence

Abstract

A fault-detection method for relative navigation based on Kullback–Leibler divergence (KLD) is proposed. Different from the traditional χ ²-based approaches, the KLD for a filter is following a hybrid distribution that combines χ ² distribution and F-distribution. Using extended Kalman filter (EKF) as the estimator, the distance between the priori and posteriori data of EKF is calculated to detect the abnormal measurements. After fault detection step, a fault exclusion method is applied to remove the error observations from the fusion procedure. The proposed method is suitable for the Kalman filter-based multisensor relative navigation system. Simulation and experimental results show that the proposed method can detect the abnormal measurement successfully, and its positioning accuracy after fault detection and exclusion outperforms the traditional χ ²-based method.

Introduction

To guarantee the safety and efficiency of systems, many applications, such as intelligent transportation systems and location-based services, ¹ are very dependent on reliable and accurate relative positioning solutions. ^2,3 The state-of-the-art relative navigation is based on the carrier-phase differences of global navigation satellite system (GNSS) observations between nodes (e.g. carrier-phase differential global positioning system (CP-DGPS)) to provide decimeter or centimeter level relative navigation accuracy. ⁴ To offer a more stable and accurate performance, the common approach is to fuse other ranging sensor measurements with CP-DGPS, ^5,6 such as inertial navigation system, ⁷ visual navigation system, ⁸ ultra-bandwidth (UWB) peer-to-peer ranging radios. ⁹ However, in practical scenarios, some measurements may be perturbed or obstructed by many factors, for example, the multipath or obstruction of ranging signals, ¹⁰ bad illumination condition, or violent antenna vibrations. Therefore, to avoid inaccurate relative positioning estimation, the abnormal data should be detected and excluded from the fusion procedure.

To improve the robustness of the navigation results in a multinode system, fault detection and exclusion (FDE) methods are needed. An FDE method was applied in a centralized cooperative localization algorithm to improve the robustness of absolute positioning performance for a single robot. ¹¹ A cooperative FDE algorithm used in a multi-unmanned aerial vehicle (UAV) context was proposed to increase the reliability of a single UAV. ¹² Although there are many FDE algorithms using Kalman filter (KF) based on χ ² distribution, ¹³ t-distribution, ¹⁴ F-distribution, ¹⁵ and so on, most of the researchers were still only focused on the absolute positioning performance of a single node. ¹⁶ To enhance the robustness of relative navigation system, the FDE methods are especially needed in this area to satisfy the high reliability needed for collision avoidance, cooperative task execution, collaborative positioning, and so on.

In this work, we propose a fault-tolerant multisensor relative navigation method based on KLD, which is adapted to the application of EKF, which can detect and exclude abnormal relative measurements. KLD is used to compare the distance between prior and posterior distributions yielded from the prediction step and update step of EKF, respectively. Through this, a general global Kullback–Leibler divergence (GKLD) is used to detect the existence of abnormal measurements, ¹⁵ and a group of sub-Kullback–Leibler divergence (SKLD) is calculated to remove the abnormal measurements from the fusion procedure.

To author’s knowledge, there have been no other literature to date that performs FDE on both internode ranging and double-differenced GNSS data in a relative navigation sense. Additionally, compared to the traditional FDE methods with χ ² test, the proposed method obtains more robustness by applying KLD instead of χ ² during hypothesis test.

This article is organized as follows: The second section presents the architecture of fault-tolerant multisensor relative navigation and the problem definition. The third section provides the FDE method used to improve the fault tolerance of relative navigation system. Simulation result data are shown and discussed in the fourth section. The last section is the conclusion of this article.

Fault-tolerant relative navigation

Problem definition and relative navigation system

As shown in Figure 1, the relative navigation system mainly consists of two parts. The first part is the sensor fusion based on EKF, and the second part is the FDE step based on the calculation of GKLD and SKLD. In the first part, to acquire multisensor measurements for relative navigation, all the vehicles are equipped with GNSS receiver and a UWB transceiver to communicate data and estimate its range to the neighboring vehicle. The relative navigation will be implemented by fusing GNSS pseudoranges and pseudorange Doppler with internode UWB ranging data. Each vehicle is assumed to have the ability to communicate its local GNSS and UWB measurements with other vehicles. The second part is the FDE step of the proposed method, the GKLD is calculated according to the covariance matrix and state estimate to diagnose the whole system. The results from step 1 will contribute to the prior knowledge of the next epoch if there was no detected false. Otherwise, it will conduct fault exclusion based on SKLD. If a fault has been detected, the final navigation results should be recalculated after fault exclusion. The detail of the FDE method is stated in the third section.

Figure 1.

Architecture of fault-tolerant relative navigation system.

Observation model

GNSS observations of relative navigation

The GNSS pseudorange is derived from the estimated time of signal receipt, which consists of the actual distance between GNSS receiver and satellite and some measurement errors. ¹⁷ So, the pseudorange observation between satellite i and vehicle A can be defined as

${\rho }_{A,i}=R_{A,i}+b_A+b_i+{\epsilon }_{A,i}+{\zeta }_{A,i}$

1

where $R_{A,i}$ is the true distance between the satellite i and vehicle A, b_A is the distance error due to receiver clock error, b_i is the error caused by clock error of satellite i, ${\epsilon }_{A,i}$ is the common noise arising from ionospheric delay, tropospheric delay, and ephemeris error, and ${\zeta }_{A,i}$ is the noncommon error related to thermal noise in GNSS receiver of vehicle A and multipath error of satellite i.

To remove the common errors caused by satellites and receivers, single-difference and double-difference techniques are used as measurements in place of the raw pseudorange measurements in the proposed relative navigation method. By taking the single difference between two pseudoranges of vehicles A and B to the same satellite i, the common error mainly due to ionospheric perturbation of satellite i can be removed. And, when two satellites i and j are both available to vehicles A and B, the receiver clock errors can be further removed by double-difference technique. ^18,19 The double-differenced pseudoranges for vehicles A and B and satellites i and j can be defined as

$\begin{array}{l}\nabla \text{Δ}{\rho }_{AB,ij}=({\rho }_{A,j}-{\rho }_{B,j})-({\rho }_{A,i}-{\rho }_{B,i})\\ =(e_j-e_i{)}^T{r}_{AB}+{\zeta }_{AB,ij}\end{array}$

2

where ${\zeta }_{AB,ij}$ is the residual of noncommon errors, which cannot be removed by double-difference technique, $e_j$ and $e_i$ are the direction cosine vectors from vehicles to satellites i and j, respectively, $r_{AB}$ is the relative distance vector between vehicles A and B.

From equation (2), the double-differenced GNSS pseudorange Doppler shifts for vehicles A, B and satellites i, j can be obtained as

$\begin{array}{l}\nabla \text{Δ}{\vartheta }_{AB,ij}=\frac{1}{\lambda }\frac{d\nabla \text{Δ}{\rho }_{AB,ij}}{dt}+{\gamma }_{AB,ij}\\ =\frac{1}{\lambda }(e_j-e_i{)}^T{v}_{AB}+{\gamma }_{AB,ij}\end{array}$

3

where λ is the wavelength of GNSS signal, $v_{AB}$ is the relative velocity vector between vehicles A and B, and ${\gamma }_{AB,ij}$ is the observation noise.

Ultra-bandwidth ranging observation

Commercial off-the-shelf UWB equipment is widely used as communication and ranging system in both indoor and outdoor environments. In this article, apart from the GNSS pseudoranges and Doppler shifts, ranging data between vehicles measured by UWB are also used. The UWB observations are susceptible to errors caused by obstructions between receiver and transmitter, leading to non-line-of-sight (NLOS) conditions, which is a positive bias in the distance estimation. Moreover, the distance estimate errors may also be caused by noise, interference, multipath, and so on. In normal case, the UWB ranging observation is modeled as

$r_{\text{uwb}}={‖r_{AB}‖}_2+{\epsilon }_{\text{uwb}}$

4

where ${\epsilon }_{\text{uwb}}$ is the observation noise.

Solution of relative navigation

To estimate the relative position between vehicles, GNSS pseudorange and GNSS pseudorange Doppler shifts are shared between vehicles via the UWB communication, and then the double-differenced observation in equations (2) and (3) and the UWB ranging observation in equation (4) can be fused to conduct relative navigation. Due to the nonlinear observation of UWB, an extended KF (EKF) is designed as the core of relative navigation.

The system model for relative navigation is the traditional constant acceleration model in target tracking

$X(t+\tau )\text{=}FX(t)+GW(t)$

5

where X is the state vector, F is the state transition model, G is the process noise model, W is the noise vector, and τ is the period of filter.

The state vector, state transition model, and process noise model are defined as

$X\text{=}{\left[r_{AB},v_{AB}\right]}^T$

6

$F=\left[\begin{array}{cc}{I}_3& \tau I_3\\ 0_3& I_3\end{array}\right]$

7

$G_1={[0.5{\tau }^2{I}_3\tau I_3]}^T$

8

where I_n is an identity matrix with size n, $0_n$ is a $n\times n$ zero matrix, $r_{AB}$ and $v_{AB}$ are the relative distance vector and relative velocity vector, respectively.

The observation model for the proposed method is defined as

$Z(t)\text{ = }h(X(t))+N(t)$

9

where Z is the observation vector, h is the nonlinear observation vector as a function of the state vector X, and N is the observation noise vector.

The observation vector and observation noise vector are defined as

$Z\text{ =}\left[\cdots ,\nabla \text{Δ}{\rho }_{AB,ij},\cdots ,\cdots ,\nabla \text{Δ}{\vartheta }_{AB,ij},\cdots ,{\widehat{r}}_{\text{uwb}}\right]$

10

$N\text{ =}\left[\cdots ,{\zeta }_{AB,ij},\cdots ,\cdots ,{\gamma }_{AB,ij},\cdots ,{\epsilon }_{\text{uwb}}\right]$

11

where satellite j is the reference satellite with the highest elevation angle, and the observation ${\widehat{r}}_{\text{uwb}}$ is the replacement of $r_{\text{uwb}}$ caused by the nonlinearity of UWB observation

${\widehat{r}}_{\text{uwb}}\text{=}{r}_{\text{uwb}}+H_{\text{uwb}}\widehat{X}-{\left.‖r_{AB}‖\right|}_{X=\widehat{X}}$

12

where $H_{\text{uwb}}$ is the linearization matrix for UWB observation and $\widehat{X}$ is the priori state estimation.

According to the GNSS observation model in equations (2) and (3) and the new UWB observation in equation (12), the observation matrix is defined as

$H\text{=}\left[\begin{array}{cc}{H}_{\text{pr}}& 0_{(m-1)\times 3}\\ 0_{(m-1)\times 3}& H_{\text{Doppler}}\\ H_{\text{uwb}}& 0_{1\times 3}\end{array}\right]$

13

where m is the number of visible satellites for both vehicles A and B

$H_{\text{pr}}\text{=}{\left[\begin{array}{c}⋮\\ e_j-e_i\\ ⋮\end{array}\right]}_{\left(m-1\right)\times 3}$

14

$H_{\text{Doppler}}\text{=}{\left[\begin{array}{c}⋮\\ \frac{e_j-e_i}{\lambda }\\ ⋮\end{array}\right]}_{\left(m-1\right)\times 3}$

15

$H_{\text{uwb}}\text{=}\left[\begin{array}{ccc}{\frac{\partial ‖r_{AB}‖}{\partial r_x}|}_{r_x={\widehat{r}}_x}& {\frac{\partial ‖r_{AB}‖}{\partial r_y}|}_{r_y={\widehat{r}}_y}& {\frac{\partial ‖r_{AB}‖}{\partial r_z}|}_{r_z={\widehat{r}}_z}\end{array}\right]$

16

Having the definitions above, we have the observation model and system model for a standard EKF scheme, and the proposed relative navigation method can be implemented by Simon. ²⁰

Fault detection and exclusion in multisensor relative navigation

After estimating the relative distance vector via EKF, the FDE algorithm is implemented to guarantee the robustness of relative navigation system and to reduce the error estimates caused by GNSS bias or NLOS of UWB.

Fault detection

The KLD between two n-dimensional Gaussian distributions f ₁ and f ₂ can be defined as ²¹

$\text{KL}(f_1||f_2)=0.5[\text{trace}(P_2{}^{-1}{P}_1)+\text{log}\left(\text{det}\left(\frac{P_2}{P_1}\right)\right)-n+{({\mathbf{\mu }}_1-{\mu }_2)}^T{P}_2{}^{-1}({\mu }_1-{\mu }_2)]$

17

where P ₁ and P ₂ are the covariance matrixes, ${\mu }_1$ and ${\mu }_2$ are the mean vectors.

Applying the KLD to the predicted step and corrected step of EKF, since the predicted states $X_{k/k-1}$ and corrected states $X_{k/k}$ are both multidimensional Gaussian distributions, and their covariance matrices are $P_{k/k-1}$ and $P_{k/k}$ , respectively, each vehicle can calculate GKLD as the indicator of the possible presence of abnormal measurements

$\begin{array}{l}\text{GKLD}(P_{k/k}||P_{k/k-1})\\ =0.5[\text{trace}(P_{k/k}{}^{-1}{P}_{k/k-1})+\text{log}\left(\text{det}\left(\frac{P_{k/k}}{P_{k/k-1}}\right)\right)-M+{(X_{k/k}-X_{k/k-1})}^T{P}_{k/k}{}^{-1}(X_{k/k}-X_{k/k-1})]\end{array}$

18

where M is the dimension of state.

The GKLD test statistic can be divided into two parts

$D_1={(X_{k/k}-X_{k/k-1})}^{\text{T}}{P}_{k/k}{}^{-1}(X_{k/k}-X_{k/k-1})$

19

$D_2=\hspace{0.17em}\text{trace}(P_{k/k}{}^{-1}{P}_{k/k-1})+\text{log}\left(\text{det}\left(\frac{P_{k/k}}{P_{k/k-1}}\right)\right)-M$

20

The component D ₁ yields the distance between X_k/k and X _k/k-1, and uses the posterior covariance matrix p_k/k as a weighing matrix. This test is sensitive to the error in state estimates caused by abnormal measurements. The test D ₂ yields the distance between the posterior and prior covariance matrices p_k/k and p_k/k-1 , respectively. ²² It will become significant when covariance matrix varies significantly before and after correction step of EKF.

The data distribution of D ₁ follows the Hotelling’s $T_{M,n-1}^2$ distribution: $D_1\text{∼}{T}_{M,n-1}^2$ , where n is the number of samples. ²³ The statistic T ² can be converted to statistic F by Rencher: ²³ $\frac{n-M}{M(n-1)}{T}_{M,n-1}^2\text{= }{F}_{M,n-M}$ . Therefore, the test D ₁ follows the distribution

$D_1\text{∼}\frac{M(n-1)}{n-M}{F}_{M,n-M}$

21

For the second part of GKLD, the distribution of D ₂ follows the χ ² distribution ²³

$D_2\text{∼}\frac{1}{n-1}\frac{1}{1-\frac{1}{6(n-1)-1}\left(2M+1-\frac{2}{M+1}\right)}{\chi }_{0.5M(M+1)}^2$

22

Therefore, in fault-free case, the distribution of GKLD test statistic is subject to the distribution

$\begin{array}{l}f(\text{GKLD}/\text{fault-free})\text{∼}\frac{M(n-1)}{2(n-M)}{F}_{M,n-M}+\\ \text{}\frac{1}{2(n-1)}\frac{1}{1-\frac{1}{6(n-1)-1}\left(2M+1-\frac{2}{M+1}\right)}{\chi }_{0.5M(M+1)}^2\end{array}$

23

Figure 2 compares the distribution of theoretical GKLD against a simulated dataset in a nominal setting. The theoretical curve of GKLD distribution is calculated from equation (23), while the empirical data are the GKLD histogram arising from the simulation of proposed fusion without fault. In this nominal case, the theoretical curve is consistent with the distribution of empirical data.

Figure 2.

Distribution comparison between empirical data and theoretical curve (GKLD in normal case). GKLD: global Kullback–Leibler divergence.

Defining the cumulative distribution function (CDF) of GKLD as

${\text{cdf}}_{\text{GKLD}}\text{=}{\displaystyle {\int }_{\lambda }^{\infty }f(\text{GKLD}/\text{fault-free})}d\text{GKLD}$

24

Based on the distribution of GKLD, the detection threshold λ is determined by the probability of false alarm (PFA), and it can be obtained using

$\lambda \hspace{0.17em}\text{=}\hspace{0.17em}\text{arg}({\text{cdf}}_{\text{GKLD}}=P_{\text{FA}})$

25

where $P_{\text{FA}}$ is the PFA required by the system. So, the judgment rule of GKLD test statistic is

$\left\{\begin{array}{c}\text{GKLD}\le \mathrm{\lambda }\text{Normal}\\ \text{GKLD}>\lambda \text{Fault}\end{array}\right.$

26

In fault detection step, the GKLD will be compared to the threshold λ to detect the presence of abnormal measurements, the fault hypothesis will be chosen if GKLD is larger than λ, otherwise, the navigation results will be considered as normal case.

Fault exclusion

After detecting an abnormal measurement during the relative navigation using the GKLD, the measurements with bias should be identified and removed from the fusion procedure. Thus, based on the greedy search strategy, ²⁴ a number of SKLD statistic should be computed for a number of measurement sets, where the n’th measurement is excluded

$\begin{array}{l}{\text{SKLD}}_n(P_{k/k}^{(n)}||P_{k/k-1})=\\ 0.5\left[\text{trace}(P_{k/k}^{(n)}{}^{-1}{P}_{k/k-1})+\text{log}\left(\text{det}\left(\frac{P_{k/k}^{(n)}}{P_{k/k-1}}\right)\right)-M+{(X_{k/k}^{(n)}-X_{k/k-1})}^T{P}_{k/k}{}^{-1}(X_{k/k}^{(n)}-X_{k/k-1})\right]\end{array}$

27

where $P_{k/k}^{(n)}$ and $X_{k/k}^{(n)}$ are the covariance matrix and state estimates after the removal of the n’th measurement

$P_{k/k}^{(n)}\text{=}{P}_{k/k\text{-}1}-K_k^{(n)}{H}^{(n)}{P}_{k/k\text{-}1}{}^{-1}$

28

$X_{k/k}^{(n)}=X_{k/k\text{-}1}\text{+}{K}_k^{(n)}(Z_k^{(n)}-H^{(n)}{X}_{k/k\text{-}1})$

29

where $H^{(n)}$ is the observation matrix that removes the observation relationship related to measurement n, $Z_k^{(n)}$ is the observation vector that excludes the measurement n, and $K_k^{(n)}$ is the corresponding gain of EKF

$K_k^{(n)}\text{=}{P}_{k/k\text{-}1}{}^{-1}{\left(H^{(n)}\right)}^T{\left(H^{(n)}{P}_{k/k\text{-}1}{}^{-1}{\left(H^{(n)}\right)}^T+R^{(n)}\right)}^{-1}$

30

where $R^{(n)}$ is the known matrix of measurement covariance.

In the fault exclusion step, if the abnormal case has been correctly excluded in $H^{(n)}$ and $Z_k^{(n)}$ , the value of SKLD _n will be lower than all others. So, the abnormal measurements can be excluded by finding the set of measurements that minimizes the SKLD.

The step-by-step execution of the FDE algorithm proposed here can be summarized below:

Step 1. Calculate the GKLD based on the corrected states $X_{k/k}$ , updated covariance matrix $P_{k/k}$ , predicted states $X_{k/k\text{-}1}$ , and predicted covariance matrix $P_{k/k\text{-}1}$ .

Step 2. If the GKLD exceeds the threshold (see International Civil Aviation Organization ²⁵ ), calculate the SKLD to find the abnormal measurements (step 3) or else the navigation results computed will be considered valid.

Step 3. Calculate a number of SKLD based on the exclusion of each observation (see Rencher ²³ ) and find the SKLD with the lowest value to exclude abnormal measurements.

Step 4. Recalculate the new GKLD based on the new corrected states $X_{k/k}^{\text{*}}$ and new covariance matrix $P_{k/k}^{\ast }$ , which arise from the exclusion step. Assume that SKLD _j has the lowest value among all cases, then j’th observation should be removed from the fusion procedure, $X_{k/k}^{\text{*}}$ and $P_{k/k}^{\ast }$ are equal to the $X_{k/k}^j$ and $P_{k/k}^j$ , respectively.

Step 5. Go back to step 3 until the GKLD satisfies the terminal conditions.

The design of the above FDE procedure is to identify abnormal (i.e. faulty) measurements and to avoid the false exclusion. According to the exclusion results from SKLD, the GKLD will be recalculated iteratively to judge whether the faults still exist. If the new GKLD still exceeds the threshold value, SKLD will be recalculated by excluding other observations, until GKLD satisfies the terminal conditions. The overall procedure of the proposed FDE method is shown in Figure 3.

Figure 3.

The fault detection and exclusion architecture of the proposed method.

Note that the greedy search strategy is based on a bunch of subfilters from the main filter, and the number of subfilters is determined by the number of available measurements at each epoch. Therefore, in faulty epochs, the computational load for fault exclusion is $N+1$ times higher than a standard KF, where N is the number of measurements. Although the fault exclusion procedure needs more processing time, the dimension for matrix multiplication is relatively low in the case of relative positioning, such computational load will not heavily affect the real-time ability of the proposed method.

Simulation and experimental results

To verify the performance of the FDE algorithm proposed in this article, multiple influential factors are considered in the simulation (see “Simulation results” section), including the fault detection results according to abnormal measurements from GNSS and UWB, and the root mean square error (RMSE) performance of relative positioning in different case of PFA.

To verify the effectiveness of the proposed method in realistic scenario, field trial data are used in “Experimental results” section. The experiment set up includes two vehicles equipped with GPS receivers and UWB transceivers, carrier-phase-based differential position estimates (RTK) of both vehicles are used as ground truth.

It should be noted that the proposed GKLD method is a typical snapshot algorithm. In the case of KF, the estimation process can be contaminated by long-lasting anomalies, especially in the case of slowly growing errors with low initial amplitudes. Therefore, long-lasting anomalies are not considered in this work.

Simulation description

The background for the simulation scenario is based on two vehicles moving in a certain formation and trajectory, assuming that the distance between two vehicles is always within the working range of UWB, and the lasting time for the trajectories is 1200 s. The trajectories of vehicles are shown in Figure 4.

Figure 4.

Trajectories of simulated vehicles.

Both the vehicles carry the UWB transceiver and GNSS receiver, and the parameters used in the simulation are listed in Table 1. In normal case, the observation errors of all measurements are considered as Gaussian noise, and the output frequencies of these sensors are both set to 1 Hz. The number of available satellites is set to 6, and their positions in earth-centered earth-fixed (ECEF) frame are listed in Table 2.

Table 1.

Sensor configuration and simulation parameters.

Sensor	Parameter	Value
GNSS	Pseudorange noise standard deviation	1 m
	Pseudorange Doppler noise standard deviation	4 Hz
UWB	Gaussian noise standard deviation	0.15 m

UWB: ultra-bandwidth.

Table 2.

Satellite position in ECEF frame.

Satellite	x (m)	y (m)	z (m)
1	19263524	−13725770	11583188
2	26124976	−5749420	−846377
3	24768710	1601307	9925575
4	8048029	−13014437	21563572
5	8543818	15561017	19676844
6	2082386	23437415	12048145

Simulation results

The simulation results are presented in two parts. In the first part, GNSS bias and UWB bias will be added to the measurements randomly, and we restrict that there will be only one abnormal measurement at each epoch, the performance of GKLD statistics and the comparison of relative positioning accuracy before and after FDE will be demonstrated. Then, in the second part, the comparison of RMSE performances based on different cases of PFAs and selected epochs with faulty measurement will be conducted. The relative positioning error in all simulations is calculated by

$E=\sqrt{Er{r}_E{}^2+Er{r}_N{}^2+Er{r}_U{}^2}$

31

where $Er{r}_E$ , $Er{r}_N$ , and $Er{r}_U$ are estimation errors in three directions of east, north, and up, respectively. The definition for RMSE is

$RMSE(k)={\left\{\frac{1}{N}{\displaystyle \sum _{i=1}^{N}E{(k)}^2}\right\}}^{\frac{1}{2}}$

32

where N is the total number of data.

In simulation 1, the PFA and the probability of missed detection used in this part are both set to 0.005, and the total duration for abnormal measurements is 100 s. Figure 5 shows the situations of abnormal measurements and the performance of GKLD statistics. It is obvious that the abnormal measurements of UWB and GNSS are successfully detected by the GKLD statistics, and the GKLD abnormal values related to GNSS are generally lower than those of UWB. This is mainly because the abnormal observations caused by GNSS have lower influence on relative position accuracy than that of UWB observation, so the GKLD will be less sensitive to the GNSS errors.

Figure 5.

Observation errors and fault-detection results.

Figures 6 and 7 show the relative positioning performance of the proposed FDE algorithm based on the abnormal measurements shown in Figure 5. After the FDE step, because all the abnormal measurements have been removed from the fusion procedure, the accuracy of relative positioning improves significantly. In Figure 6, the positioning error without FDE is obviously larger than that after FDE, during all faulty epochs, the average RMSE without FDE during faulty arises to 2.683 m, while only 1.485 m in the FDE case. It means the relative navigation accuracy increases by 80.61% when the abnormal measurements exist. From Figure 7, the RMSE without FDE is apparently larger than that before FDE, during all the navigation time, the average RMSE without FDE is 1.587 m and after FDE is 1.177 m, which demonstrates the effectiveness of the application of GKLD in multisensor relative navigation system.

Figure 6.

Relative positioning error comparison without and after FDE. FDE: fault detection and exclusion.

Figure 7.

RMSE of relative position without and after FDE. FDE: fault detection and exclusion; RMSE: root mean square error.

In simulation 2, to assess the performance of the proposed FDE algorithm in different cases of PFA and different measurements, the simulation based on GNSS and UWB measurement biases is shown in Figures 8 and 9. Four control groups are used in the simulation, the RMSE of relative positioning without FDE, and the RMSE of the proposed method with PFA = 0.01, PFA = 0.005, and PFA = 0.001, respectively. The abnormal measurements occur randomly during the simulation, and the total duration for abnormal measurements in GNSS case and UWB case is both set to 100 s.

Figure 8.

RMSE comparison among four cases (1. Without FDE, 2. Setting PFA to 0.01, 3. Setting PFA to 0.01, 4. Setting PFA to 0.01). FDE: fault detection and exclusion; RMSE: root mean square error.

Figure 9.

RMSE comparison among four cases (1. Without FDE, 2. Setting PFA to 0.01, 3. Setting PFA to 0.01, 4. Setting PFA to 0.01). FDE: fault detection and exclusion; RMSE: root mean square error.

Figure 8 compares the performance of the proposed method under the condition of a single faulty GNSS measurement. It is obvious that it is easier to detect GNSS biases with lower amplitude when the PFA is lower. So, the system has the best performance of FDE when PFA = 0.01, has lower performance when PFA is 0.005 or 0.001, and as expected, the control group without FDE has the worst performance among four cases.

Figure 9 shows the comparison of the proposed method in case of a single faulty UWB measurement using various PFA. Similar to the case of GNSS bias, the proposed FDE algorithm is able to more reliably detect UWB biases using reasonable values of PFA, and the system will have the worst performance without FDE. According to the RMSE curves of FDE cases, the proposed method can easily detect the UWB biases larger than 8 m with the proper parameter settings, which corresponds with a measurement obtained under an NLOS scenario.

Experimental results

The experimental results based on field trial data are demonstrated in this part. Two vehicles are equipped with GPS receivers and UWB transceivers to obtain noisy GPS L1 pseudoranges, pseudorange Doppler shifts, and intervehicle ranging data. The equipment used to acquire reference position was the Leica GS10 geodetic RTK receiver for vehicles. RTK receiver was not only used as ground truth data but will also be used to calculate the measurement residuals of the double-differenced pseudoranges during the experiment.

The test area was in Clifton Boulevard, Nottingham, UK, and the duration of the entire experiment is 841 s. The number of available satellites we used in the results is 8. The sampling rate for the experiment data was 1 Hz, and the GPS time was used for data synchronization in the data process procedure. Figure 10 shows the trajectories for two vehicles, both of them were driving along the lane, and vehicle 2 followed vehicle 1.

Figure 10.

Trajectory of field data in Clifton Boulevard, Nottingham, UK.

Figure 11 shows the residuals of double-differenced GPS (DD-GPS) pseudoranges during the experiment. Such residuals are computed from the ground truth and common visible satellites, which come from RTK and ephemeris. We find that there were some faulty measurements that cannot be removed by double differencing, mainly caused by multipath or NLOS. During the experiment, the fourth satellite with the highest elevation angle was used as a master satellite for double differencing. Note that, although GPS L1 pseudoranges were affected by some faulty measurements, the Leica GS10 geodetic RTK receiver is a multi-GNSS and multifrequency positioning equipment that robust to such interference, which means the ground-truth output by Leica GS10 is reliable.

Figure 11.

Residuals for DD-GPS pseudoranges. DD-GPS: double-differenced global positioning system.

To demonstrate the effectiveness of the proposed FDE method, we use three control groups:

Without FDE: the case without FDE algorithm, using EKF to fuse GPS and UWB data (see “Solution of relative navigation” section);

After FDE (χ²): The widely used FDE algorithm conducts χ ² test toward the innovation of EKF, ²⁶ also called χ ²-FDE in the following discussion. The test statistics for this method is the Mahalanobis distance of innovation vector, which follows a χ ² distribution in normal case and a noncentral χ ² distribution in a faulty case;

After FDE (GKLD): The proposed FDE algorithm based on the GKLD also called GKLD-FDE in the following content.

For realistic PFA configuration, we set PFA for all control groups as 4×10⁻⁶ based on the International Civil Aviation Organization specification. ²⁵

Figure 12 compares the relative positioning errors for three control groups. The case without FDE cannot exclude the faulty measurements from the fusion procedure, thus has the worst positioning performance, whereas the cases after FDE (χ ² and GKLD) have better performance, and both of them are robust to the malignant faulty measurements. For example, at faulty epoch 748 s, the positioning error for the case without FDE is 6.67 m, but only 0.477 m for χ ²-FDE and GKLD-FDE.

Figure 12.

Comparison for relative positioning error (without FDE vs. after FDE (χ ²) vs. after FDE (GKLD)). FDE: fault detection and exclusion; GKLD: global Kullback–Leibler divergence.

Also, in Figure 12, we find that the performance of χ ²-FDE is slightly lower than that of GKLD-FDE, this is mainly caused by their different fault detection strategies. χ ²-FDE is a typical hypothesis test in the measurement domain, which means χ ²-FDE can exclude the faulty measurements that large enough to be detected by threshold. However, faulty measurements may still contribute to the estimate process, especially in the case with a bad positional dilution of precision. Therefore, χ ²-FDE will suffer from such “overdetection” by excluding some faulty measurements that are insignificant for positioning error. For GKLD-FDE, according to equation (18), which can be seen as a typical hypothesis test in the position domain, ²⁷ it only rejects the faulty measurements that lead to significant positioning errors, thus GKLD-FDE outperforms χ ²-FDE. For example, at epoch 4 s, the positioning error for χ ²-FDE is 2.5 m, but only 1.66 m for GKLD-FDE and the case without FDE.

Figure 13 and Table 3 compare more detailed performance for all control groups. χ ²-FDE excludes the faulty measurements during the navigation, thus has better performance than the case without FDE in most cases. And the proposed GKLD-FDE has the best performance, for RMSE, GKLD-FDE is 1.63 m at the last epoch, better than that of χ ²-FDE (1.68 m) and the case without FDE (1.75 m). The detailed data can be found in Table 3.

Figure 13.

Comparison for relative positioning RMSE curve (without FDE vs. after FDE (χ ²) vs. after FDE (GKLD)). FDE: fault detection and exclusion; GKLD: global Kullback–Leibler divergence; RMSE: root mean square error.

Table 3.

Experimental results for relative positioning errors.

Methods	RMSE (m)	Mean (m)	Standard deviation (m)
Without FDE	1.75	1.58	0.76
After FDE (χ 2)	1.68	1.54	0.69
After FDE (GKLD)	1.63	1.50	0.64

FDE: fault detection and exclusion; GKLD: global Kullback–Leibler divergence; RMSE: root mean square error.

Figure 14 shows the test statistics and detection thresholds for χ ²-FDE and GKLD-FDE, which correspond with the results in Figures 12 and 13. Both χ ²-FDE and GKLD-FDE are capable of detecting and excluding the malignant faulty measurement, such as the bias in #7/#4 at epoch 748 s (see Figure 11). χ ²-FDE is “overdetection” to the bias in #6/#4 at epoch 4 s, causing the corresponding RMSE (see Figure 13) and positioning error (see Figure 12) deterioration. Such detection performance is in accord with the above analysis in Figure 12.

Figure 14.

Test statistics and detection threshold for χ ² and GKLD. GKLD: global Kullback–Leibler divergence.

Conclusion

In this article, a GNSS/UWB-based relative navigation system with FDE is proposed. The algorithm is applying the KLD to the architecture of EKF, using EKF as the sensor fusion method. Incorporating data from both the predicted step and corrected step of the EKF, GKLD is calculated to detect the occurrence of a faulty measurement. When a fault is detected, the SKLD for subsets of measurements is obtained to correctly exclude the faulty measurement.

Simulation 1 results show that the relative positioning performance after FDE is much better than that of non-FDE case. The proposed method is capable of identifying faulty epochs in both GNSS and UWB measurements. The simplicity of the proposed general architecture meant that it can be easily adopted for other multisensor fusion of relative navigation systems. In simulation 2, we showed that the proposed FDE method remains highly effective under a range of reasonably configured PFA, attesting to the robustness of the proposed approach. While the average RMSE without FDE climbs up to 2.683 m due to faulty measurements, our proposed FDE approach managed to suppress the average RMSE to be consistent to 1.485 m.

Experimental results demonstrate the effectiveness of the proposed GKLD-FDE method via field trial data, which not only better than non-FDE case but also outperforms the widely used KF innovation-based χ ²-FDE method.