IEEE INTELLIGENT TRANSPORTATION SYSTEMS MAGAZINE
•
85
•
SUMMER 2015
19391390/15©2015IEEE
Mohsen Rohani and Denis Gingras
Electrical and Computer Engineering Department, Université de Sherbrooke, Canada Emails: mohsen.rohani@usherbrooke.ca, denis.gingras@usherbrooke.ca
Vincent Vigneron
Université d’Evry, France Email: vincent.vigneron@ufrst.univevry.fr
Dominique Gruyer
IFSTTAR  IM  LIVIC, Satory, France Email: dominique.gruyer@ifsttar.fr
A New Decentralized Bayesian Approach for Cooperative Vehicle Localization Based on Fusion of GPS and VANET Based InterVehicle Distance Measurement
Introduction
ccurate and reliable vehicle localization is a key component of numerous automotive and Intelligent Transportation System (ITS) applications, including active vehicle safety systems, real time estimation of traffic conditions, and high occupancy tolling [1]. Various safety critical vehicle applications in particular, such as collision avoidance or mitigation, lane change management or emergency braking assistance systems, rely principally on the accurate and reliable knowledge of vehicles’ positioning within given vicinity. Distributed algorithms as proposed in [2] and [3] have under
lined a recent and important interest for the collaborative localization. Since the number and type of sensors used in vehicular applications increases, it is essential to find ways to better analyze and extract useful data from these sensors and share them between vehicles when it is relevant. Sensors which are used in localization can be divided in two categories: proprioceptive and exteroceptive sensors [4]. Proprioceptive sensors are those which can provide
Digital Object Identifier 10.1109/MITS.2015.2408171 Date of publication: 20 April 2015
IMAGE LICENSEDBY INGRAM PUBLISHING
A
IEEE INTELLIGENT TRANSPORTATION SYSTEMS MAGAZINE
•
86
•
SUMMER 2015
information about the vehicle dynamic states like position, velocity and acceleration (GPS, accelerometer, gyroscope etc.). Exteroceptive sensors provide information about the states of the environment (video camera, lidar, etc.). GPS is one of the most common positioning devices being used in vehicle localization as it provides absolute position of the vehicles, whereas dead reckoning sensors such as odometers or INS (Inertial Navigation Systems) provide relative information only. GPS signals are however subject to different sources of noise, and degradation as well as being subject to temporary signal loss in cluttered environment. Many of the intelligent vehicles systems like safety systems can benefit from more accurate and reliable positioning [5]. Data fusion technique is one of the common ways to improve the position estimate by exploiting the information coming from multiple sensors [6–8].
In the recent years, vehicular ad hoc network (VANET) applications has become of great interest [9–11]. With the
recent emergence of multivehicular wireless communication capabilities, cooperative architectures have become an attractive alternative for solving the localization problem [1, 9, 10, 12–20]. The main goal of cooperative localization is to
exploit different sources of information coming from different vehicles within a short range area, in order to enhance positioning system efficiency while keeping the computing cost at a reasonable level. In other words, vehicles share their location and environment information to others in order to increase their own global perception. Some of the most prominent approaches for cooperating localization are based on Kalman filtering [21–23], Bayesian methods [24],
Markovian modeling [25], maximum likelihood methods [26] and Split covariance intersection filter [27, 28].
In addition to the information exchanging, VANET has brought the capability for communicating vehicles to use their wireless communication devices to measure the distance between each other. There are basically two methods of measuring distance between communicating devices. The first method is the TimeofFlight (TOF) which is based on the time that it takes for a signal to travel from one node to another node, where the nodes are vehicles in a VANET. The second method is the Radio Signal Strength (RSS) which is based on the attenuation of the signal strength while traveling from the transmitter node to the receiver node [29]. Using the communication devices to measure distance between vehicles has several advantages and disadvantages over the other range measurement devices like radar and lidar. One of the advantages is that the vehicle identification problem and the data association between the received information and range measurements is easier to solve. Another advantage of these method is that its detection performance in the crowded areas is better since it can still be used while an object or another vehicle blocked the line of sight between two vehicles but it does not block the line of sight between antennas. However the disadvantages of these methods over radar and lidar is that in their general form they cannot provide the relative bearing between the vehicles and they can only provide the distance. In addition to this, lidars usually can provide more accurate measurements. Although not all of the RF range measurement methods can provide the acceptable accuracy needed for our method, there are several more accurate RF based methods such as [30] and [31, 32] which
can provide the needed accuracy to be used in our method. In [30] a low cost accurate radio ranging technic is proposed and field trials has been conducted in different environments to characterize the ranging error.In this article, we aim to improve the GPS vehicle position estimates by using available VANET based intervehicle distance measurements in a cluster of vehicles. The reason that we decided to use VANET based intervehicle distance measurements is that in addition to the mentioned advantages, using this method of distance measurements reduces the cost of the system as it does not need a new range sensor to be used and vehicles can use their existing communication device to measure the distance.Our proposed cooperative vehicle localization method is a decentralized Bayesian approach which allows a vehicle to incorporate its GPS position estimate with other vehicles GPS data and intervehicle distance measurements. Unlike [24, 25], which basically have been developed for indoor
robotic applications, our method is developed for outdoor usage and automotive applications. Also this method is taking the advantage of using GPS which is available in outdoor usages. Our method should be seen as a prefiltering of GPS positioning measurement using intervehicle distances and other vehicles GPS measurements, prior the tracking algorithms such as the Extended Kalman Filter (EKF) [33]. Therefore this method has the advantage to be incorporated with any existing ego localization algorithm which uses GPS (see Fig. 1). Furthermore, the data dependency problem, which is a common issue in probabilistic approaches [24–26] and
which leads to over convergence, has been circumvented in our approach. Another advantage of this method is the ability to use the true probability distribution model of distance measurement sensors instead of using their Gaussian approximation which is usually being done in Kalman based methods. In addition to the performance analysis of the method, the Sensitivity of the proposed method to the vehicle to vehicle distance measurement accuracy, communication latency and communication failure is also studied. The results presented in this article are based on Monte Carlo simulations and the data used in these simulations were provided by ProSiVIC software from Civitec and GPSoft Satellite Navigation toolbox for MATLAB.The focus of this article is to describe the derivation of the Bayesian cooperative vehicle localization method and demonstrate its performance by integrating this algorithm
IEEE INTELLIGENT TRANSPORTATION SYSTEMS MAGAZINE
•
87
•
SUMMER 2015
with a Kalman filter to incorporate the dynamic properties of the vehicle.
Proposed Method
The proposed method aims to improve GPS vehicle positioning using additional intervehicle distances and vehicletovehicle (V2V) communication capabilities in a cluster of vehicles. We assume that each vehicle is able to estimate its position and respective covariance matrix using its embedded GPS receiver independently. We consider also that each vehicle is able to estimate its distance to other vehicles, using a VANET based method and independent from their GPS signals [30–32, 34], we refer to these method as Range
Sensor in the rest of this article. Finally, it is assumed that the vehicles share their information by means of a VANET. With their GPS receivers, the vehicles can calculate the pseudo ranges between them and the visible satellites. These pseudo ranges has a standard deviation which is referred as
UERE
v
[35]. With the following notation:
Nth Vehicle
V
=
(1)
True Position of
X V
( )
0
=
(2)
Estimated positionof
X V
( )
=
t
(3)
True
distancebetween and
D V V
( )
0
=
(4)
Estimatedd
istancebetween and
D V V
( )
=
t
(5) where
X
( )
0
and
X
( )
t
are two dimensional vectors providing the (unknown) true position and estimated position of
V
respectively. The distance between two vehicles for the nonoise case is simply given by:
,
D X X
( ) ( ) ( )
ij i j
0 0 0
= 
(6) where
D
( )
0
is the distance between
X
( )
0
and
.
X
( )
j
0
As said before, we assume that
( )
t
is measured independently from
X
( )
t
and
X
( )
j
t
and that we have a zero mean additive noise on the GPS estimated positions:
( , ) ( )
X N X R f x
( ) ( ) ( )
N N X X N
0
( ) ( )
N N
+
=
(7) where
R
X
( )
N
is the covariance matrix of the position and
f
X
( )
N
is the probability density function of
.
X
( )
Also, we assume that the intervehicular distances have a zero mean additive noise:
( , ) ( )
D N D f d
( ) ( ) ( )
i j ij D Di j
02
( ) ( )
ij ij
+
v
=
(8) where
( )
0
is the true distance between
V
and
,
V
j
2
( )
v
is the distance variance corresponding to the accuracy of the sensors. Here it is important to mention that these assumptions (7, 8) is only made to simplify the problem; our method is not restricted to them. This method can work with any other probability distribution model which can better describe the properties of the range measurement method. Now it is desired to estimate
X
( )
from the observations. Let us define:
, ,
X D
( ) ( ) ( )
=
" ,
(9) where
( )
is the observation of
X
( )
j
and
( )
by
.
V
Therefore
X
( )
estimates the position
( )
from
.
( )
( )( )
,cossin
D D i j
( ) ( )( )( )
Y ij j i j i j
!
ii
= 
= G
(10) where
i
is the bearing of the intervehicle distance measurement. Since we assumed that our distance measurement device is only able to provide us with the intervehicle distance and not the bearings the value of
i
is unknown. Therefore the solution space for
( )
is a circle around
X
( )
j
with the radius of
.
( )
A. The 2 Vehicles Case
Let us first consider a simple 2vehicle scenario. From a probabilistic point of view, we have:
( )( )( )
. ( )
f x x f x f x x f x
( ) ( )( )( ) ( ) ( )
X X iY ij X Y ij X X Y ij i X i
( ) ( )( )( ) ( ) ( )
iY ij Y ij Y ij i i
e
=
ee
(11)Eq. (11) is an application of the Bayes theorem, where the left member is the posterior probability to observe
x
( )
given
x
( )
and
( )
f x x
( ) ( )
X X
Y i i
( ) ( )
Y i i
e
e
is the likelihood of the observations. Using our model (10) we find:
( )
( , )
F x f x d dd dx
( ) ( ) ( ) ( ) ( )
X Y ij X D j ij ij x x j
( ) ( ) ( )( ) ( )
Y ij j ij j Y ij
=
333
+
# #
(12)
Satellites’SignalsRange SensorGPS ReceiverVANETNewEstimatedPosition &Covariance
Inside a Vehicle
ProposedMethodCooperativePositionEstimatesUpdateCovarianceUpdateEstimateKalmanGainProjectto K
+
1Data Signals Sent to Other Vehicles
Kalman Filter
Data SignalsReceived fromOther Vehicles
FIG 1
Schematic of the system, the input data (blue), the proposed method (green), the Kalman filter (orange) and the new Position Estimation.
IEEE INTELLIGENT TRANSPORTATION SYSTEMS MAGAZINE
•
88
•
SUMMER 2015
Differentiating with respect to
x
( )
we obtain:
( )
( , )
f x f x x x
dx
( ) ( ) ( ) ( ) ( )
X Y ij X D j j Y ij j
( ) ( ) ( )
Y ij j ij
= 
33
+
#
(13)Since
( )
j
and
D
( )
are independent, we have:
( ) ( ) ( )
f x f x f x x dx
( ) ( ) ( ) ( ) ( )
X Y ij X j D j Y ij j
( ) ( ) ( )
Y ij j ij
= 
33
+
#
(14)Similarly we can obtain:
( ) ( )
. ( )
f x x f x x f x x x dx
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
X X Y ij i X j i D j Y ij i j
( ) ( ) ( ) ( )
Y ij i j ij
e e e
= 
e33
+
#
(15)
X
( )
j
is independent of
( )
and the second term in the integral (15) is the probability that
V
and
V
j
observe each other at the given distance
,
x x
( ) ( )
j i

Which becomes like a doughnut whose center is
x
( )
(see Fig. 2). Therefore (15) becomes:
( ) ( ). ( )
f x x f x f x x dx
( ) ( ) ( ) ( ) ( ) ( )
X X Y ij i X j D j i j
( ) ( ) ( ) ( )
Y ij i j ij
e
= 
e33
+
#
(16)Also we have:
( ) ( ). ( )( ). ( ). ( )
f x f x f x x dx f x f x f x x dx dx
( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
X Y ij X i X X Y ij i i X i X j D j i j i
( ) ( ) ( ) ( )( ) ( ) ( )
Y ij iY ij ii j ij
e
= =
e333333
+++
# # #
(17)As we do not know the true position of the vehicles neither the distance between them, we use instead, the PDFs of the estimates coming from the GPS receivers and range sensors:
( ) ( , )
f x
N X R
( ) ( ) ( )
GPSGPS GPS X i i i
( )
i
=
t t
(18) where,
( )
GPSi
t
is the position estimate obtained from the GPS receiver of
V
and
R
( )
GPSi
t
is its estimated covariance.
( )
( , )
f d N D
( ) ( )
RS RS RS Dij ij
2
( )
ij
v
=
t
(19) where,
D
( )
RSij
t
is the distance estimate between
V
and
V
j
given by the range sensor (
RS
) and
R
2
v
is the variance of distance measurements using range sensor.Our prior PDF for
( )
is therefore the one of its GPS estimate:
( ) ( )
f x f x
( ) ( )
GPS X i X i
( ) ( )
i i
=
(20)In a similar way for the intervehicular distances, we use the PDF of our range sensor measurement:
( ) ( )
f d f d
( ) ( )
RS Dij Dij
( ) ( )
ij ij
=
(21)As an example, Fig. 2 shows the real positions, estimated GPS positions for
V
and
V
along with their PDF in red and the intervehicle distance PDF centered at
.
x
( )
The uncertainty ellipses have been drawn using a 3 sigma deviation from the center. Substituting (20) and (21) in (16) and (17) we have:
( ) ( ). ( )
f x x f x f x x dx
( ) ( ) ( ) ( ) ( ) ( )
GPS RS X X Y ij i X j D j i j
( ) ( ) ( ) ( )
Y ij i j ij
e
= 
e33
+
#
(22)And,
( ) ( ). ( )( ). ( ). ( )
f x f x f x x dx f x f x f x x dx dx
( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( ) ( )
GPSGPS GPS RS X Y ij X i X X Y ij i i X i X j D j i j i
( )( ) ( ) ( )( ) ( ) ( )
Y ij iY ij ii j ij
e
= =
e333333
+++
# # #
(23)Therefore by substituting (22) and (23) in (11) we obtain the following posterior PDF for
:
V
( )( ). ( ). ( )( ). ( ). ( )
f x x f x f x f x x dx dx f x f x f x x dx
( ) ( )( ) ( ) ( ) ( ) ( ) ( )( ) ( ) ( ) ( ) ( )
GPS GPS RSGPS GPS RS X X
iY ij X i X j D j i j i X i X j D j i j
( ) ( )( ) ( ) ( )( ) ( ) ( )
iY ij i j ij i j ij
e
=
e333333
+++
# # #
(24)The corresponding maximum a posteriori probability (MAP) estimator is given by:
argmax X f x x
i x X iY ij
ii X Y ij
=
t
^
^ ^ ^
^^ ^
h
h h h
hh h
(25) where
t
^ h
is the new estimated position for
.
V
Fig. 3 shows this posterior PDF for
i
1
=
and
,
j
2
=
obtained from (24). As one can foresee, such a Bayesian approach allows one vehicle to exploit the information contained in the other vehicles GPS position and in the range sensor measurements to improve its own position estimate and more importantly reduce its uncertainty.
B. The N Vehicles Case
Now, let us consider the case of
N
1
+
vehicles present in a cluster. We assume that each vehicle has its own GPS position estimate and that all the intervehicle distances can be measured. We also assume that all range sensors and GPS
V i V j X (i)
FIG 2
Real positions (squares), estimated GPS positions (dots), their PDF and uncertainty ellipses (red) and intervehicle PDFs centered at
x
i
^ h
(blue) for .
m
65
UERE
v
=
^
h
and ..
m
45
RS
v
=
^
h
IEEE INTELLIGENT TRANSPORTATION SYSTEMS MAGAZINE
•
89
•
SUMMER 2015
receivers are the same, within the vehicle cluster, leading to identical prior distributions. Again this is only to simplify the equations but in practice, for each GPS receivers and range sensors, we can use their own prior distribution. We then have:
( ,
..., )
f x x x
,...,( ) ( ) ( )
X X X iY iY
1
( ) ( ) ( )
1
e
=
e
( ,..., ). ( )( ,..., ). ( )
f x x x f x dx f x x x f x
,...,( ) ( ) ( ) ( ) ( ),...,( ) ( ) ( ) ( )
X X X Y iY
N
X i i X X X Y iY
N
X i
11
( ) ( ) ( ) ( )( ) ( ) ( ) ( )
Y iY
N N
11
ee
e33e
+
#
(26)Considering that
,
...,
( ) ( )
iN
1
are independent from each other, equation (26) becomes:
( ,..., )( ). ( )( ). ( )
f x x x f x f x x dx f x f x x
,...,( ) ( ) ( )( ) ( ) ( ) ( )( ) ( ) ( )
X X X
i
Y
i
Y
iN
X
i
X X
Y
i i i N i
X
i
X X
Y
i i i N
111
( ) ( ) ( )( ) ( ) ( )( ) ( ) ( )
i
Y
i
Y
iN i
Y
i ii
Y
i i
1
eee
=
ee33e
!!
=+=
%% #
(27)By substitution of (22) in (27) we have:
,...,
f x x x
,...,( ) ( ) ( )
X X X iY iY
1
( ) ( ) ( )
1
e
=
e
^ h
( ) ( )
. (
)
U f x f x f x x dx
( ) ( ) ( ) ( ) ( )
GPS GPS RS X i X j D j i j j j i N
1
( ) ( ) ( )
i j ij
#

33
!
+=
% #
(28) where
U
is a normalization factor that can be computed by substituting (22) into(27):
( ) ( )
. (
)
U f x f x f x x dx dx
( ) ( ) ( ) ( ) ( ) ( )
GPS GPS
X i X
j D
RS i j j j j i N i
1
( ) ( ) ( )
i j ij
#
= 
3333
!
+=+
% # #
(29)Finally, the MAP estimator for the position of vehicle
V
given the
N
other vehicles, is given by:
, ,
argmax X f x x X
, ,
i x X X X iY iY
iN
1
( )
iY iY iN
1( )
i
e f
=
e f
t
^
^ ^ ^ ^
^ ^
h
h h h h
h h
(30)Since there is no analytical solution for equation (30), a particle filter [33] has been applied in order to solve this equation. A sample set of 1000 particles has been selected for each
,( ... )
X j
1
( )
=
according to its PDF
( )
.
f x
( )
GPS X
j
( )
j
The results shown here have been obtained with 5 filter iterations.Fig.4 shows the posterior probability (28) for
V
1
in a cluster of 5 vehicles. The new position estimation for
V
1
is calculated using (30) and is shown as a star in the central green area. As we can see from Fig. 4 the new estimated position is much closer to the real position of
V
1
and the uncertainty has been reduced as the green area illustrated in Fig. 4 is smaller in size than the red area. Since the most computationally complex part of this algorithm is solving (30) with a particle filter, therefore the order of this algorithm is
( )
,
O N N
p
2
$
where
N
is the number of vehicles in the cluster and
N
p
is the number of particles. With proper choose of
N
p
and
N
each iteration of the algorithm can run in realtime and the result can be sent for further use in the tracking algorithms.One should note that depending on the method chosen for estimating the intervehicular distances, different methods for identifying neighbor vehicles and associating the measured distances with the received information from VANET can be applied. Although in the case of VANET based distance measurement methods there is no need to use any special method because vehicles can be identified and data can be associated using their communication device MAC address, However in the case of Radar and Lidar, different approaches can be utilized like the one used in [21] which is based on the Mahalanobis distance between position estimations. Details on association and identification methods is outside the scope of this article as we will focus only on the fusion method for cooperative localization.One important point that must be mentioned here with our Bayesian approach is the
overconvergence
issue. This usually occurs when the fused information coming from different vehicles are not independent of each other. For each time step of our method, only the GPS and intervehicle distance measurements of the vehicles present in the cluster are used in the calculation of the posterior PDF. These
V
2
V
1
FIG 3
Posterior probability of
V
1
and its uncertainty ellipse (green), new estimated position (star on green area), real position (squares), estimated GPS position (star on red area), their PDF and uncertainty ellipses (red) for .
m
65
UERE
v
=
^
h
and ..
m
35
RS
v
=
^
h
V
1
V
3
V
4
V
5
V
2
FIG 4
Posterior PDF of
V
1
in a cluster of 5 vehicles (green), real position (squares), new estimated position for
V
1
(star on green area) for.
m
65
UERE
v
=
^
h
and ..
m
15
RS
v
=
^
h