Fusion Filter Overconfidence and Error Propagation

After presenting the IR-UWB TOF-based range observation model, we rely on a similar PF framework to that used in Chapter 3 for GNSS/ITS-G5 data fusion (see Algorithm 1), while benefiting from more accurate V2V range-dependent measurements and keeping on using ITS-G5 to broadcast fusion results. The new V2V measurements are also nonlinear with respect to the vehicles’ positions, thus somehow justifying the choice of a PF as core fusion engine. Our GNSS/ITS-G5/IR-UWB data fusion scheme is based on a (bootstrap)

Algorithm 3 Bootstrap PF for GNSS/ITS-G5/IR-UWB data fusion (iteration k, “ego”

vehiclei)

1: CAM Collection:Receive CAMs from the setN→i,k of perceived neighbors, extract the parametric beliefs, and draw samples to reconstruct the particle approximate beliefs{Xe^(p)_j,k,1/P}^Pp=1,j∈ N→i,k. 2: Data Resynchronization:Perform prediction of both “ego” and neighboring particle beliefs based

on mobility models at the “ego” estimation instantk(i.e.,ti,k):

X^(p)_i,k ∼p(Xi,k|X^(p)_i,k−1), w_i,k^(p)= 1/P, p= 1, . . . , P,

X^(p)_j,k

i ∼p(Xj,k_i|Xe^(p)_j,k), w^−(p)_j,k

i = 1/P, p= 1, . . . , P, j∈ N→i,k, and build the LDM of vehiclei’s neighbors (as another possible output of the algorithm):

Xb⁻_j,ki ≈ 1

3: Observation Query and Aggregation:Check whether the TDMA MAC superframe or the ranging handshakes with the subsetS→i,k ⊂ N→i,k of IR-UWB paired “virtual anchors” are completed to perform fusion-based CLoc:

zi,k=

( (z^x_i,k, z^y_i,k)^†, if non-fusion instantk, (z^x_i,k, z^y_i,k, . . . ,dbj→i,k, . . .)^†, j∈ S→i,k, if fusion instantk.

4: Observation Update: Calculate the new weights according to the likelihood:

w^(p)_i,k ∝

normalize them to sum to unity, and compute the approximate MMSE estimator and its empirical covariance as the second filter outputs:

Xbi,k≈

6: Message Approximation and Broadcast:Use parametric unimodal Gaussian to approximate the particle “ego” belief and thus broadcast{Xbi,k,Σi,k}.

PF, as described in Algorithm 3.

This algorithm uses the (joint) mobility model as sequential importance distribution, which does not account for the most recent observation. Hence, particles are generated from the mobility models (Algorithm 3, Step 2), whereas the corresponding weights are updated by simply computing the measurement likelihood given the current observation and the states of these predicted particles (Algorithm 3, Step 4). This suboptimal choice, unfortunately, can lead to specific problems as described below.

First, the efficiency of the bootstrap PF relies critically on the “match” between the prior distribution and the observation likelihood [87, 88]. Since the mobility model is not binded to the observation (and thus, to the likelihood), there might exist a “mismatch”

between them. For instance, if the ranging technology is highly accurate leading to

concen-200 400 600 800 1000

Figure 5.4: Illustration of particles depletion when fusing accurate IR-UWB ranges with GNSS (top subfigures) and no depletion when using inaccurate RSSIs and GNSS (bottom subfigures) in a bootstrap PF. In this scenario, the “ego” vehicle in the center cooperates with its eight nearest neighbors, as shown in Figure 5.7 in one snapshot. Left top/bottom subfigures illustrate the position estimate and the corresponding confidence ellipse at the

“ego” car, when fusing 8 IR-UWB ranges/RSSIs with respect to other cars with “ego”

and neighboring prior beliefs in comparison with theoretical BCRLB. Right top/bottom subfigures show the updated weights accounting for the collapsed/distributed particle cloud approximating the “ego” posterior density. Main simulation parameters include:

prior bias ∼ U(0,0.5) [m] to account for poor initialization, prior 1-σ uncertainty of 1 [m]

on bothx- andy-axes independently,σ_UWB= 0.2 [m],σ_Sh = 2.5 [dB], and 1000 particles.

trated (joint) likelihood but the mobility is not (due to either imperfect prediction model or poor initialization³), then only a few particles close to the true state are assigned sig-nificant weights, resulting in particle depletion. As a result, the posterior density support is concentrated to a submanifold of the state space, leading possibly to be overconfident in biased location estimates. Figure 5.4 illustrates this phenomenon with a single snap-shot simulation. If, on the one hand, the neighbors’ positions are perfectly known, which may not be reasonable in a pure VANET case, the “ego” posterior density is concentrated but located close to the true position. However such estimation is unstable since it does not fix the particle depletion. If, on the other hand, the neighbors’ positions are biased (either strongly or weakly), the corresponding error terms are propagated to the “ego”

3In general, it is reasonable to assume rather poor initial guess. For example, in order to perform V2V IR-UWB ranges, vehicles need to be paired. During this period, they can only rely on GNSS, which does not always provide accurate location beliefs.

localization error [m]

0 0.5 1 1.5 2

empirical CDF(error)

0 0.2 0.4 0.6 0.8 1

filtered GNSS CLoc (GNSS + ITS-G5)

CLoc (GNSS + IR-UWB) (bias propagation) biases

Figure 5.5: Illustration of bias propagation while fusing accurate IR-UWB ranges with GNSS in a bootstrap PF. In spite of the accurate ranges, the GNSS+IR-UWB only gives similar accuracy performance as that of the GNSS+ITS-G5. In addition, its accuracy is the worst in low error regime due to marked biases. The simulation parameters are taken from the heterogeneous GNSS scenario detailed in Section 5.5.

position estimate, which thus quickly converges to an inaccurate value, whereas extremely high confidence is still given to the result (see Figure 5.4 (left top)). Such a situation can be fatal: this malicious information is then broadcast over the network and degrades the position accuracy of all neighbors. Note that the particle depletion does not occur when fusing inaccurate RSSIs because their (joint) likelihood is a broad distribution, indicating that most particles retain a meaningful weight (Figure 5.4 (bottom)).

Second, though the bootstrap PF is implemented in a distributed manner, in the VANET context, the state must be augmented to account for uncertain “virtual anchors”

positions i.e., neighboring beliefs (see Algorithm 3, Step 2 and 4). Said differently, the position estimation is performed in high-dimensional space. In this case, there might be no particle in the vicinity to the correct augmented state because the number of particles cannot be sufficiently high to cover all relevant regions concerned by the concentrated (joint) likelihood density [88].

Hence, jointly or separately, the compact distribution of the measurements (e.g., using accurate IR-UWB ranges) and the high dimensionality of the state space both lead to the inefficiency of the bootstrap PF in the very fusion context. Figure 5.5 illustrates this counter-intuitive observation.

To avoid particle depletion, we aim at having more particles with significant weights in order to maintain particle diversity and therefore, to avoid overconfidence issues. One

immediate and intuitive approach is to increase the number of particles. Such a solution can solve the problem at the expense of extremely high computation load, as shown in Figure 5.6 (top). However, it is unsuitable to real-time vehicular tracking under high mobility. Thus, we solve the problem without increasing the number of particles in the following sections.