Cooperative Vehicle Localization in Multi-Sensor Multi-Vehicle Systems Based on an Interval Split Covariance Intersection Filter with Fault Detection and Exclusion

Abstract

In the cooperative multi-sensor multi-vehicle (MSMV) localization domain, the data incest problem yields inconsistent data fusion results, thereby reducing the accuracy of vehicle localization. In order to address this problem, we propose the interval split covariance intersection filter (ISCIF). At first, the proposed ISCIF method is applied to the absolute positioning step. Then, we combine the interval constraint propagation (ICP) method and the proposed ISCIF method to realize relative positioning. Additionally, in order to enhance the robustness of the MSMV localization system, a Kullback–Leibler divergence (KLD)-based fault detection and exclusion (FDE) method is implemented in our system. Three simulations were carried out: Simulation scenarios 1 and 2 aimed to assess the accuracy of the proposed ISCIF with various capabilities of absolute vehicle positioning, while simulation scenario 3 was designed to evaluate the localization performance when faults were present. The simulation results of scenarios 1 and 2 demonstrated that our proposed vehicle localization method reduced the root mean square error (RMSE) by 8.9% and 15.5%, respectively, compared to the conventional split covariance intersection filter (SCIF) method. The simulation results of scenario 3 indicated that the implemented FDE method could effectively reduce the RMSE of vehicles (by about 55%) when faults were present in the system.

1. Introduction

High-accuracy localization of vehicles is one of the most important functions in intelligent transportation systems (ITS) and autonomous driving. In recent years, many methods of high-accuracy vehicle localization have been developed, such as vision-based localization methods [1], map-based methods [2,3], vehicle-to-vehicle (V2V)-based methods [4,5], vehicle-to-infrastructure (V2I)-based methods [6,7,8,9], and so on. However, both vision- and map-based vehicle localization methods have high computation and storage resource overhead, making them ineffective for multi-sensor multi-vehicle systems (MSMVs) [10]. Furthermore, because V2I-based vehicle localization algorithms rely on the deployment of infrastructure, such as base stations, the positioning of vehicles using V2I faces certain limitations when compared to V2V-based methods. Cooperative vehicle localization methods are more effective than traditional vehicle localization methods. Cooperative vehicle localization methods can achieve higher accuracy and robustness, benefiting from data sharing among vehicles by using different communication techniques. These techniques include ultra-wideband (UWB) [11], 5G [12], and so on. Furthermore, concerning application scenarios, cooperative vehicle localization methods are extensively used in various domains, including those of autonomous underwater vehicles (AUVs) [13], multiple robots [14], unmanned aerial vehicles (UAVs) [15], intelligent vehicles (IVs) [16], and so on.

In the field of nonlinear vehicle localization, the particle filter (PF) [17,18] is a famous technique. However, the main drawback of the PF is its high computational complexity. A large number of particles are required to accurately estimate the positions of vehicles. Other methods, such as Huber’s M-estimation-based Kalman filter (HKF) [19] and the maximum-correntropy-based Kalman filter (MC-KF) [20], have limited localization accuracy, which was emphasized in [21]. Moreover, the extended Kalman filter (EKF) has the ability to operate effectively in nonlinear vehicle localization systems. It is based on linear approximation [22]. However, the process of linear approximation may lead to a reduction in localization accuracy. In order to address this problem, the authors of [23] proposed a cubature Kalman filter (CKF), which was designed based on the spherical–radial cubature rule. While CKF has the potential to enhance vehicle localization accuracy, its performance may diverge in the presence of process uncertainties. In order to enhance the vehicle localization performance when facing uncertain process noise, the improved strong tracking seventh-degree spherical simplex–radial cubature Kalman filter (IST-7thSSRCKF) was proposed in [21]. Moreover, the adaptive cubature Kalman filter (ACKF) [24] was proposed to correct the uncertainties in the statistical process in nonlinear systems. In addition, the robust cubature Klaman filter (RCKF) was designed in [25] to enhance the robustness of vehicle localization systems.

For cooperative vehicle localization in MSMVs, one serious problem is that of data incest [26], which yields inconsistent fusion localization results. When the same data are reused multiple times and these data are treated as independent in the data fusion process, the data incest problem will occur. The data flow of two vehicles when they realize cooperative localization is shown in Figure 1; the flows of position data of vehicle 1 and vehicle 2 are denoted by black and blue lines, respectively. After the initialization, data are processed in an iterative way. At each time step, the position of each vehicle at the last time step is used to calculate the estimated position (relative position) for the neighboring vehicles and estimate its position. Each vehicle realizes its own relative positioning update when it receives the relative position information from its neighbor. If different data are considered independently of each other, the data incest problem occurs in the second time step because the position information of each vehicle and the estimated relative position received from its neighbor are not independent of each other. It should be noted that if there are more than two vehicles communicating and providing their estimated relative positions with each other at the same time step, the data incest problem will be more serious.

In order to solve the data incest problem, one famous method is the covariance intersection (CI) algorithm [27]. Although the CI method can achieve consistent estimation with unknown correlation fusion resources, pessimistic fusion results cannot be avoided because the fusion resources are treated as totally dependent. So, the split covariance intersection filter (SCIF) [28] was proposed. In the SCIF, the covariance part of each fusion resource is divided into correlated and absolutely independent parts, which can yield accurate fusion results. However, the SCIF may yield over-convergence in the fusion results in cooperative vehicle localization in MSMVs.

On the other hand, the interval constraint propagation (ICP) method can also solve the data incest problem in MSMVs [29]. Interval constraints on the position of the vehicle at each time are defined, and the result is determined by calculating the intersection that satisfies all constraints. However, the ICP method may lose accuracy since it treats all of the constraints as if they have the same weight. Furthermore, the interval Kalman filter (IKF) [30] and interval extended Kalman filter (IEKF) [31] were proposed to solve the incest problem; both the IKF and the IEKF have the same iterative structures as those of the traditional KF and EKF. However, the inverse of the interval matrix is an NP-hardness problem.

The fault detection and exclusion (FDE) method can effectively enhance the robustness of a vehicle localization system [32]. It can improve localization accuracy, particularly in the presence of sensor faults. Kullback–Leibler divergence (KLD), also known as relative entropy, is a widely recognized tool for FDE [33]. KLD quantifies the difference in information between two probability density functions (pdfs). It can also be seen as the anticipated logarithm of the likelihood ratio between two sets of data distributions [34]. One example of the FDE method based on KLD was presented in [35]. At first, decentralized vehicle localization based on the SCIF is proposed. Then, a KLD-based fault detection and isolation method was introduced and implemented to enhance the robustness of the system. Another FDE work based on KLD was outlined in [36], where it was implemented in extended information filter (EIF)-based positioning systems. Additionally, the discussion of the results revolved around utilizing a threshold determined by conditional-entropy-based criteria and employing the receiver operating characteristic (ROC) curve. So, the KLD-based FDE method can effectively enhance the robustness of decentralized cooperative vehicle localization systems.

Motivated by addressing the drawbacks of the different methods mentioned above and aiming to enhance the robustness of decentralized cooperative vehicle localization, we propose a new data fusion method named the interval split covariance intersection filter (ISCIF); both the ISCIF and the KLD-based FDE method were implemented in the proposed MSMV decentralized cooperative vehicle localization system. The contributions of this study are as follows:

• The ISCIF algorithm is proposed and implemented for vehicle localization in MSMVs in both absolute and relative positioning steps.
• The proposed ISCIF method can avoid the inverse of the interval matrix compared with the IEKF.
• A KLD-based FDE method is implemented to reduce the RMSE of the localization results when faults are present in the system.
• Based on the simulation results, our proposed method can achieve better accuracy than that of the SCIF. In addition, the implemented FDE method can achieve better accuracy when faults are present in the system.

This paper is organized as follows: Section 2 presents the related techniques and the proposed ISCIF method. Section 3 shows the system model and MSMV vehicle localization by using our proposed method. Section 4 presents the KLD-based FDE method. In Section 5, we present the simulation results. Finally, Section 6 gives the conclusions and the directions of future work.

2. Related Techniques and the Proposed ISCIF

2.1. Interval Analysis

Interval analysis is a numerical method that can represent values as intervals by using lower and upper bounds. The notation $\left[\right]$ can be used to represent an interval. For example, an interval $\left[x\right]$ can be denoted as:

$$\left[x\right]=[\underline{x},\overline{x}]=\left\{x\in \mathbb{R},\underline{x}\le x\le \overline{x}\right\}$$

(1)

where $\underline{x}$ and $\overline{x}$ are the lower bound and upper bound, respectively.

In addition, a box (also named an interval vector) is represented by the Cartesian product of n intervals:

$$\left[X\right]=\left[x_1\right]\times \left[x_2\right]\times \cdots \times \left[x_n\right]$$

(2)

Moreover, the basic operations of an interval or a box have been defined in [37,38]. For example, these operations include calculating the width and midpoint of an interval, the addition and subtraction rules, the intersection rules of intervals or boxes, and the inclusion functions of the intervals and boxes.

Furthermore, the inclusion function of $f:{\mathbb{R}}^a\to {\mathbb{R}}^b$ is defined as $\left[f\right]$ to realize arithmetical operations and elementary functions in the function $f\left(\right)$. The inclusion function is also suitable for the box $\left[A\right]$.

$$f\left(\left[A\right]\right)\in \left[f\right]\left(\left[A\right]\right),\forall \left[A\right]\in {\mathbb{R}}^a$$

(3)

2.2. Interval Constraint Propagation (ICP)

One interval constraint satisfaction problem (ICSP) can be solved with the ICP method, and the result should satisfy all of the constraints. Considering one box [X] that includes n variables and m relations linking these variables, the ICSP can be denoted by

$$f_j\left(x_1,x_2,\cdots ,x_n\right)=0,j=1,2,\cdots ,m$$

(4)

Especially in the vehicle localization domain, considering the relative positioning stage, when m vehicles provide a relative position for vehicle i, the ICSP of vehicle i can be solved by using the ICP method (as shown in Equation (5)). More details on ICP methods were proposed in [29].

$$\mathbb{P}1:\left[X_i^R\right]=\left\{\begin{array}{c}\left[x_i^1\left(t\right)\right]\\ \left[y_i^1\left(t\right)\right]\end{array}\right\}\bigcap \cdots \bigcap \left\{\begin{array}{c}\left[x_i^m\left(t\right)\right]\\ \left[y_i^m\left(t\right)\right]\end{array}\right\}$$

(5)

where $\left[X_i^R\right]$ represents the result of the ICP method. $x_i^m\left(t\right)$ and $y_i^m\left(t\right)$ are the estimated relative position of vehicle i provided by vehicle m on the x and y coordinates, respectively.

2.3. Interval Kalman Filter (IKF) and the Proposed Interval Split Covariance Intersection Filter

For the relative localization stage of the target vehicles, the relative position provided by their neighbors is no longer an unbiased estimate. The reason is that the relative position information data are based on the observers’ positions (these positions are not true values, as they also have unknown errors). So, in order to effectively reduce the bad influence caused by uncertain errors, the IKF was proposed in [39]. The IKF method has the same statistical assumptions about noise and recursive structures as those of the traditional KF algorithm. The prediction and correction steps of the IKF are defined by Equations (6) and (7), respectively. It should be noted that all of the calculations in the traditional KF have been replaced by interval-based calculations.

$$\{\begin{array}{cc}\hfill \left[\widehat{x_k^{-}}\right]& =\left[A\right]\left[\widehat{x_{k-1}}\right]+\left[B\right]\left[U_{k-1}\right]\hfill \\ \hfill \left[P_k^{-}\right]& =\left[A\right]\left[P_{k-1}\right]{\left[A\right]}^T+Q\hfill \end{array}$$

(6)

$$\{\begin{array}{cc}\hfill \left[K_k\right]& =\left[P_k^{-}\right]{\left[H\right]}^T{\left(\left[H\right]\left[P_k^{-1}\right]{\left[H\right]}^T+R\right)}^{-1}\hfill \\ \hfill \widehat{\left[x_k\right]}& =\widehat{\left[x_k^1\right]}+\left[K_k\right](\left[Z_k\right]-\left[H\right]\widehat{\left[x_k^{-}\right]})\hfill \\ \hfill \left[P_k\right]& =\left(I-\left[K_k\right]\left[H\right]\right)\left[P_k^{-}\right]\hfill \end{array}$$

(7)

Since the result of the inverse of the interval matrix is an approximate solution [40], the possible fusion result may be lost and the fusion result’s accuracy may be reduced. We would like to solve this problem by proposing the ISCIF. In our proposed ISCIF, the interval covariance matrix is replaced by an ordinary matrix, which benefits from the optimization operation between the split covariance matrices in the traditional SCIF. The estimates to be fused in the ISCIF are denoted as $\left(\left[X\right],P_I+P_D\right)$, where $\left[X\right]$ is in the interval form of the state vector, which can be provided by the interval analysis technique. $P_I$ is the degree of the independent covariance matrix, and $P_D$ is the maximum degree of the possible correlated covariance matrix. It should be noted that the covariance matrix is always divided into independent and correlated parts, making it similar to that in the traditional SCIF. So, with two input estimates to be fused—$\left(\left[A\right],P_{AI}+P_{AD}\right)$ and $\left(\left[B\right],P_{BI}+P_{BD}\right)$—the ISCIF method with the fusion result $\left(\left[C\right],P_{CI}+P_{CD}\right)$ can be denoted by Equation (8):

$$\{\begin{array}{cc}\hfill P_1& =P_{AD}/w+P_{AI}\hfill \\ \hfill P_2& =P_{BD}/(1-w)+P_{BI}\hfill \\ \hfill P_C^{-1}& =P_1^{-1}+P_2^{-1}\hfill \\ \hfill \left[C\right]& =P_C\left(P_1^{-1}\left[A\right]+P_2^{-1}\left[B\right]\right)\hfill \\ \hfill P_{CI}& =P_C\left(P_1^{-1}{P}_{AI}{P}_1^{-1}+P_2^{-1}{P}_{BI}{P}_2^{-1}\right)P_C\hfill \\ \hfill P_{CD}& =I-P_{CI}\hfill \end{array}$$

(8)

The weighting coefficient w can be determined by minimizing the trace or determinant of the output covariance matrix $P_C$ in each time step, and it belongs to the interval $\left[0,1\right]$.

3. Cooperative Vehicle Localization with MSMVs by Using the ISCIF

3.1. System Model

We use the general system model proposed in [10], in which there are N vehicles and multiple sensors. The V2V communication technique is employed, and we assume that vehicles in the system can communicate with their neighbors within their communication range. Moreover, our system model has the following assumptions.

(1) Each vehicle in the system is equipped with sensors that can provide absolute positioning data (provided by GPS), relative positioning data (provided by LiDAR), and motion state data (provided by an IMU sensor).

(2) Data sharing can be guaranteed when vehicles can perceive and communicate with their neighbors. In this study, we concentrate on the topic of localization with highly redundant relative position data. We assume that each vehicle can be observed by at least two neighbors at each time.

(3) The decentralized manner is employed in our system model, and this is illustrated in Figure 2. In this figure, we assume that vehicle i has two neighbors, and each vehicle has its own data fusion center. Furthermore, each vehicle can timestamp its data with a global system time.

In addition, our cooperative localization architecture is shown in Figure 3. First, each vehicle predicts its state vector by using the motion data. Then, the absolute positioning step is executed by using data provided by the GPS sensor. When each vehicle receives its relative position information provided by its neighbor, the state is updated in the relative positioning step. The details of different steps are presented in the following section.

3.2. Prediction of the State Vector

The state vector of vehicle i at time t is represented as

$$X_i\left(t\right)={\left[x_i\left(t\right),y_i\left(t\right),{\theta }_i\left(t\right)\right]}^T$$

(9)

where the $x_i\left(t\right)$ and $y_i\left(t\right)$ are the coordinates of the vehicle on the x and y axes, respectively. ${\theta }_i\left(t\right)$ is the direction of the vehicle.

Using the kinematic bicycle model, the prediction equations are defined as

$$\{\begin{array}{cc}\hfill x_i\left(t\right)& =x_i(t-1)+\Delta d_i\left(t\right)cos\left({\theta }_i(t-1)+\frac{\Delta d_i\left(t\right)}{2}\right)\hfill \\ \hfill y_i\left(t\right)& =y_i(t-1)+\Delta d_i\left(t\right)sin\left({\theta }_i(t-1)+\frac{\Delta d_i\left(t\right)}{2}\right)\hfill \\ \hfill {\theta }_i\left(t\right)& ={\theta }_i(t-1)+\Delta {\theta }_i\left(t\right)\hfill \end{array}$$

(10)

where $\Delta d_i\left(t\right)$ is the moving distance from time $t-1$ to time t, and $\Delta {\theta }_i\left(t\right)$ is the difference in the directional angle of the vehicle from time $t-1$ to time t.

The function form of the prediction model can be denoted as

$$\left[\begin{array}{c}{x}_i\left(t\right)\\ y_i\left(t\right)\\ {\theta }_i\left(t\right)\end{array}\right]=f(\left[\begin{array}{c}{x}_i(t-1)\\ y_i(t-1)\\ {\theta }_i(t-1)\end{array}\right],\left[\begin{array}{c}\Delta d_i\left(t\right)\\ \Delta {\theta }_i\left(t\right)\end{array}\right])$$

(11)

We can assume that the motion measurements follow a zero-mean Gaussian distribution with the covariance matrix $Q_i^U\left(t\right)$. Since $\Delta d_i\left(t\right)$ and $\Delta {\theta }_i\left(t\right)$ are independent of each other, the covariance matrix can be defined as

$$Q_i^U\left(t\right)=\left[\begin{array}{cc}{\sigma }_{\Delta d_i\left(t\right)}^2& 0\\ 0& {\sigma }_{\Delta {\theta }_i\left(t\right)}^2\end{array}\right]$$

(12)

It is assumed that $Q_i\left(t\right)$ characterizes the motion model’s error covariance matrix and that $Q_{iI}\left(t\right)$ is the independent component of $Q_i\left(t\right)$. By adding Gaussian white noise to the motion model, the total covariance $P_i\left(t\right)$ and the independent component $P_{iI}\left(t\right)$ of the state of vehicle i at time t can be updated by

$$\begin{array}{cc}\hfill P_i\left(t\right)=& F_i\left(t\right)P_i(t-1)F_i{\left(t\right)}^T\hfill \\ \hfill & +B_i\left(t\right)Q_i^U\left(t\right)B_i{\left(t\right)}^T+Q_i\left(t\right)\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill P_{iI}\left(t\right)=& F_i\left(t\right)P_{iI}(t-1)F_i{\left(t\right)}^T\hfill \\ \hfill & +B_i\left(t\right)Q_i^U\left(t\right)B_i{\left(t\right)}^T+Q_{iI}\left(t\right)\hfill \end{array}$$

(14)

where $F_i\left(t\right)$ and $B_i\left(t\right)$ are the Jacobian matrix, and they are calculated by:

$$F_i\left(t\right)=\left[\begin{array}{ccc}1& 0& -\Delta d_i\left(t\right)sin\left\{{\theta }_i(t-1)+\frac{\Delta {\theta }_i\left(t\right)}{2}\right\}\\ 0& 1& \Delta d_i\left(t\right)cos\left\{{\theta }_i(t-1)+\frac{\Delta {\theta }_i\left(t\right)}{2}\right\}\\ 0& 0& 1\end{array}\right]$$

(15)

$$\begin{array}{cc}\hfill & B_i\left(t\right)=\left[\begin{array}{cc}cos\left\{{\theta }_i(t-1)+\frac{\Delta {\theta }_i\left(t\right)}{2}\right\}& -\frac{1}{2}\Delta d_i\left(t\right)sin\left\{{\theta }_i(t-1)+\frac{\Delta {\theta }_i\left(t\right)}{2}\right\}\\ sin\left\{{\theta }_i(t-1)+\frac{\Delta {\theta }_i\left(t\right)}{2}\right\}& \frac{1}{2}cos\left\{{\theta }_i(t-1)+\frac{\Delta {\theta }_i\left(t\right)}{2}\right\}\\ 0& 1\end{array}\right]\hfill \end{array}$$

(16)

3.3. Absolute Positioning

For each vehicle i, the absolute measurement provided by the GPS sensor can be denoted as $Z_i\left(t\right)=\left(x_i^G,y_i^G\right)$, which satisfies the following equations:

$$Z_i\left(t\right)=H_i\left(t\right)X_i\left(t\right)+R_i\left(t\right)$$

(17)

where

$$H_i\left(t\right)=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]$$

(18)

$R_i\left(t\right)$ is the Gaussian white noise, which follows $N\left(0,{\sigma }_i{\left(t\right)}^2\right)$. Furthermore, the absolute localization measurement provided by the GPS at each time is completely independent of any existing estimates or measurements. So, absolute positioning with the GPS measurement by using the ISCIF can be degenerated into the fusion method proposed in [28], which can be denoted as follows:

$$\begin{array}{cc}\hfill K& =P_i\left(t\right)H_i^T{(H_i{P}_i\left(t\right)H_i^T+{\sigma }_i{\left(t\right)}^2)}^{-1}\hfill \\ \hfill X_i\left(t\right)& =X_i\left(t\right)+K(Z_i\left(t\right)-H_i{X}_i\left(t\right))\hfill \\ \hfill P_i\left(t\right)& =(I-K{H}_i)P_i\left(t\right)\hfill \\ \hfill P_{iI}\left(t\right)& =(I-K{H}_i)P_{iI}\left(t\right){(I-K{H}_i)}^T+K{\sigma }_i{\left(t\right)}^2{K}^T\hfill \\ \hfill P_{iD}\left(t\right)& =P_i\left(t\right)-P_{iI}\left(t\right)\hfill \end{array}$$

(19)

3.4. Relative Positioning

$\left[X_i^j\left(t\right)\right]$ is the box of the relative position data for vehicle i received from vehicle j. The relative positioning between vehicles is shown in Figure 4. The green arrow in Figure 4 represents the relative direction between vehicle i and vehicle j, and the yellow dot lines represent the direction of vehicle j. By using the relative data measured by vehicle j, it can be denoted in the coordinate system of vehicle j as follows:

$$\left[X_i^j\left(t\right)\right]=\left[\begin{array}{ccc}\left[cos\right]\left(\left[{\theta }_j\left(t\right)\right]\right)& -\left[sin\right]\left(\left[{\theta }_j\left(t\right)\right]\right)& 0\\ \left[sin\right]\left(\left[{\theta }_j\left(t\right)\right]\right)& \left[cos\right]\left(\left[{\theta }_j\left(t\right)\right]\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}[\Delta x_i^j\left(t\right)]\\ [\Delta y_i^j\left(t\right)]\\ [\Delta {\theta }_i^j\left(t\right)]\end{array}\right]+\left[\begin{array}{c}\left[x_j\left(t\right)\right]\\ [y_j\left(t\right)]\\ [{\theta }_j\left(t\right)]\end{array}\right]$$

(20)

where the functions $\left[sin\right]$ and $\left[cos\right]$ are the inclusion functions. The box of the relative estimate $[\Delta X_i^j\left(t\right)]={\left\{[\Delta x_i^j\left(t\right)],[\Delta y_i^j\left(t\right)],[\Delta {\theta }_i^j\left(t\right)]\right\}}^T$ is the relative data of vehicle i in the coordinate system of vehicle j at time t. It can be calculated as follows:

$$\begin{array}{cc}\hfill & \left[\Delta x_i^j\left(t\right)\right]=\left[cos\right]\left(\left[\alpha \left(t\right)\right]\right)\ast \left[d_{ij}\left(t\right)\right]\hfill \\ \hfill & \left[\Delta y_i^j\left(t\right)\right]=\left[sin\right]\left(\left[\alpha \left(t\right)\right]\right)\ast \left[d_{ij}\left(t\right)\right]\hfill \end{array}$$

(21)

where $\left[\alpha \left(t\right)\right]$ and $\left[d_{ij}\left(t\right)\right]$ are provided by the sensors, and we assume that the ${\sigma }_{\alpha }$ and ${\sigma }_d$ are the standard deviations of the sensors. So, they can be denoted as

$$\begin{array}{cc}\hfill \left[\alpha \left(t\right)\right]& =\left[\alpha \left(t\right)-3{\sigma }_{\alpha },\alpha \left(t\right)+3{\sigma }_{\alpha }\right]\hfill \\ \hfill \left[d_{ij}\left(t\right)\right]& =\left[d_{ij}\left(t\right)-3{\sigma }_d,d_{ij}\left(t\right)+3{\sigma }_d\right]\hfill \end{array}$$

(22)

Moreover, for the purpose of including the real position of vehicle i in the estimated relative interval as much as possible, we used tolerance variables (${\alpha }_{xj}$, ${\alpha }_{yj}$, and ${\alpha }_{\theta j}$) to optimize the box state of vehicle j$\left[X_j\left(t\right)\right]={\left\{\left[x_j\left(t\right)\right],\left[y_j\left(t\right)\right],\left[{\theta }_j\left(t\right)\right]\right\}}^T$ in the global coordinate system:

$$\begin{array}{cc}\hfill \left[x_j\left(t\right)\right]& =\left[x_j\left(t\right)-{\alpha }_{xj},x_j\left(t\right)+{\alpha }_{xj}\right]\hfill \\ \hfill \left[y_j\left(t\right)\right]& =\left[y_j\left(t\right)-{\alpha }_{yj},y_j\left(t\right)+{\alpha }_{yj}\right]\hfill \\ \hfill \left[{\theta }_j\left(t\right)\right]& =\left[{\theta }_j\left(t\right)-{\alpha }_{\theta j},{\theta }_j\left(t\right)+{\alpha }_{\theta j}\right]\hfill \end{array}$$

(23)

where $x_j\left(t\right)$, $y_j\left(t\right)$, and ${\theta }_j\left(t\right)$ are the results of Equation (19).

Since there are redundant relative estimates at each time step in our model, we reduce the redundancy by using the ICP at first. Details of the ICP method were presented in the previous section; we can assume that all relative position information of vehicle i after fusion using the ICP method is expressed by $\left[X_i^R\right]$ (as shown in Equation (5)).

With the same idea as that of Equation (23), we use parameters ${\beta }_{xi}$, ${\beta }_{yi}$, and ${\beta }_{\theta i}$ to include the real position of vehicle i as much as possible in the interval form. The input state box on the relative positioning step based on the result of the absolute positioning step in Equation (19) can be denoted as

$$\begin{array}{cc}\hfill & \left[x_i\left(t\right)\right]=\left[x_i\left(t\right)-{\beta }_{xi},x_i\left(t\right)+{\beta }_{xi}\right]\hfill \\ \hfill & \left[y_i\left(t\right)\right]=\left[y_i\left(t\right)-{\beta }_{yi},y_i\left(t\right)+{\beta }_{yi}\right]\hfill \\ \hfill & \left[{\theta }_i\left(t\right)\right]=\left[{\theta }_i\left(t\right)-{\beta }_{\theta i},{\theta }_i\left(t\right)+{\beta }_{\theta i}\right]\hfill \end{array}$$

(24)

At this time, the box state of vehicle i with relative information can be represented as follows:

$$\left\{\left[X_i\left(t\right)\right],P_{iI}\left(t\right)+P_{iD}\left(t\right),\left[X_i^R\left(t\right)\right]\right\}$$

(25)

where $\left[X_i\left(t\right)\right]$ is the state box of vehicle i at time t, $P_{iI}\left(t\right)$ and $P_{iD}\left(t\right)$ are the totally independent and maximum degree of correlated parts of the covariance matrix, respectively, and $\left[X_i^R\left(t\right)\right]$ is a result that contains all relative position information obtained with the ICP method.

Meanwhile, for the covariance component, the covariance of the relative estimate of vehicle i provided by vehicle j is determined with the following equations:

$$\begin{array}{cc}\hfill P_{X_i^j}\left(t\right)& =J_1{P}_j\left(t\right)J_1^T+J_2{Q}_j{J}_2^T\hfill \\ \hfill P_{X_{iI}^j}\left(t\right)& =J_1{P}_{jI}\left(t\right)J_1^T+J_2{Q}_{jI}{J}_2^T\hfill \end{array}$$

(26)

where $P_{X_i^j}\left(t\right)$ and $P_{X_{iI}^j}\left(t\right)$ are the covariance of the relative estimate and the covariance of the independent component of $P_{X_i^j}\left(t\right)$ at time t, respectively. $P_j\left(t\right)$ and $P_{jI}\left(t\right)$ are the covariance of the state of vehicle j in the global coordinate system and the independent component of $P_j\left(t\right)$, respectively. $Q_j\left(t\right)$ and $Q_{jI}\left(t\right)$ are the covariance matrices of zero-mean Gaussian white noise and the independent component of $Q_j\left(t\right)$ at time t, respectively. In addition, $J_1$ and $J_2$ are Jacobian matrices with respect to $X_j\left(t\right)$ and the relative estimate $\Delta X_i^j\left(t\right)$, respectively. They are denoted as

$$\begin{array}{cc}\hfill J_1& =\frac{\partial X_i^j}{\partial X_j\left(t\right)}\hfill \\ & =\left[\begin{array}{ccc}1& 0& -\Delta x_i^{j}sin{\theta }_j-\Delta y_i^{j}cos{\theta }_j\\ 0& 1& \Delta x_i^{j}cos{\theta }_j-\Delta y_i^{j}sin{\theta }_j\\ 0& 0& 1\end{array}\right]\hfill \\ \hfill J_2& =\frac{\partial X_i^j}{\partial \Delta X_i^j\left(t\right)}\hfill \\ \hfill & =\left[\begin{array}{ccc}cos{\theta }_j& -sin{\theta }_j& 0\\ sin{\theta }_j& cos{\theta }_j& 0\\ 0& 0& 1\end{array}\right]\hfill \end{array}$$

(27)

We can assume that the related covariance of $\left[x_i^R\right]$ equals the one with the smallest trace among all matrices $P_{X_i^j}\left(t\right)$. If k is the ID of the vehicle that has the smallest trace, the related covariance can be determined as follows:

$$\begin{array}{cc}\hfill P_{X_{iI}^R}& =P_{X_{iI}^k}\hfill \\ \hfill P_{X_{iD}^R}& =P_{X_{iD}^k}\hfill \end{array}$$

(28)

The relative positioning by using the ISCIF can be represented as follows:

$$\begin{array}{cc}\hfill P_1& =\frac{P_{iD\left(t\right)}}{w}+P_{iI}\left(t\right)\hfill \\ \hfill P_2& =\frac{P_{X_{iD}^R\left(t\right)}}{1-w}+P_{X_{iI}^R}\left(t\right)\hfill \\ \hfill K& =P_1{(P_1+P_2)}^{-1}\hfill \\ \hfill \left[X_i\left(t\right)\right]& =\left[X_i\left(t\right)\right]+K\left\{\left[X_i^R\left(t\right)\right]-\left[X_i\left(t\right)\right]\right\}\hfill \\ \hfill P_i\left(t\right)& =(I-K)P_1\hfill \\ \hfill P_{iI}\left(t\right)& =(I-K)P_{iI}{(I-K)}^T+K{P}_{X_{iI}^R}\left(t\right)K^T\hfill \\ \hfill P_{iD}\left(t\right)& =P_i\left(t\right)-P_{iI}\left(t\right)\hfill \end{array}$$

(29)

In order to form a loop with the state prediction step (Equation (10)) and absolute positioning step (as shown in Equation (19)), we calculate the input variables in the state prediction step by determining the midpoint of the output result in the state box form $\left[X_i\left(t\right)\right]$ in the relative positioning step (the result of Equation (29)). Additionally, a flowchart of our proposed cooperative localization method is illustrated in Figure 5.

4. Fault Detection and Exclusion

In order to enhance the robustness of the system, we implement a fault detection and exclusion (FDE) method based on Kullback–Leibler divergence (KLD). Our method mainly includes three steps: fault detection, threshold optimization, and fault isolation. Each vehicle in the system uses the proposed FDE method after the absolute positioning stage.

Considering two probability density functions (pdfs) ($f_1$ and $f_2$), the KLD from $f_1$ to $f_2$ can be denoted as

$$KLD(f_1\left|\right|f_2)=\sum _{x}log\frac{f_1\left(x\right)}{f_2\left(x\right)}{f}_1\left(x\right)$$

(30)

The KLD for vehicle i is computed based on the state vector and covariance matrix both before and after absolute positioning. The calculation formula for the KLD is as follows:

$$\begin{array}{cc}\hfill KL{D}^i& =\frac{1}{2}trace\left(P_{t/t}{P}_{t/t-1}^{-1}\right)+\frac{1}{2}log\frac{det\left(P_{t/t-1}\right)}{det\left(P_{t/t}\right)}-\frac{1}{2}M\hfill \\ & +\frac{1}{2}{(X_{t/t}-X_{t/t-1})}^T(X_{t/t}-X_{t/t-1})\hfill \end{array}$$

(31)

where $P_{t/t-1}$ and $P_t$ denote the total covariance of vehicle i before and after data fusion in the absolute positioning stage, respectively. $X_{t/t-1}$ and $X_t$ denote the state vector of vehicle i before and after data fusion in the absolute positioning stage, respectively. M is the dimension of the state vector. $trace\left(\right)$ and $det\left(\right)$ represent matrix operations for the calculation of the trace and determinant, respectively.

Equation (31) is a generalized residual test that considers both the Mahalanobis distance and mean differences between predicted and corrected distributions. Moreover, in the fault-free case, the distribution of the KLD is related to the F and Chi-square distributions, which are denoted in the following equation [36]:

$$\begin{array}{cc}\hfill KLD& \sim \frac{M(n-1)}{2n(n-M)}{F}_{M,n-M}+\frac{1}{2n-2}\hfill \\ & \ast \frac{1}{1-\frac{1}{6(n-1)-1}(2M+1-\frac{2}{M+1})}{\chi }_{\frac{1}{2}M(M+1)}^2\hfill \end{array}$$

(32)

where n is the number of samples and M is the dimension.

After obtaining the KLD residual, a threshold should be determined to evaluate whether there is a fault in the system. The decision includes the detection probability $p_D$ and the false alarm probability $p_F$, which can be denoted as

$$\begin{array}{cc}\hfill & p_D=p\left(s_1\right|H_1)\hfill \\ \hfill & p_F=p\left(s_1\right|H_0)\hfill \end{array}$$

(33)

where $H_1$ is the hypothesis that there is no fault in the system, $H_0$ is the hypothesis that there is a fault in the system, $s_1$ is the action of selecting hypothesis $H_1$, and $s_0$ is the action of selecting hypothesis $H_0$. We can assume that $H_0$ and $H_1$ are known:

$$\begin{array}{cc}\hfill & p\left(H_0\right)=p_0\hfill \\ \hfill & p\left(H_1\right)=1-p_0\hfill \end{array}$$

(34)

We define the pdfs of KLDs $f\left(KLD\right|H_0)$ and $f\left(KLD\right|H_1)$ as being under the $H_0$ and $H_1$ cases, respectively. They satisfy

$$\begin{array}{cc}\hfill & H_0:KLD\sim f\left(KLD\right|H_0)\hfill \\ \hfill & H_1:KLD\sim f\left(KLD\right|H_1)\hfill \end{array}$$

(35)

One example of these distributions is shown in Figure 6. So, the detection probability and the false alarm probability in Equation (33) can be calculated as follows:

$$\begin{array}{cc}\hfill & P_D={\int }_{\lambda }^{\infty }f\left(KLD\right|H_1)d\left(KLD\right)\hfill \\ \hfill & P_F={\int }_{\lambda }^{\infty }f\left(KLD\right|H_0)d\left(KLD\right)\hfill \end{array}$$

(36)

Threshold $\lambda$ can be optimized by minimizing the conditional entropy [41]. The formulation is shown in Equation (37).

$$\begin{array}{cc}\hfill h\left(H\right|s)& =E\left[log\frac{1}{p\left(H\right|s)}\right]\hfill \\ \hfill & =-\sum _{i\in [0,1]}(v_{i}log\frac{v_i}{v_i+k_i}+k_{i}log\frac{k_i}{v_i+k_i})\hfill \end{array}$$

(37)

So, combining Equations (33) and (37), when the conditional entropy is minimized, threshold $\lambda$ can be determined. The decision criterion based on the KLD and threshold can be expressed as

$$\begin{array}{cc}\hfill & s_0:KLD<\lambda \hfill \\ \hfill & s_1:KLD\ge \lambda \hfill \end{array}$$

(38)

When a fault is detected, an isolation method should be implemented to mitigate the impact of faulty data on the positioning accuracy. The strategy that we propose is that if a fault is detected after the absolute positioning stage of a vehicle, it should immediately cease cooperation with other vehicles to avoid affecting their positioning performance. In addition, given that we use the ICP method in the relative positioning stage, when a fault occurs in a relative position measurement sensor, the fault can be automatically detected during the execution of the ICP method.

For example, considering a system with three vehicles, if a fault is detected in the absolute positioning stage of vehicle 1, it withholds the provision of relative position measurements for vehicle 2 and vehicle 3 until the KLD is lower than the threshold. In addition, when no vehicles detect any faults in the absolute positioning stage and vehicle 1 detects no intersections between the relative position intervals provided by its neighboring vehicles during the execution of the ICP algorithm, this means that there is a fault in the relative position measurement of vehicle 2 or vehicle 3. In such cases, vehicle 1 should halt its relative positioning until the relative measurement data are deemed free of faults.

5. Simulation Results

5.1. Simulation Scenarios and Parameters

A comparison of the positioning performance of different techniques was performed based on the scenario illustrated in Figure 7. There were three vehicles that could observe each other, and the data sharing was performed by using the V2V communication technique. The reason for our selection of this scenario was that we focused on solving the data incest problem with high-redundancy data. The more mutual observation between vehicles, the more data redundancy there is. In addition, this scenario is a basic scenario with highly redundant data, and this is also common in real traffic transportation systems.

In this study, the proposed localization method, along with several other methods, was implemented using identical data, followed by a comparison of their respective performances. The methods being tested included the following:

• Extended Kalman filter (EKF) [42];
• Cubature Kalman filter (CKF) [23];
• Adaptive cubature Kalman filter (ACKF) [24];
• Split covariance intersection Kalman filter (SCIF) [28].

Without loss of generality, in order to thoroughly test the positioning performance with different absolute positioning abilities, we set up two simulation scenarios in [28]: The absolute positioning ability of all vehicles in the system was the same, and one vehicle in the system had an excellent absolute localization ability. Furthermore, the parameters in our simulations are shown in

Table 1. Details of the simulation parameters.

Parameter	Value
Discrete time step	0.1 (s)
Simulation duration	60 (s)
Velocity of vehicles	15 (m/s)
Standard error of velocity	0.2 (m/s)
Standard error of direction	0.3 (degree)
Standard error of relative distance	0.2 (m)
Standard error of relative orientation r	0.1 (degree)
Standard error of absolute positioning on x-axis	5 (m)
Standard error of absolute positioning on y-axis	5 (m)
Standard error of excellent absolute positioning on x-axis	0.5 (m)
Standard error of excellent absolute positioning on y-axis	0.5 (m)

. Simulation experiments were conducted to test and compare the accuracy of our proposed method with that of the SCIF method [28]. In addition, a third scenario was built in order to evaluate the performance of the implemented FDE method.

5.2. Results of Scenario 1: All Vehicles Have the Same Absolute Positioning Ability

Figure 8 displays the true trajectory and the estimated trajectories of vehicle 1 using different methods over a span of 1 min. It should be noted that our method can provide a real-time interval that includes the true position of the vehicle almost all the time.

In addition, Figure 9, Figure 10 and Figure 11 illustrate the root mean square errors (RMSEs) of vehicle 1, vehicle 2, and vehicle 3 in 30 rounds of simulations, respectively. The x coordinate indicates the test index, and the RMSE is indicated by the y coordinate. Moreover, average RMSEs of the different vehicles based on the different methods in scenario 1 are demonstrated in

Table 2. Average RMSEs of different vehicles based on different methods in scenario 1.

Methods	RMSE of V1	RMSE of V2	RMSE of V3	Average RMSE
Our method	1.498 m	1.424 m	1.444 m	1.455 m
SCIF	1.761 m	1.581 m	1.450 m	1.597 m
ACKF	1.693 m	1.661 m	1.650 m	1.668 m
CKF	1.791 m	1.793 m	1.800 m	1.790 m
EKF	2.455 m	2.475 m	2.430 m	2.450 m

. Based on our proposed method, each vehicle was able to achieve the most accurate localization result. In 30 tests, the average RMSEs for the three vehicles when utilizing the SCIF, ACKF, EKF, CKF, and our method were 1.597 m, 1.668 m, 2.450 m, 1.790 m, and 1.455 m, respectively. When all vehicles possessed similar absolute positioning capabilities, our method demonstrated an approximate reduction in the RMSE by 8.9%, 12.8%, 40.6%, and 18.7% compared to the SCIF, ACKF, EKF, and CKF methods, respectively.

5.3. Results of Scenario 2: One Vehicle Has an Excellent Absolute Positioning Ability

Figure 12 illustrates the true trajectory and the estimated trajectories of vehicle 1 when using different methods over a span of 1 min. Compared with the results in scenario 1, more closed trajectories were obtained by using the SCIF and our method. The reason is that the vehicle had an excellent absolute positioning ability, and it could obtain a more accurate absolute positioning result. In addition, the RMSEs of vehicle 1, vehicle 2, and vehicle 3 based on 30 rounds of simulations are shown in Figure 13, Figure 14 and Figure 15, respectively. Furthermore, the average RMSEs of the different vehicles based on different methods in scenario 2 are listed in

Table 3. RMSEs of different vehicles based on different methods in scenario 2.

Methods	RMSE of V1	RMSE of V2	RMSE of V3	Average RMSE
Our method	0.450 m	0.788 m	1.010 m	0.750 m
SCIF	0.440 m	1.033 m	1.190 m	0.888 m
ACKF	0.510 m	1.643 m	1.670 m	1.274 m
CKF	0.520 m	1.800 m	1.770 m	1.363 m
EKF	0.460 m	2.497 m	2.431 m	1.796 m

. The average RMSEs across all vehicles when using the SCIF, ACKF, EKF, CKF, and our method were 0.888 m, 1.274 m, 1.796 m, 1.363 m, and 0.750 m, respectively. Our method showcased reductions in the RMSE of 15.5%, 41.1%, 58.2%, and 45.0% compared to the SCIF, ACKF, EKF, and CKF methods, respectively. This effectively emphasized the advantageous suitability of our proposed method for vehicle localization in scenarios involving redundant data with MSMVs.

5.4. Results of Scenario 3: Cooperative Vehicle Localization with FDE

The values of the probability density function (pdf) of Kullback–Leibler divergence (KLD) in the both presence and absence of faults in the system were calculated based on empirical data [36]. Threshold $\lambda$ was approximately 2.137 when the conditional entropy was about 0.1267. In the absence of faults, the RMSE of vehicle 1 over the time steps is depicted in Figure 16. The KLD values based on the time steps are illustrated in Figure 17. We can notice that all of the KLD values were less than the threshold, which indicated the accuracy of the relationship between the threshold and KLD values.

In order to evaluate the effectiveness of our proposed FDE method, we added impulse noise to the sensor for achieving the absolute positioning of vehicle 1 from 51 s to 54 s [35]. The results of the RMSE based on the time step are shown in Figure 18. We can notice that the RMSE increased dramatically when there was a fault in the system. At the same time, the KLD value based on the time step is illustrated in Figure 19. At time 51 s (time step 510), the KLD value was immediately higher than the threshold, which showed the good real-time performance and sensitivity of our proposed FDE method. In addition, the KLD value was lower than the threshold value when there were no faults in the system.

After the fault detection, we implemented fault exclusion based on our proposed isolation strategy. The trajectories of vehicle 2 estimated using the FDE method and without using the FDE method when the sensor of vehicle 1 had a fault are shown in Figure 20. We can notice that our FDE method could provide a more accurately estimated position when there was a fault in the system. The RMSEs of vehicle 2 and vehicle 3 when their neighbor (vehicle 1) had a fault are illustrated in Figure 21 and Figure 22, respectively. By employing our proposed FDE method, there was about an average 55 percent reduction in the RMSE for vehicles 2 and 3, which indicated the effectiveness of the proposed FDE method.

6. Conclusions and Future Work

In this study, we proposed a new data fusion method named the ISCIF. In this method, the inverse of the interval matrix in the traditional IKF and IEKF was replaced by using a regular matrix, which prevented potential issues that could impact the data fusion accuracy. Then, the proposed ISCIF method was applied to both the absolute and relative positioning steps. It should be noted that the ICP method was employed in the relative positioning step to reduce redundancy in data on multiple relative positions. Moreover, a KLD-based FDE method was implemented in our localization system to enhance the robustness. Furthermore, we conducted simulations in three different scenarios. Both scenario 1 and scenario 2 were carried out to evaluate the accuracy of our proposed cooperative vehicle localization method. In scenario 1, it was assumed that all vehicles in the system had the same absolute positioning ability, while in scenario 2, vehicle 1 was assumed to have an excellent absolute positioning ability. Scenario 3 was carried out to evaluate the performance of the implemented FDE method. The simulation results showed that our proposed method could effectively address the data incest problem and enhance the localization accuracy in MSMVs. In comparison to the SCIF method, our approach could reduce the RMSE by 8.9% and 15.5% in the two different simulation scenarios, respectively. Additionally, in the presence of faults, the implemented FDE method effectively reduced the RMSE compared to situations in which the FDE method was not employed.

In future work, we would like to research the cubature split covariance intersection filter (CSCIF) [43] and adaptive cubature split covariance intersection filter (ACSCIF) [44].