arXiv:1606.03736v2 [cs.RO] 31 Aug 2016

Enhancement of Low-cost GNSS Localization in
Connected Vehicle Networks Using
Rao-Blackwellized Particle Filters
Macheng Shen
Department of Naval Architecture
and Marine Engineering
University of Michigan
Ann Arbor, Michigan 48109
Email: macshen@umich.edu

Ding Zhao
Transportation Research Institute
University of Michigan
Ann Arbor, Michigan 48109
Email: zhaoding@umich.edu

Abstract—An essential function for automated vehicle technologies is accurate localization. It is difficult, however, to
achieve lane-level accuracy with low-cost Global Navigation
Satellite System (GNSS) receivers due to the biased noisy
pseudo-range measurements. Approaches such as Differential
GNSS can improve the accuracy, but usually require an enormous amount of investment in base stations. The emerging
connected vehicle technologies provide an alternative approach
to improving the localization accuracy. It has been shown in this
paper that localization accuracy can be enhanced by fusing
GNSS information within a group of connected vehicles and
matching the configuration of the group to a digital map to
eliminate the common bias in localization. A Rao-Blackwellized
particle filter (RBPF) was used to jointly estimate the common
biases of the pseudo-ranges and the vehicles positions. Multipath
biases, which are non-common to vehicles, were mitigated by
a multi-hypothesis detection-rejection approach. The temporal
correlation was exploited through the prediction-update process.
The proposed approach was compared to the existing static and
smoothed static methods in the intersection scenario. Simulation
results show that the proposed algorithm reduced the estimation
error by fifty percent and reduced the estimation variance by
two orders of magnitude.
Index Terms—Cooperative vehicle localization, GNSS, Map
matching, Multipath biases, Rao-Blackwellization, Particle filter.

I. I NTRODUCTION
An essential function of intelligent transportation systems
is accurate localization. One common solution to the vehicle localization problem is the Global Navigation Satellite
System (GNSS) which calculates the absolute position of
the vehicle from pseudo-range measurements. Pseudo-range
measurements are different from the actual distances between the receiver and the satellites. This difference results
in localization inaccuracy, which can be decomposed into
common errors due to satellite clock error, ionospheric and
tropospheric delay and non-common errors due to receiver
noise and multipath biases. The nominal accuracy for a
single-band receiver is about 10 to 20 meters, which results in
a position error of several meters [2]. Thus, it is very difficult

Jing Sun
Department of Naval Architecture
and Marine Engineering
University of Michigan
Ann Arbor, Michigan 48109
Email: jingsun@umich.edu

to achieve lane-level accuracy robustly using low-cost GNSS
receivers alone.
Differential GNSS (DGNSS) is an enhancement to GNSS
that improves the localization accuracy to the sub-meter
level through correction of the common biases broadcasted
by a network of fixed reference stations. Centimeter-level
accuracy is achievable by the Real Time Kinematic (RTK)
technique, which uses carrier phase measurements to provide
real-time corrections [3]. These techniques, however, rely on
infrastructure that is expensive and not readily available. It
would be highly desirable, therefore, to develop low-cost
solutions for lane-level accurate localization.
II. R ELATED W ORKS
The accuracy of GNSS localization can also be improved
using either sensor fusion or map matching, with some
promising results shown in recent years [4], [5], [6].
Another low-cost navigation solution is the Inertial
Navigation System (INS). The problem, however, is that
due to sensor noise, the navigation error using INS alone
can grow without bounds. In contrast, GNSS provides
an absolute position measurement whose error is always
bounded. Data fusion algorithms such as Extended Kalman
Filter (EKF) [7], Unscented Kalman Filter (UKF) [8] and
particle filter [9] can be used to fuse the complementary
measurements from low-cost INS and GNSS sensors to
produce high accuracy navigation solutions.
With the rapid development of digital maps, navigation
algorithms based on map matching have also been
extensively studied [5], [6], [10], [11]. Map matching
algorithms match the noisy raw GNSS positioning results
to a trajectory that is compatible with road constraints.
Additional sensors such as cameras allow for accurate
localization down to the centimeter-level [11]. Nevertheless,
these approaches all require infrastructure or additional
sensors to reach lane-level accuracy. Wide application is
unlikely in the near future. Therefore, the ability to use

low-cost GNSS receivers alone while achieving the same
degree of accuracy is highly desired.
Recently, Dedicated Short Range Communications
(DSRC)–based V2V technique has been developed and
validated in the real world [12]. The introduction of
connected vehicles provides an alternative for improving
navigation solutions through V2X, such that map matching
can be conducted cooperatively to correct the common
localization error [13]. Assuming that most vehicles travel
within lanes, the correction to the common localization
biases can be determined so that the corrected positions
of a group of vehicles best fit the road map. This requires
communication within the group of vehicles, hereafter
referred to as cooperative map matching (CMM). Schubert
et al.[14] have shown the advantage of exchanging raw
GNSS measurements and vehicle relative distances have
been shown. More recently, the capability of CMM to
mitigate the biases from the localization results obtained
from low-cost GNSS receivers alone has been demonstrated
in Rohani et al.[1].
Three main difficulties arise for CMM using low-cost
GNSS receivers alone are:
•

•

•

Effect of non-common error: In the presence of noncommon errors, a correction for the common localization error that makes all vehicle positions compatible
with the road constraints, may not exist. If the road
constraints are enforced aggressively, without consideration for the non-common errors, the localization
solution may overly converge, that is, the variance of
the estimation may be underestimated.
Correlation between common biases and vehicles positions estimation: An estimation error of the common
biases may induce an estimation error of the vehicles
positions and vice versa. If they are estimated sequentially and the respective estimation results are then fused
with each other for a more comprehensive estimation,
the corresponding data fusion scheme must be carefully
selected to avoid over-convergence due to fusion of
correlated data.
Application of road constraints: Road constraints may
be regarded as additional observations to correct for the
estimation of vehicles positions. However, unlike sensor observations, road constraints cannot be described
analytically by observation equations, which makes it
almost impossible for traditional filtering schemes based
on the Kalman filter to implement the map matching.

Rohani et al. [1] presented a particle-based CMM algorithm to address these three difficulties. The first difficulty,
that of the effect of non-common pseudo-range error was
considered by a weighted road map approach to preserve
consistency. The second difficulty is handled by tracking
the origin of the common bias corrections from different
vehicles and fusing only those corrections from independent
sources to avoid data incest, which effectively avoids overconvergence, though some of the correlated corrections con-

taining additional information have to be discarded. The third
difficulty is handled by a particle-based method that uses
only the vehicle positions’ estimation of the current epoch.
Algorithms that better utilizes all available data are expected
to yield improved localization performance.
In this work, the problem of inferring the true vehicle positions, as well as the GNSS common biases from the biased
noisy pseudo-range observables of a group of vehicles given
a precise digital map, is addressed by a Bayesian filtering approach. The aforementioned difficulties are solved by jointly
estimating the common biases and vehicle positions using a
Rao-Blackwellized particle filter (RBPF). In the RBPF, the
correlation between common biases and vehicle positions is
modeled implicitly through the diversity of the particles. As a
result, there is no need for explicit data fusion. The effect of
the multipath biases is mitigated through a detection-rejection
method based on a statistical test. The particle filter structure
allows multiple hypotheses with respect to the detection of
multipath biases, thus making the detection more robust. In
addition, the particle filter is flexible enough to handle road
constraints in a straightforward manner by manipulating the
particle weights according to the road constraints. It also fully
exploits the temporal correlation through a prediction-update
process, eliminating impossible configurations in the joint
space of common biases and vehicle state variables, drastically reducing the estimation variance. The computational
complexity only varies linearly with respect to the number
of vehicles, which makes the proposed RBPF both effective
and efficient.
In the following sections, the formulation and implementation of the RBPF for CMM are presented. In Section 2,
the derivation of the RBPF is presented in detail, and the
localization enhancement algorithm is described with the
assumptions under which the algorithm is valid. Simulation
results are presented in Section 3. The simulation scenario
and noise models are introduced, followed by the performance analysis of the RBPF as it is compared with those of
the existing methods. Conclusions are presented in Section
4.
III. T HEORY AND M ETHOD
In this section, the theoretical aspect of the RBPF is
introduced first. Then the prediction-update structure of the
RBPF is shown in detail.
Particle filter takes the Bayes filter algorithm and uses
a set of particles to represent the posterior probability
distribution of the estimated quantities conditioned on the
measurements. In the prediction step, each particle predicts
its state according to the process model; in the update step,
the weight of each particle is calculated according to the
principle of importance sampling given the measurements;
then the particles are resampled according to the weight to
avoid weight degeneracy [15]. Although the particle filter
is computationally intractable if the dimension of the state
vector is high, the Rao-Blackwellization technique takes
advantage of the conditional independence property so that it

is only necessary to represent a subspace of the state vector
using particle filters, conditioned on which the remaining
independent subspace can be marginalized out analytically
[16].
The following assumptions are made in this work:
1) The non-common errors of different vehicles are uncorrelated.
2) The common biases vary slowly over time.
3) The vertical positions of the vehicles can be obtained
with reasonable accuracy from the digital map.
Assuptions (1) - (3) are unrestrictive in the following
senses. The first assumption is valid as long as the participating vehicles are not concentrated in the same area; otherwise,
the multipath errors may be correlated. It will be true for most
rural areas and some urban areas. The second assumption is
reasonable because the tropospheric and ionospheric delays,
as the major components of the common biases, typically
change very slowly over time [2]. The third assumption
is also reasonable, as the difference between the vertical
positions recorded by the digital map and the ground truth can
be considered equivalent measurement noise and accounted
for by increasing the noise variance parameter.
The vehicle cooperative localization has been formulated
in this work as a Bayesian filtering problem which estimates
the joint posterior distribution of the GNSS common biases
and the states of the vehicles conditioned on the raw pseudorange measurements. Assumption 1 implies that, conditioned
on the common biases, the posterior distributions of the
vehicle states are independent from each other, which means
that the joint distribution can be factorized as follows:
1:N s
1:N v
1:N v
1:N s
1:N s
p(C1:t
, X1:t
|Z1:t ) = p(X1:t
|C1:t
, Z1:t )p(C1:t
|Z1:t )

=

Nv
Y

and velocities need be modeled explicitly. Therefore, the
state vector of the ith vehicle is
Xti = ( xit

ẋit

yti

ẏti

ḃit )T ,

bit

(3)

where xit and yti are the horizontal positions; ẋit and ẏti are
the horizontal velocities; and bit and ḃit are the receiver clock
bias and drift, respectively.
The mean is propagated by




B 0 0
1 ∆t
i
i


X̄t = AXt−1 , with A = 0 B 0 , B =
,
0 1
0 0 B
(4)
where X̄ti is the predicted mean.
The associated covariance matrix is propagated by


Rx 0
0
Σ̄it = AΣit−1 AT + Rt , with Rt =  0 Ry 0  ,
0
0 Rb
#
" σ2 ∆t4 σ2 ∆t3 #
" 2 4
2
3
σax ∆t
4 3
2
σax
∆t
2

Rx =
"
Rb =

σax ∆t
2
2
∆t2
σax

σd2 ∆t4
+ σb2 ∆t2
4
σd2 ∆t3
2

ay

, Ry =
σd2 ∆t3
2
σd2 ∆t2

ay

4

2

2

2
∆t2
σay

2
σay
∆t3

,

#
,

(5)
where Σit−1 is the covariance matrix of the state vector;
2
2
are the
and σay
Σ̄it is the predicted covariance matrix; σax
2
variances of the horizontal accelerations; and σb and σd2 are
the variances of the clock bias and drift time derivatives [17],
which are assumed to be uncorrelated during the derivation.
B. Multipath Rejection and Measurement Update

i
1:N s
1:N s
p(X1:t
|C1:t
, Z1:t )p(C1:t
|Z1:t ),

i=1

(1)
1:N s
where C1:t
denotes the common biases of the total N s
1:N v
visible satellites, X1:t
denotes the state vectors of the
total N v vehicles, and Z1:t denotes all the pseudo range
measurements, from time 1 to time t.
The posterior distribution of the common biases
1:N s
p(C1:t
|Z1:t ) is estimated by a particle filter, and the
conditionally independent posterior distribution of the vehii
1:N s
, Z1:t ) are estimated by independent
cle states p(X1:t
|C1:t
EKFs.
A. Prediction of States

The pseudo-range measurement model between satellite j
and vehicle i is
i
i
Ztj,i = kpit − sjt k + Ctj + bit + λj,i
t mt + vt ,

(6)

where pit is the position of the vehicle and sjt is the satellite
i
position; λj,i
t is a binary indicator variable. mt is the potential
multipath bias and vti ∼N (0, σz2 ) is the receiver noise, which
is assumed to be white. The binary value of the indicator
variable is to be determined through a χ2 test to indicate the
presence of multipath bias.
In the absence of multipath biases, the predicted mean of
the pseudo-range measurement will be

Assumption 2 implies that the common biases can be
modeled by the first-order Gaussian-Markov process:

Z̄tj,i = kpit − sjt k + Ctj + bit

j
Ctj = Ct−1
+ wtj ∆t,

The difference between the actual pseudo-range
measurement and the predicted mean will obey a Gaussian
distribution, and the Mahalanobis distance of this random
variable will obey the χ2 distribution of one degree of
freedom:

(2)

where wtj ∼N (0, σc2 ), with σc2 denoting the variance of
the common bias drift, ∆t is the length of the time
interval between two successive updates of the states and
j = 1, 2, ..., N s is the index for satellites.
Assumption 3 implies that only the horizontal positions

−1
2
Dj,i
= (Ztj,i − Z̄tj,i )T Pj,i
(Ztj,i − Z̄tj,i )∼χ21

(7)

(8)

Pj,i =

T
Hj,i Σixy Hj,i

+

σz2 , Hj,i

∂Ztj,i
,
=
∂(xit , yti )

(9)

Σixy

where
is the submatrix of the covariance matrix
representing the uncertainty of the horizontal position,
and Hj,i is the Jacobian of the measurement function
with respect to the horizontal position, which projects the
uncertainty of the position space to the range space.
The indicator variable is determined by
(
2
≤F −1 (α1 )
0
Dj,i
j,i
λt =
F (D 2 )−α1 , (10)
2
1
Dj,i
≥F −1 (α2 )kuj,i ≤ α2j,i
−α1

 2
σz
Qt =  ...
0

0
...
0


0
... 
,
σz2 N ×N

(16)

where Σ̄it is calculated by Eq. (8); H̃ti is the measurement
Jacobian for batch update; I is the identity matrix.
C. Applying Map Constraint
After the conventional EKF update and weight
determination, the map constraint is used to further
modify the particle weights such that those particles with
vehicle configurations incompatible with the map constraint
are assigned a low weight and will be eliminated with high
probability during the resampling. In this paper, the particle
weights are modified by
Z
[k]
[k]
wi = wi−1 ε(xit , yti )p(xit , yti )dxit dyti , i = 1, 2...N v,

1
(xit , yti )on lane
i i
,
with ε(xt , yt ) =
0
(xit , yti )out of lane
(17)
where p(xit , yti ) is the joint Gaussian distribution drawn from
the EKF.
The integral in Eq. (24) is difficult to calculate analytically
due to the potentially complicated geometry. Therefore, it
is again calculated by Monte Carlo Integration, where the
proposal distribution is p(xit , yti ) and the importance weight
is ε(xit , yti ).
Z
Nm
1 X
i,[l] i,[l]
ε(xit , yti )p(xit , yti )dxit dyti ≈
ε(xt , yt ), (18)
Nm

where F is the Cumulative Distribution Function (CDF) of
the χ21 distribution. α1 , alpha2 (with α1 < α2 ) are the
confidence levels for the rejection and acceptance of the
multipath presence hypothesis, respectively. uj,i is a random
number generated according to the uniform distribution on
[0, 1], k is the logical ”or”.
α1 is chosen as a small number such that only those
measurements compatible with the predicted measurements
are accepted with certainty as normal, while α2 is chosen
as a large number such that only those measurements
incompatible with the predicted measurement are rejected
with certainty as spurious. In the intermediate cases, the
measurements are accepted with probability. As a result,
the particle filter keeps multiple hypotheses with respect to
the assumptions on multipath biases. Particles that make
wrong hypotheses will be eliminated by applying the map
constraints.
The weights of the particles are calculated according to the
principal of importance weight. The detailed mathematical
derivation can be found in [18]. For the pseudo-range
measurement from the jth satellite, the weights of the
particles are updated as follows
(
[k]
2
)
λj,i
wj−1 2πP1 j,i exp(− 21 Dj,i
t =0
[k]
,
wj =
[k]
j,i
1 −1
1
wj−1 2πPj,i exp(− 2 F (α3 ))
λt = 1
(11)
where α3 is a parameter that can be tuned depending on
the environment and frequency of multipath occurrence, and
superscript [k] is the index of particles.
The vehicle states are then updated using all the pseudorange measurements regarded as free of multipath biases, that
is, with λjtn ,i = 0, n = 1, 2...N by

= 1, 2...Nm is the set of samples drawn
where
from the distribution p(xit , yti ), and Nm is the size of the
sample set. The pseudo code of the proposed RBPF is
shown as follows and depicted in Fig.1:
[k]
[k]
[k]
[k]
[k]
[k]
(Ct , Xt , wt ) = RBP F (Ct−1 , Ct−1 , Ct−1 )
1) Predict Ct and Xt according to Eq. (3,5)
for vehicle i = 1 : N v
2) Determine the indicator variable according to Eq. (17)
3) Calculate particle weights and update Xt according to
Eq. (19)
4) Modify particle weights according to Eq. (24)
5) Resample
end RBPF

Xti = X̄ti + Kti (Zti − Z̄ti ),

IV. S IMULATION R ESULTS AND D ISCUSSIONS

X̄ti

(12)

Z̄ti

where
and
are calculated by Eq. (5) and Eq. (14),
respectively. Zti is the actual measurement. The Kalman filter
matrix Kti is calculated as:
Kti = Σ̄it (H̃ti )T (H̃ti Σ̄it H̃ti )T + Qt )

(13)

Σit = (I − Kti H̃ti )Σ̄it

(14)

 ∂Z j1 ,i
∂xit


H̃ti =  ...

∂Z jN ,i
∂xit

0
...
0

∂Z j1 ,i
∂yti

...
∂Z jN ,i
∂yti

0
...
0

0
...
0


0

...
0

N ×6

(15)

l=1

i,[l] i,[l]
(xt , yt ), l

In this section, a simulation scenario and results are
presented and discussed.
A. Simulation Scenario and Noise Models
The configuration of the simulated scenario is shown in
Fig. 2, where four vehicles are traveling in each lane of
two orthogonal roads, respectively. The performance of the
proposed algorithm is illustrated through comparison with
the CMM algorithm proposed in Rohani et al. [1]. In their
approach, the common correction of the vehicle positions is

TABLE I
S IMULATION PARAMETERS (x AND y ARE LOCAL COORDINATES WHERE
x IS ALONG THE LANE AND y IS TRANSVERSAL TO THE LANE )

Value
Unit

∆t
0.1
s

Value
Unit

Schematics of the RBPF algorithm

searched in the position space using a particle-based approach
by applying map constraints. Due to the uncertainty caused
by the non-common errors, the map constraints are blurred
to avoid over-convergence.
Three CMM algorithms are compared. The first algorithm

Fig. 2.

Configuration of the vehicles for CMM, lane width 3.5m

is the aforementioned one proposed by Rohani et al. (referred
to as the static method); the second algorithm is a modified
version of the first one, where the prior estimation of each
vehicle is smoothed by a Kalman filter in the position
space before implementing CMM. The third algorithm is the
proposed RBPF.
The simulation parameters appear in Table 1. The simulation uses 200 particles. In the RBPF, the initial common
biases are the ground truth corrupted by white noise, with
variance σn2 = 0.25 m2 .
The performances of these three algorithms under two
measurement noise models are simulated. The first noise
model simulates common biases and uncorrelated white
noise with variance σz2 ; the second noise model simulates
common biases, uncorrelated white noise with variance σz2
and multipath biases. Due to the rapidly varying nature of the
multipath biases, they are emulated by inserting a 4 m bias
to each of the pseudo range measurements with probability
p = 0.25 randomly at each time point. The common biases

σc2
0.01
m2 /s2

σd2
1
m2 /s4

σz2
1
m2

2
σax
1
m2 /s4

α1
0.95
/

2
σay
0.01
m2 /s4

α2
1
/

α3
0.99
/

σb2
1
m2 /s2
Nm
100
/

and satellite geometry are emulated using the GPSoft Satellite
Navigation Toolbox [19]. The satellite orbits are assumed
to be perfectly circular, which means the ephemris error, as
part of the common biases, is not emulated. This assumption
is generally acceptable, as it forms only a small portion of
the common biases. The emulated common biases include
slowly varying ionospheric and tropospheric effects based on
physical models of the atmosphere, which should be valid
under mild atmospheric conditions.
B. Localization Results
The horizontal position errors and associated covariance
of one of the vehicles using the three described algorithms
are shown in Figs. 3-6.
Fig. 3 and Fig. 5 show that the localization error using
the static method is much larger and noisier than either that
using the smoothed static method or the proposed RBPF.
This is to be expected, as the white noise and multipath
biases in the pseudo range measurements cause significant
non-common ego localization error if they are not filtered.
For the first two algorithms to perform well, the non-common
error must be much less than the common error. Thus,
the modified algorithm significantly outperforms the static
method because the non-common error caused by the white
noise is effectively filtered out by the additional Kalman
filter. Nevertheless, the RBPF outperforms both of them.
5
Static method

Localization error (m)

Fig. 1.

Ns
6
/

Smoothed static method

4

RBPF

3

2

1

0
0

50

100

150

Number of iterations
Fig. 3. Horizontal position errors, noise model: common biases+white noise

Another benefit of the proposed RBPF is that it signif-

Determinant of covariance (m2 )

Static method
Smoothed static method
RBPF

10 2

10 0

10 -2

0

50

100

150

Number of iterations
Fig. 4.
Determinant of horizontal position covariance, noise model:
common biases+white noise

14
Static method
Smoothed static method
RBPF

Localization error (m)

12
10
8
6
4

icantly outperforms the other two algorithms with respect
to estimation effectiveness (see Figs. 4 and 6), because it
does not have to search the position space exhaustively at
each time step for all compatible corrections of the common
error. This calculation is both unnecessary and ineffective
because most of the corrections, though compatible with the
current map constraints, would be eliminated if the previous
localization results were also considered. In other words, the
temporal correlation of the common biases is not exploited. In
contrast, the proposed RBPF keeps track of the most probable
common biases. The common biases of small probability are
eliminated through resampling, and the time correlation of
the common biases is taken advantage of by Eq. (3), that is,
enforcing the estimation of the common biases to be timecontinuous. Thus, the estimation covariance turns out to be
much smaller than that of the first two algorithms.
It should be noted that the estimation results from RBPF
are not only effective but also consistent because the correlation between vehicle positions and the common biases
are implicitly modeled by the conditionally independent
filter structure. Fig.6 shows that the covariance using the
static method occasionally drops below that using the RBPF,
because the multipath biases induce large non-common errors, making just about all corrections of the common error
incompatible. As a result, only a small number of corrections
remain after resampling and the estimation result is overconfident.
Table.2 presents the Root Mean Square (RMS) of the
localization errors shown in Figs. 3 and 5.

2

TABLE II
ROOT M EAN S QUARE OF THE LOCALIZATION ERRORS ( M )

0
0

50

100

150

Number of iterations
Fig. 5. Horizontal position errors, noise model: common biases+white
noise+multipath biases

Multipath free
Multipath included

Static
2.37
4.09

Smoothed static
0.80
1.11

RBPF
0.40
0.68

Determinant of covariance (m2 )

C. Estimation Results of Common Biases
The left and right figures in Fig. 7show the common bias
estimation errors corresponding to one of the satellites using
the smoothed static method and the proposed RBPF and the
corresponding error bars, respectively.
The two algorithms show consistent estimation results,
while the estimation from the RBPF is more accurate and
effective than that using the smoothed static method. In addition, since the RBPF estimates the common biases directly
while the smoothed static method uses the noisy pseudo range
measurements, the estimation using the latter is much noisier
than that using the former.

Static method
Smoothed static method
RBPF

10 2

10 0

10 -2

V. C ONCLUSIONS
0

50

100

150

Number of iterations
Fig. 6.
Determinant of horizontal position covariance: common biases+white noise+multipath biases

In this paper, a Rao-Blackwellized particle filter has been
proposed for the simultaneous estimation of GNSS common biases and vehicles cooperative localization using map
matching. The following conclusions can be drawn based on
the simulation results:

4

Common bias estimation error (m)

Common bias estimation error (m)

4
Error (Smoothed static method)
3 sigma error bars

3
2
1
0
-1
-2
-3

Error (RBPF)
3 sigma error bars

3
2
1
0
-1
-2
-3

0

50

100

150

Number of iterations

0

50

100

150

Number of iterations

Fig. 7. Common bias estimation error using the smoothed static method and the associated error bars, noise model:common biases+white noise; left:
Smoothed static method, right: RBPF

1) The proposed method fully exploits the temporal correlation of the common biases and vehicle positions
through the prediction-update process such that the
estimation covariance is reduced by at least two orders
compared with previous algorithms while preserving
consistency.
2) The proposed method effectively filters out white noise
using the conditionally independent EKFs and detects
and rejects measurements corrupted by multipath biases, thus significantly mitigating the effect of the noncommon errors and improving localization accuracy by
about 50% compared with previous algorithms.
3) The proposed method estimates common biases and
vehicle positions jointly. The variances of the estimated
common biases are reduced by about 75% compared
with previous algorithms.
For future work, the geometric dependence of the multipath biases will be investigated and exploited for better
localization results.
ACKNOWLEDGMENT
This work is funded by the Mobility Transformation Center
at the University of Michigan.
R EFERENCES
[1] M. Rohani, D. Gingras and D. Gruyer.,”A Novel Approach for Improved
Vehicular Positioning Using Cooperative Map Matching and Dynamic
Base Station DGPS Concept,” in IEEE Transactions on Intelligent
Transportation Systems , vol. 17, no. 1, pp. 230-239, Jan. 2016.
[2] E. Kaplan, Understanding GPS. Boston: Artech House, 1996.
[3] G. Satheesh, Global positioning System: Principles and applications.
Tata McGraw-Hill Education, 2005.
[4] K. Shunsuke, G. Yanlei and L. T. Hsu, ”GNSS/INS/On-board Camera
Integration for Vehicle Self-Localization in Urban Canyon,” 2015 IEEE
18th International Conference on Intelligent Transportation Systems,
Las Palmas, 2015, pp. 2533-2538.
[5] T. Heidenreich, J. Spehr and C. Stiller, ”LaneSLAM – Simultaneous
Pose and Lane Estimation Using Maps with Lane-Level Accuracy,”
2015 IEEE 18th International Conference on Intelligent Transportation
Systems, Las Palmas, 2015, pp. 2512-2517.

[6] S. Dominguez, B. Khomutenko, G. Garcia and P. Martinet, ”An Optimization Technique for Positioning Multiple Maps for Self-Driving
Car’s Autonomous Navigation,” 2015 IEEE 18th International Conference on Intelligent Transportation Systems, Las Palmas, 2015, pp.
2694-2699.
[7] R. Toledo-Moreo, M. A. Zamora-Izquierdo, B. Ubeda-Minarro and A.
F. Gomez-Skarmeta, ”High-Integrity IMM-EKF-Based Road Vehicle
Navigation With Low-Cost GPS/SBAS/INS,” in IEEE Transactions on
Intelligent Transportation Systems, vol. 8, no. 3, pp. 491-511, Sept.
2007.
[8] P. Zhang, J. Gu, E. E. Milios and P. Huynh, ”Navigation with
IMU/GPS/digital compass with unscented Kalman filter,” IEEE International Conference Mechatronics and Automation, 2005, Niagara Falls,
Ont., Canada, 2005, pp. 1497-1502 Vol. 3.
[9] F. Caron, M. Davy, E. Duflos and P. Vanheeghe, ”Particle Filtering
for Multisensor Data Fusion With Switching Observation Models:
Application to Land Vehicle Positioning,” in IEEE Transactions on
Signal Processing, vol. 55, no. 6, pp. 2703-2719, June 2007.
[10] C. White, D. Bernstein and A. Kornhauser, ”Some map matching
algorithms for personal navigation assistants”, Transportation Research
Part C: Emerging Technologies, vol. 8, no. 1-6, pp. 91-108, 2000.
[11] D. Kim, B. Kim, T. Chung, K. Yi, ”Lane-Level Localization using
an AVM camera for An Automated Driving Vehicle in Urban Environments,” in IEEE/ASME Transactions on Mechatronics, vol.PP, no.99,
pp.1-1. [In Press]
[12] X. Huang, D. Zhao, and H. Peng, ”Empirical Study of DSRC Performance Based on Safety Pilot Model Deployment Data.” under review,
arXiv preprint arXiv:1606.08365, 2016.
[13] D. Bezzina and J. R. Sayer, Safety Pilot: Model Deployment Test
Conductor Team Report, 2014.
[14] S. Robin, et al. ”Cooperative localization and map matching for urban
road applications.” 18th ITS World Congress, 2011.
[15] T. Sebastian. ”Particle filters in robotics.” Proceedings of the Eighteenth conference on Uncertainty in artificial intelligence, Morgan
Kaufmann Publishers Inc., 2002.
[16] D. Arnaud, et al. ”Rao-Blackwellised particle filtering for dynamic
Bayesian networks.” Proceedings of the Sixteenth conference on Uncertainty in artificial intelligence, Morgan Kaufmann Publishers Inc.,
2000.
[17] A. Giremus, J. Y. Tourneret and V. Calmettes, ”A Particle Filtering
Approach for Joint Detection/Estimation of Multipath Effects on GPS
Measurements,” in IEEE Transactions on Signal Processing, vol. 55,
no. 4, pp. 1275-1285, April 2007.
[18] M., Michael, et al. ”FastSLAM: A factored solution to the simultaneous
localization and mapping problem.” in Aaai/iaai. 2002.
[19] ”Satellite Navigation (SatNav) Toolbox 3.0”, GPSoft — Navigation Simulation and Analysis Software, 2016. [Online]. Available:
http://gpsoftnav.com/products/satellite-navigation-satnav-toolbox-3-0/.

