Improving Localization Accuracy in Connected Vehicle Networks Using Rao-Blackwellized Particle Filters: Theory, Simulations, and Experiments

Transcription

1 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 Improving Localizaion Accuracy in Conneced Vehicle Neworks Using Rao-Blackwellized Paricle Filers: Theory, Simulaions, and Experimens Macheng Shen, Ding Zhao, Jing Sun and Huei Peng arxiv:7.579v [cs.sy] 6 Mar 7 Absrac A crucial funcion for auomaed vehicle echnologies is accurae localizaion. Lane-level accuracy is no readily available from low-cos Global Navigaion Saellie Sysem (GNSS) receivers because of facors such as mulipah error and amospheric bias. Approaches such as Differenial GNSS can improve localizaion accuracy, bu usually require invesmen in expensive base saions. Conneced vehicle echnologies provide an alernaive approach o improving he localizaion accuracy. I will be shown in his paper ha localizaion accuracy can be enhanced using crude GNSS measuremens from a group of conneced vehicles, by maching heir locaions o a digial map. A Rao-Blackwellized paricle filer (RBPF) is used o joinly esimae he common biases of he pseudo-ranges and he vehicle posiions. Mulipah biases, which inroduce receiverspecific (non-common) error, are miigaed by a muli-hypohesis deecion-rejecion approach. The emporal correlaion of he esimaions is exploied hrough he predicion-updae process. The proposed approach is compared o exising mehods using boh simulaions and experimenal resuls. I was found ha he proposed algorihm can eliminae he common biases and reduce he localizaion error o below meer under open sky condiions. Index Terms Localizaion, GNSS, conneced vehicles, paricle filer, Rao-Blackwellized I. INTRODUCTION An essenial funcion of inelligen ransporaion sysems is accurae localizaion. A Global Navigaion Saellie Sysem (GNSS) receiver calculaes is posiion from pseudo-range measuremens of muliple saellies. Pseudo-ranges conain error which can be decomposed ino common error (due o saellie clock error, ionospheric and ropospheric delays) and non-common error (due o receiver noise, receiver clock error and mulipah error). The nominal accuracy of pseudo-ranges for a single-band receiver is abou o meers, which resuls in a posiion error of several meers []. Wihou furher improvemen, his crude GNSS error is oo large for many safey funcions as he lane of vehicles canno be robusly *This work is funded by he Mobiliy Transformaion Cener a he Universiy of Michigan wih gran No. N58. The firs wo auhors, M. Shen and D. Zhao have equally conribued o his research. M. Shen and J. Sun are wih he Deparmen of Naval Archiecure and Marine Engineering, Universiy of Michigan, Ann Arbor, MI, 89. ( macshen@umich.edu; jingsun@umich.edu) D. Zhao (corresponding auhor, zhaoding@umich.edu) is wih he Universiy of Michigan Transporaion Research Insiue, Ann Arbor, MI, 89. H. Peng is wih he Deparmen of Mechanical Engineering, Universiy of Michigan, Ann Arbor, MI, 89. ( hpeng@umich.edu) Manuscrip received XXX XX, 6; revised XXX XX, X. idenified. Fig. shows he biased posiioning caused by pseudo-range error. Fig.. Illusraion of correlaed GNSS localizaion error due o correlaed pseudo-range error Differenial GNSS (DGNSS) is an enhancemen o GNSS ha can achieve sub-meer level accuracy by correcing he common biases hrough a nework of fixed reference saions. Moreover, cenimeer-level accuracy is achievable by he Real Time Kinemaic (RTK) echnique, which uses carrier phase measuremens o provide real-ime correcions []. These echniques, however, rely on an expensive infrasrucure. In his paper, we explore an alernaive low-cos soluion for lane-level accurae localizaion by using only crude GNSS measuremens from a se of conneced vehicles. The following secions are arranged as follows. In Secion, relaed works are reviewed. The challenges of localizaion using only single frequency marke mass receivers are discussed. In Secion 3, he derivaion of he localizaion enhancemen algorihm is presened in deail. Simulaion resuls are presened in Secion. The simulaion scenario and error models are inroduced, followed by performance analysis of he proposed mehod compared wih exising mehods. In Secion 5, experimenal resuls are presened using u-blox EVK-6T, an auomoive grade receivers wih auomoive pach anenna. The implemenaion deails are firs described. Then he raw daa is processed o verify he error models. Finally, he performance of he algorihms using he raw daa is shown. Conclusions are presened in Secion 6.

2 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 II. RELATED WORKS The GNSS localizaion accuracy can also be improved using eiher sensor fusion or map maching, wih some promising resuls shown in recen years [3] [5]. Inerial Navigaion Sysem (INS) in combinaion wih vehicle dynamics can be used o improve vehicle locaion esimaion. Daa fusion algorihms such as Exended Kalman Filer (EKF) [6], Unscened Kalman Filer (UKF) [7] and paricle filer [8] can be used o fuse GNSS and INS measuremens, leading o high accuracy navigaion soluions. Wih he rapid developmen of digial maps, navigaion algorihms based on map maching have also been exensively sudied [6], [7], [9], []. Map maching algorihms mach he noisy GNSS posiioning resuls o a rajecory ha saisfies known road geomery consrains. Addiional sensors such as cameras or lidars in combinaion wih high-definiion maps can reduce localizaion error down o he cenimeer-level []. This approach requires boh accurae sensors and accurae maps. Wih he deploymen of Dedicaed Shor Range Communicaions (DSRC) echnique in he real world [], conneced vehicles provide an alernaive for improving localizaion error by correcing common localizaion error of muliple GNSS receivers insalled on muliple vehicles []. Alam e al. [3] developed a cooperaive posiioning mehod ha improves he relaive posiioning beween wo vehicles by fusing he shared pseudo-range observaions. The relaive posiioning accuracy of his mehod ouperforms ha of DGNSS alone. Wang [] proposed an augmen DGPS ha ighly inegraed DGPS, range and bearing observaions in a small nework of vehicles or infrasrucure poins, which ouperforms he DGPS in erms of absolue posiiong. An alernaive o improving GNSS absolue posiioning wihou incurring infrasrucure coss is cooperaive map maching (CMM). Assuming ha mos vehicles ravel wihin lanes, he correcion o he common localizaion biases can be deermined so ha he correced posiions of a group of vehicles bes fi he map. Recenly, he capabiliy of CMM o miigae he biases from he localizaion resuls obained from low-cos GNSS receivers alone has been demonsraed in Rohani e al. [5]. The hree main difficulies arise for CMM using low-cos GNSS receivers alone are: Effec of non-common error: Because of non-common error, a correcion for he common localizaion error ha makes all vehicle posiions compaible wih he road consrains may no exis. If he road consrains are enforced aggressively wihou considering he non-common error, he localizaion soluion may overly converge, i.e., he variance of he esimaion error may be underesimaed. Correlaion beween common biases and vehicle posiion esimaion: he esimaed common bias may induce error in he esimaed vehicle posiions and vice versa. If hey are esimaed sequenially, he daa fusion scheme should be designed o avoid over-convergence due o fusion of correlaed daa. Incorporaion of road consrains: Lane posiion consrains of real roads canno be easily described analyically. Incorporaion of hese consrains in map maching requires a flexible filer scheme. Wang e al. [6] presened a decenralizaed approach ha uses local EKFs and an opimal global fusion scheme o deal wih he correlaion. Noneheless, heir global fusion approach requires a full order covariance marix inversion and no road consrains are considered in heir work. Rohani e al. [5] presened a paricle-based CMM algorihm o address he hree difficulies menioned above. The firs difficuly was addressed by a weighed road map approach o preserve consisency. The second difficuly is handled by racking he origin of he common bias correcions from differen vehicles and fusing only hose correcions from independen sources o avoid daa inces, which avoids over-convergence. In heir approach, some of he correlaed correcions conaining addiional informaion have o be discarded. The hird difficuly is handled by a paricle-based approach ha uses only he vehicle posiion esimaion of he curren epoch. Algorihms ha beer uilizes all available daa are expeced o yield improved localizaion performance. In our previous work which was presened a he conference [7], he problem of inferring he rue vehicle posiions, as well as he GNSS common biases from he pseudo-range measuremens from a group of vehicles, is addressed by a Bayesian filering approach. The aforemenioned difficulies are solved by joinly esimaing he common biases and vehicle posiions using a Rao-Blackwellized paricle filer (RBPF). In he RBPF, he correlaion beween common biases and vehicle posiions is modeled implicily hrough he diversiy of he paricles. As a resul, here is no need for explici daa fusion. The effec of he mulipah biases is miigaed hrough a deecion-rejecion mehod based on a saisical es. The paricle filer srucure allows muliple hypoheses wih respec o he deecion of mulipah biases, hus making he deecion more robus. In addiion, he paricle filer is flexible enough o handle road consrains in a sraighforward manner by manipulaing he paricle weighs according o he road consrains. I also fully explois he emporal correlaion hrough a predicion-updae process, eliminaing impossible configuraions in he join space of common biases and vehicle sae variables, drasically reducing he esimaion variance. The compuaional complexiy varies linearly wih he number of vehicles, which makes he proposed RBPF boh effecive and efficien. This paper expands he iniial resuls from he conference paper. More specifically, he performance of he algorihm in mulipah environmens wih signal blockage is shown hrough simulaions based on 3-D Ray Tracing mehod for mulipah error. The performance of he RBPF under open sky condiions is validaed hrough experimens, which validaes he pseudo-range error model. The robusness of he algorihm wih respec o signal blockage is also sudied. One poenial drawbacks of his CMM mehod is ha he localizaion accuracy and robusness highly depend on he configuraion of he available road consrains. The impac of road consrains on CMM is discussed in our more recen work [8].

3 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 3 III. THEORY AND METHOD In his secion, he srucure of he CMM problem is illusraed by a Dynamic Bayesian Nework (DBN), which encodes he condiional independence ha moivaes he RBPF. The heoreical aspec of he RBPF is hen inroduced, and he predicion-updae srucure of he RBPF is shown in deail. Fig. shows he DBN corresponding o he CMM problem ha involves only wo vehicles wih index i and i. C represens he se of pseudo-range common biases, X is he vehicle sae vecor including vehicle posiions, and Z represens he se of error corruped pseudo-ranges, which is observed by he receivers. The subscrip represens he ime. The direced edges represen causal relaionships beween he node variables. For example, he pseudo-ranges Z i are deermined by he vehicle sae X i and he pseudo-range common biases C. As a resul, X i is correlaed wih C hrough he observaion Z i and he he saes of all he vehicles wihin he nework are correlaed wih each oher hrough heir correlaion wih he pseudorange common biases. This correlaion is encoded by he pahs beween one vehicle sae o anoher hrough he common biases nodes. If he common biases are condiioned on, hen he pahs would be blocked, indicaing ha he saes of all he vehicles are independen wih each oher if he common biases are given. The following assumpions are made in his work: ) The non-common error of differen vehicles are uncorrelaed. ) The common biases vary slowly over ime. 3) The verical posiions of he vehicles can be obained wih reasonable accuracy from he digial map. recorded by he digial map and he ground ruh can be considered equivalen measuremen noise and accouned for by increasing he noise variance parameer. Assumpion, ogeher wih he DBN represenaion Fig., resuls in he following facorizaion of he join poserior disribuion of he pseudo-range common biases and he vehicle saes condiioned on he pseudo-range observaions: p(c :Ns :, X :Nv : Z : ) = p(x :Nv : C :Ns :, Z : )p(c :Ns : Z : ) N v = p(x: C i :Ns :, Z : )p(c :Ns : Z : ), i= () where N s and N v are he number of saellies and vehicles respecively, and he superscrip : N s and : N v are shorhands for he corresponding variables of all he saellies and all he vehicles. The subscrip : is he shorhand for he corresponding variables of all he ime insances. The RBPF explois his condiional independence propery for efficien inference of he pseudo-range common biases and he vehicle saes given he pseudo-range observaions. The poserior disribuion of he common biases p(c :Ns : Z : ) is esimaed by paricle filer, and he disribuions of he vehicle saes condiioned on he common biases p(x i : C :Ns :, Z : ) are independen wih each oher and esimaed by a se of EKFs whose dimension is he dimension of he sae vecor [9]. The recursive predicion-updae equaions are presened as follow. A. Predicion of Saes Wih assumpion, we model he ime variaion of he common biases as a firs-order Gaussian-Markov process: C j = C j + wj, () where w j N(, σc ), wih σc denoing he variance of he common bias drif, is he lengh of he ime inerval beween wo successive updaes of he saes and j =,,..., N s is he index for saellies. Assumpion 3 implies ha only he horizonal posiions and velociies need be modeled explicily. Therefore, he sae vecor of he ih vehicle a ime is X i = ( x i ẋ i y i ẏ i b i ḃ i )T, (3) Fig.. DBN represenaion of he CMM problem in Fig. Assupions () - (3) are unresricive in he following senses. The firs assumpion is valid as long as he paricipaing vehicles are no concenraed in he same area; oherwise, he mulipah error may be correlaed. I will be rue for mos rural areas and some urban areas. The second assumpion is reasonable because he ropospheric and ionospheric delays, as he major componens of he common biases, ypically change very slowly over ime []. The hird assumpion is also reasonable, as he difference beween he verical posiions where x i and y i are he horizonal posiions; ẋ i and ẏ i are he horizonal velociies; and b i and ḃi are he receiver clock bias and drif, respecively. The mean is propagaed by X i = AX, i wih A = B B, B = B where X i is he prediced mean. The associaed covariance marix is propagaed by [ ], ()

4 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 R x Σ i = AΣ i A T + R, wih R = R y, ] R b ] R x = R b = [ σ ax σ ax 3 [ σ d σ ax 3, R y = σax ] + σb σ d 3, σ d 3 σd [ σ ay σ ay 3 σ ay 3 σ ay where Σ i is he covariance marix of he sae vecor; Σ i is he prediced covariance marix; σax and σay are he variances of he horizonal acceleraions; and σb and σ d are he variances of he clock bias and drif ime derivaives [], which are assumed o be uncorrelaed in he derivaion. B. Mulipah Rejecion and Measuremen Updae The pseudo-range measuremen model beween saellie j and vehicle i is Z j,i, (5) = p i s j + C j + b i + λ j,i m i + v i, (6) where p i is he posiion of he vehicle and s j is he saellie posiion. m i is he poenial mulipah bias and v N(, i σz) is he receiver noise, which is assumed o be whie. λ j,i is a binary indicaor variable ha is o be deermined hrough a χ es o indicae he presence of mulipah bias. In he absence of mulipah biases, he prediced mean of he pseudo-range measuremen will be Z j,i = p i s j + C j + b i (7) The difference beween he acual pseudo-range measuremen and he prediced mean will obey a Gaussian disribuion, and he Mahalanobis disance of his random variable will obey he χ disribuion wih one degree of freedom: D j,i = (Z j,i j,i Z ) T P j,i (Zj,i j,i Z ) χ (8) P j,i = H j,i Σ i xyh T j,i + σ z, H j,i = Zj,i (x i, y i ), (9) where Σ i xy is he submarix of he covariance marix represening he uncerainy of he horizonal posiion, and H j,i is he Jacobian of he measuremen funcion wih respec o he horizonal posiion, which projecs he uncerainy of he posiion space o he range space. The indicaor variable is deermined by λ j,i = { D j,i F (α ) Dj,i F (α ) u j,i F, () (D j,i ) α α α where F is he Cumulaive Disribuion Funcion (CDF) of he χ disribuion. α, α (wih α < α ) are he confidence levels for he rejecion and accepance of he mulipah presence hypohesis, respecively. u j,i is a random number generaed according o he uniform disribuion on [, ], is he logical or. The choices of α and α deermine he aggressiveness o rejec ouliers. The paricle filer keeps muliple hypoheses wih respec o he assumpions on mulipah biases. Paricles ha make wrong hypoheses will be eliminaed by applying he map consrains. The weighs of he paricles are calculaed according o he imporance sampling principal. The deailed mahemaical derivaion can be found in []. For he pseudo-range measuremen from he jh saellie, he weighs of he paricles are updaed as follows w [k] j = { w [k] j w [k] j πp j,i exp( D j,i ) λj,i = λ j,i =, πp j,i exp( F (α 3 )) () where α 3 is a parameer ha can be uned depending on he environmen and frequency of mulipah occurrence, and superscrip [k] is he index of paricles. The vehicle saes are hen updaed using all he pseudorange measuremens regarded as free of mulipah biases, ha is, wih λ jn,i =, n =,...N by X i = X i + K i (Z i Z i ), () where X i and Z i are calculaed by Eq. () and Eq. (7), respecively. Z i is he acual measuremen. The Kalman gain marix K i is calculaed as: H i = K i = Σ i ( H i ) T ( H i Σ i H i ) T + Q ) (3) Σ i = (I K i H ) Σ i i () Z j,i y i (5) Z j,i x i Z j N,i x i Z j N,i y i σz Q = σz N N N 6, (6) where Σ i is calculaed by Eq. (5); Hi is he measuremen Jacobian for bach updae; I is he ideniy marix. C. Applying Map Consrain Afer he EKF updae for he vehicle saes esimaion and updaing he weigh of he paricles for he common biases esimaion, he posiioning of each vehicle represened by a se of EKFs wih differen weighs will sill be biased as no correcion has been applied o compensae he pseudo-range common biases. In order o correc he biases, he map consrain is used o furher modify he paricle weighs such ha hose paricles wih vehicle configuraions incompaible wih he map consrain are assigned a low weigh and will be eliminaed wih high probabiliy during he resampling. In his paper, he paricle weighs are modified by w [k] i = w [k] i ε(x i, y)p(x i i, y)dx i i dy, i i =,...N v, { (x i, y)on i (7) lane (x i, y)ou i of lane, wih ε(x i, y i ) =

5 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 5 where p(x i, y i ) is he join Gaussian disribuion drawn from he EKF. The inegral in Eq. (7) is difficul o calculae analyically due o he poenially complicaed geomery. Therefore, i is again calculaed by Mone Carlo Inegraion, where he proposal disribuion is p(x i, y i ) and he imporance weigh is ε(x i, y i ). ε(x i, y i )p(x i, y i )dx i dy i N m N m l= ε(x i,[l], y i,[l] ), (8) where (x i,[l], y i,[l] ), l =,...N m is he se of samples drawn from he disribuion p(x i, y), i and N m is he size of he sample se. The pseudo code of he proposed RBPF is shown as follows: (C [k], X [k], w [k] ) = RBP F (C [k], C[k], C[k] ) ) Predic C and X according o Eqs. (, ) for vehicle i = : N v ) Deermine he indicaor variable according o Eq. () 3) Calculae paricle weighs and updae X according o Eqs. (, ) ) Modify paricle weighs according o Eq. (7) 5) Resample end RBPF A. Simulaion Scenario and Error Models The configuraion of he simulaed scenario is shown in Fig. 3, where four vehicles are raveling in each lane of wo orhogonal roads, respecively. The widh of each lane is 3.5 m and all he four vehicles are raveling on he cener of he lanes. The boundary of he roads are represened by he whie solid lines, ouside which he posiioning is considered as violaing he road consrains. The performance of he proposed algorihm is illusraed hrough comparison wih he CMM algorihm proposed in Rohani e al. [5]. In heir approach, he common correcion of he vehicle posiions is searched in he posiion space using a paricle-based approach by applying map consrains. Due o he uncerainy caused by he non-common error, he map consrains are blurred o avoid over-convergence. This approach is no a Bayesian filering approach as he esimaion is no updaed according o he Bayesian rule and he esimaion a each ime insance does no require any hisorical informaion. D. Compuaional Complexiy The compuaional complexiy of he RBPF is linear in he number of paricles. For each paricle a each ime insance, a se of N v EKFs wih dimensions equal o 6 have o be updaed. As a resul, he compuaional complexiy is O(N p N v ), which is also linear in he number of vehicles. The linear complexiy wih respec o he number of conneced vehicles is an aracive propery as he poenial number of vehicles can be hundreds. This prohibis he use of many filering schemes such as Kalman filers, which has quadraic complexiy, and paricle filer wihou Rao-Blackwellizaion, which has exponenial complexiy. The number of paricles N p has an effec on he esimaion accuracy and robusness of he RBPF. The minimal N p o ensure robusness depends on he number of visible saellies and increases exponenially wih he number of saellies. Noneheless, his exponenial growh does no pose a compuaional difficuly in pracice because he number of visible saellies is always bounded. We show ha he required number of paricles for an accurae localizaion can be quie small (O()) in he nex secion. IV. SIMULATION RESULTS AND DISCUSSIONS In his secion, simulaion resuls are presened and discussed. The simulaion scenario is described firs. The improvemens in localizaion accuracy and he common biases esimaion are illusraed by comparing he RBPF wih algorihms proposed in Rohani e al. [5]. Fig. 3. Inersecion used for CMM Three CMM algorihms are compared. The firs algorihm is he aforemenioned one proposed by Rohani e al. (referred o as he saic mehod); he second algorihm is a smoohed version of he firs one, where he GNSS posiioning is smoohed by a Kalman filer before implemening CMM. The hird algorihm is he proposed RBPF. The simulaion parameers appear in Table where x and y are local coordinaes alone and ransversal o he lane on which he vehicle ravels. The simulaion uses paricles. In he RBPF, he iniial common biases are he rue common biases corruped by whie noise, wih variance σ n =.5 m. In he simulaion, he clock error is no included. TABLE I SIMULATION PARAMETERS Parameer Value Uni Parameer Value Uni N m / σ ax m/s N s 6 / σ ay. m/s α.95 / σ b m/s α / σ c. m/s α 3.99 / σ d m/s. s σ z m The performances of hese hree algorihms under wo measuremen noise models are simulaed. The firs noise model

6 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 6 simulaes common biases and uncorrelaed whie noise wih variance σz; he second noise model simulaes common biases, uncorrelaed whie noise wih variance σz and mulipah biases. The common biases and he saellie consellaion are emulaed using he GPSof Saellie Navigaion Toolbox []. This oolbox emulaes he consellaion using Keplerian orbial parameers and he saellie orbi error is negleced. The common biases are generaed according o is empirical ionospheric and ropospheric error models. The mulipah signals are simulaed by Ray-racing mehod given a 3-D digial map of he environmen. The delay-lock loop (DLL) wih half chip lengh correlaion funcion is simulaed o mimic he code racking mechanism of he GNSS receiver. This DLL causes he code mulipah error o oscillae as a funcion of he receiver locaion. More deails abou he code racking mechanism can be found in [3]. The refleced signal srengh and phase difference relaive o hose of he direc pah are required o simulae he DLL mechanism if boh of he wo signals exis. Therefore, following simplificaions are made wih respec o he mulipah simulaion: ) The ampliude of he signal is reduced by a half upon reflecion, ha is, he reflecion coefficiens of all he building surfaces are assumed o be a consan value.5. ) Signals ha have been refleced wice are no considered for mulipah conribuion due o heir negligible signal srengh. 3) If he direc pah does no exis, he received signal, no maer exiss or no, is no used o calculae he corresponding pseudo-range because of he low signal o noise raio. Based on he firs simplificaion, he GPS signal including he mulipah signal can be wrien as s() = a e I(ω+φ) [x( )+ e Iφ d x( d )], (9) where a is he ampliude of he direc pah, I = is he imaginary uni, ω is he angular frequency of he GPS carrier wave, φ is he received carrier phase of he direc pah, x() is he complex wave form of he ransmied signal, is he ime for he signal o propagae from he saellie o he receiver hrough he direc pah, φ d and d are he mulipah phase delay and propagaion ime delay relaive o he direc pah, respecively. The composie wave form inside he square bracke in Eq. (9) is correlaed wih he receiver generaed wave forms x( p + c ) and x( p c ), where p is he mulipahcorruped signal propagaion ime o be deermined by he DLL, and c is he ime for ligh o ravel a half chip lengh. The resuls of hese correlaions are wo funcions of p, which are defined as early and lae correlaor funcion, denoed as R E ( p ) and R L ( p ), respecively. Their difference D( p ) = R E ( p ) R L ( p ) is defined as discriminaor funcion. In he absence of he mulipah signal, he propagaion ime of he direc pah will be he unique zero of he discriminaor funcion wihin a neighbor of radius c, ha is, D( ) =. Hence he receiver DLL deermines he signal propagaion ime by finding he zero of he discriminaor funcion, ha is, p = arg {D() = }. For he single mulipah-corruped signal represened by Eq. (9), simple analyic formula for he pseudo-range mulipah error can be obained as ρ = c( p ) = cosφ d + cosφ d d, () where c is he nominal speed of ligh in a vacuum. The mulipah ime delay d is deermined by d = d d d m, () c where d d and d m is he pah lengh hrough he direc pah and he mulipah, respecively, which can be obained from he Ray-racing mehod. The mulipah phase delay φ d is deermined by φ d = ω d + π, () where he π is o accoun for half-wave loss upon reflecion. The aim of his mulipah model is no o provide a faihful deerminisic mulipah simulaion bu raher o capure he major feaures of his mulipah error signal ha deermine how well he localizaion algorihms migh perform. Those feaures should include he approximae order of magniude, he locaion dependence and he ime variaion characerisics of he mulipah signal, which are expeced o be refleced by his simplified model. As he wavelengh of he GPS signal carrier wave (λ =.9 m for GPS L signal) is ypically much smaller han he disances beween he receiver o he buildings, he mulipah phase delay is expeced o be sensiive o he receiver moion. As a resul, he mulipah error should oscillae rapidly as he phase delay changes. This oscillaing error may have similar effec as an addiive whie noise, which increases he apparen noise covariance of he observed pseudo-ranges. In order o accoun for his effec, he noise covariance used by he Kalman filer is esimaed from he innovaion. One of he unbiased esimaions using k seps previous innovaions is Q = k j= k+ (Z j Z j )(Z j Z j ) T H Σ H T, (3) where Z, Σ and H have been defined in Eq. () and Eq. (3), wih he vehicle idenificaion omied. Then he diagonal noise covariance marix is formed such ha Q (n, n) = max( Q (n, n), σ z), n =,,...N, () where (n, n) denoes he n-h diagonal elemen of a square marix. The 3-D map around he inersecion beween Souh h Avenue and Eas William Sree in Ann Arbor is obained from Google Earh, shown in Fig.. The surrounding buildings are modeled as recangular blocks. The geomeric quaniies of he map, including he lane widhs and building locaions and dimensions, are measured using he Ruler ool provided in Google Earh Pro.

7 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 7 Deerminan of covariance (m ) - Saic mehod Smoohed saic mehod RBPF Fig.. Inersecion beween Souh h Avenue and Eas William Sree in Ann Arbor used for mulipah simulaion (Google Earh) B. Localizaion Resuls The horizonal posiion error and he associaed covariance of one of he four vehicles using he hree described algorihms are shown in Fig Fig. 5 shows ha he localizaion error using he saic mehod is much larger and noisier han eiher ha using he smoohed saic mehod or he proposed RBPF. This is o be expeced, as he whie noise resuls in non-common error, which is no filered in he saic mehod. As he smoohed algorihm filers ou his non-common error, i ouperforms he saic mehod. Neverheless, he RBPF ouperforms boh of hem. Localizaion error (m) 5 3 Saic mehod Smoohed saic mehod RBPF 5 5 Number of ieraions Fig. 5. Horizonal posiion errors wih common biases and whie noise Anoher benefi of he proposed RBPF is ha i significanly ouperforms he oher wo algorihms in erms of esimaion covariance (see Fig. 6), because all he paricles have a smooh esimaion of he common error. In conras, he oher wo mehods search for all he compaible correcions for he common error a each insan. This search is boh unnecessary and ineffecive because mos of he correcions, hough compaible wih he curren map consrains, would be eliminaed if he previous measuremens were also considered. In oher words, he emporal correlaion of he common biases 5 5 Number of ieraions Fig. 6. Deerminan of horizonal posiion covariance wih common biases and whie noise Localizaion error (m) Number of ieraions smoohed saic mehod RBPF Fig. 7. Horizonal posiion error wih common biases and whie noise+mulipah error is no exploied. In conras, he proposed RBPF keeps rack of he mos probable common biases. The common biases of small probabiliy are eliminaed hrough resampling, and he ime correlaion of he common biases is enforced by Eq. (). Thus, he esimaion covariance urns ou o be much smaller han ha of he oher wo algorihms. Fig. 7 shows he localizaion error using he smoohed saic mehod and he RBPF in he presence of mulipah error and signal blockage. The saic mehod is no able o give localizaion resuls a all he ime insans due o he signal blockage, so i is no compared. As he number of ieraion increases, he vehicles approaches he inersecion, where he mulipah reflecion and signal blockage from he high srucure are severe. As a resul, he localizaion error increases. The performance of he smoohed saic mehod degrades severely while ha of he RBPF does no degrade as much. This resul is also expeced as he smoohed saic mehod uses he posiions insead of he raw pseudo-range measuremens o do he CMM. In he presence of signal blockage, differen vehicles migh use differen ses of saellies for heir ego-localizaion, which resuls in an unknown non-

8 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 8 Common bias esimaion error (m) Error (Smoohed saic mehod) 3 sigma error bars Common bias esimaion error (m) Error (RBPF) 3 sigma error bars Number of ieraions Number of ieraions Fig. 8. Common bias esimaion error common posiion bias in addiion o he mulipah error. This addiional non-common bias makes CMM more difficul. In conras, he RBPF is formulaed based on he raw pseudorange measuremens, which is more flexible o handle he signal blockage issue. Table shows he mean of he localizaion error of hese hree mehods, where ± is followed by he 3 σ confidence inerval. TABLE II MEAN LOCALIZATION ERROR (M) Saic Smoohed saic RBPF Mulipah free.37.79±.5.5±.8 Mulipah included C. Esimaion Resuls of Common Biases The lef and righ figures in Fig. 8 show he common bias esimaion error corresponding o one of he saellies in he mulipah-free case using he smoohed saic mehod and he proposed RBPF, respecively. Boh of he wo esimaion errors are consisen wih heir covariance, while he esimaion from he RBPF is more accurae and effecive han ha using he smoohed saic mehod. In addiion, since he RBPF esimaes he common biases hrough filering while he smoohed saic mehod uses only insananeous measuremens, he esimaion using he laer is noisier han ha using he former. D. Compuaional Complexiy Fig. 9 shows he effec of he number of paricles on he compuaion ime and localizaion error. We ran simulaions for each poin wih vehicles and 6 saellies under mulipah free condiions on MATLAB 6a wih an Inel i-7 65U processor. There are 3 ime insances in each simulaion and he ime sep is. s, which is equivalen o 3 s simulaion ime. As he number of paricles increases, he compuaion ime grows linearly and he localizaion error decreases. Even if he number of paricles is small, he localizaion accuracy and robusness do no degrade oo much. CPU ime for each simulaion (s) CPU ime Mean error Maximum error. 6 8 Number of paricles Fig. 9. Compuaional complexiy, mean and maximum localizaion error wih respec o he number of paricles used in he RBPF V. EXPERIMENT VALIDATION In his secion, experimenal resuls conduced a Norh Campus parking lo 9 in he Universiy of Michigan are presened. Firs, he experimen scenario is described. Then he raw measuremen daa colleced from he low-cos GPS receivers is processed and used o validae he presumed GPS pseudo-range error model presened in Secion 3. Afer ha, he CMM algorihms are applied on he raw daa, and he CMM localizaion resuls are compared wih each oher and wih ego localizaion resuls. A. Experimen Scenario Descripion The experimen was conduced a open sky a he parking lo shown in Fig.. We drew sraigh lines on he ground as virual road boundaries according o he skech Fig. 3. Four u-blox EVK-6T receivers were placed m (abou half a lane widh) away from he corresponding road boundaries, wih heir configuraion he same as ha of he four vehicles shown in Fig. 3. The disance of he receivers from he road boundaries could affec he compacness of he CMM posiioning. As he disance decreases, he road consrains Localizaion error of one vehicle (m)

9 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 9 Fig.. The surroundings of he experimen place and he parking lo (encircled by he red circle) where he experimen was conduced, image from Google Earh B. Validaion of pseudo-range error model In Secion 3, i is assumed ha he range error is comprised of slowly varying common biases, non-common whie Gaussian noise and clock biases a open sky. In his secion, hese assumpions are examined by processing he raw pseudo-range measuremens. Fig. shows he composie pseudo-range signal by differencing he pseudo-range corresponding o wo saellies colleced by wo receivers. This signal includes he variaion of he amospheric delay, he mulipah error and he receiver noise. The hisogram of he receiver noise signal is ploed in Fig.. I shows ha he hisogram can be reasonably fied by a Gaussian disribuion. 5 become igh and so does he posiioning covariance. In he limiing case ha he disance goes o zero, he posiioning covariance reaches is non-zero lower bound. In he limiing case ha he disance goes o infiniy, he CMM becomes almos equivalen o ego-localizaion. A Novael DL- RTK GPS, wih horizonal localizaion error less han cm, was used o measure he posiion of he u-blox receiver on he upper lef side, hereafer referred as he hos receiver. The anenna of he hos receiver was mouned on he seel op of a vehicle and hose of he oher hree receivers were placed on he ground. This placemen eliminaed he mulipah inerference from he ground reflecion. The raw measuremens, including he pseudo-ranges, he corresponding saellie idenificaions and he ime samps, were logged a a sampling rae of 5 Hz, while he four receivers were kep saic. The saellie ephemeris broadcas by he saellies were logged o calculae he saellie posiions. Number of sample Noise signal E (m) Fig.. Hisogram of he composie receiver noise signal Pseudo-range error difference (m) Time (s) Fig.. Pseudo-range error, including he slowly varying amospheric delay (several minues), he mulipah error (abou one minues) and he whie receiver noise (less han seconds) Fig. 3. Skyplo of he GPS consellaion a Universiy of Michigan Norh Campus during he experimen. Saellie idenificaion number as well as heir availabiliy are marked. C. Localizaion Resuls In order o validae he CMM algorihms in real world, a segmen of he experimen daa, colleced from he Coordinaed Universal Time (UTC) :56: o :: on Sepember h, 6, is used for CMM. During his ime period, he skyplo of he visible saellies a he experimen locaion according o he almanac is shown in Fig. 3.

10 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER ego localizaion Saic mehod RBPF ego localizaion Saic mehod RBPF Norh-Souh error (m) - Norh-Souh error (m) Wes-Eas error (m) Wes-Eas error (m) (a) Open sky (b) Wih signal blockage Fig.. Localizaion error using ego-localizaion, he saic mehod and he RBPF on experimenal daa In Fig. 3, he hree saellies denoed by he crosses, G6, G and G6, were no received due o blockage and low elevaion angle. Those seven saellies of high elevaion angle, ha is, G5, G3, G5, G8, G, G and G9, are used for localizaion. The corresponding horizonal diluion of precision (HDOP) is.79. The HDOP indicaes how much he error of he pseudo-range will resul in he horizonal posiioning error. The mulipah error appears as a bias wihin a shor ime inerval, which causes he localizaion resuls o be biased. This localizaion bias may resul in over-confidence. The adapive covariance approach used for localizaion in mulipah environmen is applied here. The simulaion parameers used are he same as hose lised in Table, excep ha he ime inerval beween wo successive ime insans is. s; he number of saellies is 7; and he covariance of he receiver noise is esimaed from he daa. The iniial values of he receiver posiion, clock bias and clock drif are obained from he ego-localizaion resuls. The iniial values of he receiver horizonal velociy are given as zeros. The iniial values of he common bias are esimaed from he difference beween he RTK soluion and he measured pseudo-ranges. The resuled localizaion error using ego-localizaion, he saic mehod and he RBPF using paricles are shown in Fig..a. I shows ha he RBPF effecively eliminaes he bias, while he saic mehod parially eliminaes he bias. Neverheless, neiher of hese wo algorihms is able o make effecive reducion on he non-common error which is driven by he low frequency par of he receiver noise, as expeced. Performance degradaion in he presence of signal blockage is also of pracical ineres. In order o sudy he effec of signal blockage, he pseudo-range measuremens from saellies G8 and G received by he hos vehicle receiver are no used. The resuled localizaion error is shown in Fig..b. In his case, he localizaion resuls given by he saic mehod rack he biased ego-localizaion resuls more closely. This degradaion should be aribue o he use of differen ses of saellies, which resuls in addiional non-common error ha is no modeled in he saic mehod. In conras, he localizaion resuls from he RBPF is almos unbiased, alhough he error is also amplified due o he absence of he wo saellies, which increases he HDOP o.7. VI. CONCLUSIONS In his paper, a Rao-Blackwellized paricle filer has been proposed for he simulaneous esimaion of GNSS common biases and vehicles cooperaive localizaion using map maching. The following conclusions can be drawn based on he simulaion and experimenal resuls: ) The proposed mehod fully explois he emporal correlaion of he common biases and vehicle posiions hrough he predicion-updae process such ha he esimaion covariance is reduced by a leas wo orders compared wih previously proposed algorihms. ) The proposed mehod almos enirely eliminaes he slowing varying common localizaion bias, hus achieving a higher accuracy han boh ego-localizaion and he previous CMM algorihm. Neverheless, none of hese hree mehods can effecively eliminae he low frequency par of he receiver noise error. The localizaion error wih experimen a open sky is wihin meers. 3) The proposed mehod is more robus wih respec o signal blockage han he previous CMM algorihm. Wih moderae signal blockage, he proposed mehod is sill able o eliminae he common bias effecively, while he localizaion will be less accurae as he HDOP increases due o he loss of saellies. REFERENCES [] E. Kaplan and C. Hegary, Undersanding GPS: principles and applicaions. Arech house, 5. [] Y. Masumoo, Global posiioning sysem, May 993, us Paen 5,,5.

11 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS, VOL. XX, NO. XX, DECEMBER 6 [3] K. Shunsuke, G. Yanlei, and L.-T. Hsu, Gnss/ins/on-board camera inegraion for vehicle self-localizaion in urban canyon, in Inelligen Transporaion Sysems (ITSC), 5 IEEE 8h Inernaional Conference on. IEEE, 5, pp [] T. Heidenreich, J. Spehr, and C. Siller, Laneslam simulaneous pose and lane esimaion using maps wih lane-level accuracy, in Inelligen Transporaion Sysems (ITSC), 5 IEEE 8h Inernaional Conference on. IEEE, 5, pp [5] S. Dominguez, B. Khomuenko, G. Garcia, and P. Marine, An opimizaion echnique for posiioning muliple maps for self-driving car s auonomous navigaion, in Inelligen Transporaion Sysems (ITSC), 5 IEEE 8h Inernaional Conference on. IEEE, 5, pp [6] R. Toledo-Moreo, M. A. Zamora-Izquierdo, B. Úbeda-Miñarro, and A. F. Gómez-Skarmea, High-inegriy imm-ekf-based road vehicle navigaion wih low-cos gps/sbas/ins, IEEE Transacions on Inelligen Transporaion Sysems, vol. 8, no. 3, pp. 9 5, 7. [7] P. Zhang, J. Gu, E. E. Milios, and P. Huynh, Navigaion wih imu/gps/digial compass wih unscened kalman filer, in Mecharonics and Auomaion, 5 IEEE Inernaional Conference, vol. 3. IEEE, 5, pp [8] F. Caron, M. Davy, E. Duflos, and P. Vanheeghe, Paricle filering for mulisensor daa fusion wih swiching observaion models: Applicaion o land vehicle posiioning, IEEE ransacions on Signal Processing, vol. 55, no. 6, pp , 7. [9] C. E. Whie, D. Bernsein, and A. L. Kornhauser, Some map maching algorihms for personal navigaion assisans, Transporaion research par c: emerging echnologies, vol. 8, no., pp. 9 8,. [] D. Kim, B. Kim, T. Chung, and K. Yi, Lane-level localizaion using an avm camera for an auomaed driving vehicle in urban environmens, IEEE/ASME Transacions on Mecharonics, 6. [] X. Huang, D. Zhao, and H. Peng, Empirical sudy of dsrc performance based on safey pilo model deploymen daa, arxiv preprin arxiv: , 6. [] D. Bezzina and J. Sayer, Safey pilo model deploymen: Tes conducor eam repor, Repor No. DOT HS, vol. 8, p. 7,. [3] N. Alam, A. T. Balaei, and A. G. Dempser, Relaive posiioning enhancemen in vanes: A igh inegraion approach, IEEE Transacions on Inelligen Transporaion Sysems, vol., no., pp. 7 55, 3. [] D. Wang, Cooperaive vx relaive navigaion using igh-inegraion of dgps and vx uwb range and simulaed bearing, Ph.D. disseraion, Universiy of Calgary, 5. [5] M. Rohani, D. Gingras, and D. Gruyer, A novel approach for improved vehicular posiioning using cooperaive map maching and dynamic base saion dgps concep, IEEE Transacions on Inelligen Transporaion Sysems, vol. 7, no., pp. 3 39, 6. [6] D. Wang, K. O?Keefe, and M. G. Peovello, Decenralized cooperaive navigaion for vehicle-o-vehicle (vv) applicaions using gps inegraed wih uwb range, in Proceedings of he ION Pacific PNT 3 Conference, Honolulu, HI, USA, 3, pp. 5. [7] M. Shen, D. Zhao, and J. Sun, Enhancemen of low-cos gnss localizaion in conneced vehicle neworks using rao-blackwellized paricle filers, in Inelligen Transporaion Sysems (ITSC), 6 IEEE 9h Inernaional Conference on. IEEE, 6, pp [8], The impac of road configuraion on vv-based cooperaive localizaion, arxiv preprin arxiv:73.98, 7. [9] A. Douce, N. De Freias, K. Murphy, and S. Russell, Raoblackwellised paricle filering for dynamic bayesian neworks, in Proceedings of he Sixeenh conference on Uncerainy in arificial inelligence. Morgan Kaufmann Publishers Inc.,, pp [] A. Giremus, J.-Y. Tournere, and V. Calmees, A paricle filering approach for join deecion/esimaion of mulipah effecs on gps measuremens, IEEE Transacions on Signal Processing, vol. 55, no., pp , 7. [] M. Monemerlo, S. Thrun, D. Koller, B. Wegbrei e al., Fasslam: A facored soluion o he simulaneous localizaion and mapping problem, in Aaai/iaai,, pp [] Saellie navigaion (sanav) oolbox 3., hp://gpsofnav.com/ producs/saellie-navigaion-sanav-oolbox-3-/. [3] M. S. Braasch and A. Van Dierendonck, Gps receiver archiecures and measuremens, Proceedings of he IEEE, vol. 87, no., pp. 8 6, 999. Macheng Shen Macheng Shen received his B. S. degree in 5 from Shanghai Jiao Tong Universiy and his M. S. E. degree in 6 from Universiy of Michigan, Ann Arbor. He is currenly a Ph. D. suden in Universiy of Michigan, Ann Arbor. His research ineres includes conneced vehicle localizaion and Bayesian filering. Ding Zhao Ding Zhao received he Ph.D. degree in 6 from he Universiy of Michigan, Ann Arbor. He is currenly a Research Fellow in he Universiy of Michigan Transporaion Research Insiue. His research ineres includes evaluaion of conneced and auomaed vehicles, vehicle dynamic conrol, driver behaviors modeling, and big daa analysis Jing Sun Jing Sun received her Ph. D. degree from Universiy of Souhern California in 989, and her B. S. and M. S. degrees from Universiy of Science and Technology of China in 98 and 98 respecively. From , she was an assisan professor in Elecrical and Compuer Engineering Deparmen, Wayne Sae Universiy. She joined Ford Research Laboraory in 993 where she worked in he Powerrain Conrol Sysems Deparmen. Afer spending almos years in indusry, she came back o academia and joined he faculy of he College of Engineering a he Universiy of Michigan in 3, where she is now Micheal G. Parsons Professor in he Deparmen of Naval Archiecure and Marine Engineering, wih couresy appoinmens in he Deparmen of Elecrical Engineering and Compuer Science and Deparmen of Mechanical Engineering. Her research ineress include sysem and conrol heory and is applicaions o marine and auomoive propulsion sysems. She holds 39 US paens and has co-auhored a exbook on Robus Adapive Conrol. She is an IEEE Fellow and a recipien of he 3 IEEE Conrol Sysem Technology Award. Huei Peng Huei Peng received he Ph.D. degree from he Universiy of California, Berkeley, CA, USA, in 99. He is currenly a Professor wih he Deparmen of Mechanical Engineering, Universiy of Michigan, Ann Arbor, MI, USA. He is currenly he U.S. Direcor of he Clean Energy Research CenerClean Vehicle Consorium, which suppors 9 research projecs relaed o he developmen and analysis of clean vehicles in he U.S. and in China. He also leads an educaion projec funded by he Deparmen of Energy o develop en undergraduae and graduae courses, including hree laboraory courses focusing on ransporaion elecrificaion. He has more han echnical publicaions, including 85 in refereed journals and ransacions. His research ineress include adapive conrol and opimal conrol, wih emphasis on heir applicaions o vehicular and ransporaion sysems. His curren research focuses include design and conrol of hybrid vehicles and vehicle acive safey sysems.