Paper Code: F02I131 MAP-AIDED POSITIONING SYSTEM Forssell, Urban 1 Hall, Peer 1 Ahlqvis, Sefan 1 Gusafsson, Fredrik 2 1 NIRA Dynamics AB, Sweden; 2 Linköpings universie, Sweden Keywords Posiioning; Navigaion; Geographical Informaion Sysems; Global Posiioning Sysem; Sensor Fusion Absrac This paper describes he vehicle posiioning sysem MAP (Map-Aided Posiioning) developed by NIRA Dynamics AB. MAP uses sensor fusion o combine relaive posiion informaion from he wheel speed sensors wih digial map informaion, and is capable of compuing an accurae esimae of a vehicle s absolue posiion wihou suppor from GPS or oher exernal posiioning service. MAP can also be combined wih GPS, in which case a very robus and accurae posiioning sysem is obained. MAP is available as a sofware module suiable for inegraion in a PDA or similar hardware plaform. 1. Inroducion This paper describes NIRA Dynamics AB s vehicle posiioning sysem MAP; MAP for Map-Aided Posiioning. MAP combines informaion from available posiion sensors (relaive and absolue) and a digial map in a saisically opimal way and produces accurae posiioning informaion ha e.g. can be used for navigaion purposes. Provided wih a rough iniial guess of he rue posiion, MAP is able o calculae an esimae of he rue, absolue posiion using only relaive posiion informaion from he wheel speed sensors and digial map informaion. MAP can herefore run as an auonomous posiioning sysem wihou GPS suppor and sill yield similar or beer funcionaliy (as GPS). MAP can also be combined wih sandard GPS-based soluions o improve he availabiliy and posiioning accuracy. Nex we will review he echnology area and give some background informaion o why MAP is ineresing. However, before coninuing a brief noe on he nomenclaure used: In our view, posiioning is abou deermining where you are relaive o some fixed reference, while navigaion is abou finding he opimal (e.g. neares or fases) pah from a posiion A o a posiion B. 1. 1 Building Blocks of a Modern Vehicle Navigaion Sysem Mos modern vehicle navigaion sysems can be described using he following hree-layer model, c.f. Figure 1. Applicaion layer Sysem layer Hardware: CPU, I/O, display ec GPS Relaive and inerial sensors Map Daabase Figure 1. Three-layer model illusraing he srucure of sandard navigaion sysems.
Saring from he op we have an applicaion layer feauring for example 1. Navigaion funcionaliy such as roue planning and roue guidance. 2. Advanced HMI feaures such as voice conrol and guidance and differen graphical presenaion feaures. 3. Oher services and funcionaliy including SOS alarm funcions, vehicle racking funcions (ani-hef), and various so called locaion based services funcions like direced adverisemen and ravel services including cusomized direcions o hoels, poins of ineres, gas saions, repair shops ec. The sysem layer consiss of low-level applicaions such as 1. Posiioning funcions ha calculae an esimae of he rue posiion using he available sensor informaion from GPS and ypically also relaive and/or inerial sensors. 2. Funcions for efficien map daabase handling. 3. HMI funcionaliy such as menu sysems and presenaion of posiion informaion (including map-maching funcions). The hird layer consiss of he physical hardware and sensors: 1. Hardware for inerfacing various sensors, for running various sofware applicaions, and for displaying he navigaion informaion. 2. GPS anenna and receiver, which gives posiion informaion. 3. Relaive posiion sensors (e.g. wheel speed and seering wheel sensors) and inerial sensors (e.g. yaw rae gyros and acceleromeers). 4. A map daabase, ofen sored in a read-only-memory such as CD ROM or DVD ROM In summary we hus see ha almos all vehicle navigaion sysems rely on GPS as he main posiion informaion source and employ maps sored on a CD or DVD ROM. Mos sysems also use relaive posiion informaion from e.g. inerial sensors o correc he GPS informaion. 1.2 Disadvanages Wih Today s Navigaion Sysems The navigaion sysems of oday which rely solely on GPS are no ideal, especially no in urban areas wih high buildings, muli-level highways, and unnels where saellie coverage may be poor and/or where he GPS signals are corruped by muli-pah and fading. To enhance performance under hese circumsances, he sysems herefore frequenly feaure dead reckoning funcionaliy, i.e. he sysems use relaive posiioning informaion obained from an inerial navigaion sysem (ypically based on wheel speed informaion and yaw rae informaion) o predic fuure posiions. However, despie hese effors o improve he posiioning accuracy of he GPS based sysems i is someimes surprisingly poor. And GPS based sysems ypically show he wors performance when hey are needed he mos: in dense urban areas. A furher feaure of many navigaion sysems is some kind of map maching o enhance he precision e.g. when urning around sharp corners and following characerisic road paerns. In principle, map maching is abou moving he symbol indicaing he vehicle s posiion on he map from he posiion calculaed by he posiioning module o he neares road. If done correcly his can reduce he posiioning error and also improve he user inerface. A problem in many sysems, hough, is ha he posiioning and map-maching funcions are no inegraed and do no suppor each oher, which someimes resuls in sub-opimal performance. As an example consider a case where he GPS signal is los and he sysem relies on dead reckoning informaion only for a period of ime. Then, due o errors accumulaed in he dead reckoning sysem, he map-maching algorihm will have an almos fuile job rying o cach up wih he rue posiion. This ype of (poor) behavior is very common in oday s navigaion sysems. Anoher drawback wih GPS based sysems is he relaively high oal sysem cos, including anenna, receiver, cables, connecors, insallaion ec. Due o he coss for he anenna, cables, connecors, and insallaion he overall cos will never drop o levels near zero. Since cos is an issue for all sysem inegraors, he suppliers of hese kinds of sysems have o build in more value in heir produc. And he improved posiioning accuracy and higher availabiliy provided by MAP is one way o do his.
2. General Descripion of MAP 2.1 Basic Feaures of MAP MAP is a posiioning sysem for vehicles designed o give very accurae posiion informaion, which can be used in a navigaion sysem of he ype described above. Through he use of advanced nonlinear filering echniques MAP fuses informaion from a DRS (Dead Reckoning Sysem) and a digial map. Given an iniial (rough) esimae of he rue posiion MAP can auonomously calculae he rue, absolue posiion of he vehicle wihou suppor from exernal posiion informaion sources such as GPS. Among oher hings his means ha MAP does no have any problems in areas where he GPS accuracy is poor e.g. near high-rises and in unnels. MAP also allows new, cheaper navigaion sysems o be buil as he hardware requiremens are reduced (no GPS anenna, no cables, no GPS receiver ec). We are currenly focusing on wo main applicaions of he MAP echnology: 1. A cheap, flexible navigaion sysem o be used in ciies. 2. An enhanced GPS navigaion sysem. In he firs case one idea is o combine MAP wih GSM posiioning for iniializaion and suppor and digial maps downloaded o a read-wrie memory such as a Smar Card or similar. Here he working principle is basically as follows. When saring he sysem afer a memory loss, so called cold sar, he sysem connecs o a GSM posiioning service offered by some mobile operaor. This gives an iniial esimae of he hrough posiion wih an accuracy of 300-3000m nominally (he exising GSM posiioning service delivers posiion informaion in form of a secor area in a mobile elephony cell). Using his informaion he MAP algorihm is iniialized and as he vehicle moves and relaive posiion informaion is calculaed by he dead reckoning sysem, MAP can deermine he rue, absolue posiion by fusing his relaive posiion informaion wih he map informaion using he nonlinear filering algorihm. Our idea is o implemen he above sysem on a PDA, which also can be used for oher purposes ouside he vehicle (e.g. navigaing o he neares resauran using oher GSM posiioning services). I is however possible o hink of a number of oher ways o implemen he sysem, one being o mimic he design of oday s GPS based sysems. For insance, insead of using maps downloaded using a wireless link one can use maps sored on permanen media such as CD or DVD ROM. Insead of using GSM posiioning for iniializaion and suppor i is possible o use manual sar where he user inpus an area in which MAP should sar he search. I is also possible o use oher radio-based services such as RDS for his purpose wih minor modificaions. In he second case, he idea is o uilize o bes feaures of he GPS and MAP echnologies o consruc a really highperformance sysem ha mees he markes requiremens on posiioning accuracy and reliabiliy in he op-of-he-line navigaion sysems. In his case he MAP algorihm will fuse informaion from he GPS, he dead reckoning funcion, and from he digial map in a saisically opimal way. A nice feaure of his sysem is ha loss of GPS informaion is handled auomaically and o calculae he posiion esimae he bes available informaion is always used. This enhanced GPS sysem will herefore no have problems in dense urban areas, in unnels ec. And since i combines informaion from several differen sources in an opimal way you will always ge he bes possible posiion esimae. Our produc in his case is firs and foremos a sofware module implemening he MAP algorihm, which akes inpus from he GPS, he dead reckoning sysem, and he map daabase and compues an accurae posiion esimae. The idea is ha his sofware should be inegraed in a navigaion sysem and hus complemened wih rouines for graphical presenaion, map maching, roue planning ec. 2.2 Background The algorihm used in MAP is a recursive Bayesian esimaion algorihm. Such algorihms have become increasingly popular in academia [4,5] and some applicaion areas, mainly miliary [6]. A sysem for errain navigaion using Bayesian esimaion is known from [1]. The main resul in [1] is a navigaion sysem using errain aliude informaion sored in a GIS (Geographical Informaion Sysem) combined wih aliude measuremens from radar. MAP combines advanced nonlinear filering echniques such as he ones discussed in [1] wih a novel use of digial road map informaion, hus obaining a posiioning sysem for vehicles wih very high posiion accuracy wihou he drawbacks of oday s (saellie-based) posiioning/navigaion sysems.
2.3 Sysem Design This secion is devoed o a general descripion of he MAP sysem. As shown in Figure 2, MAP calculaes an accurae posiion esimae using all possible sensors and services, e.g. GPS and/or GSM posiioning or Inerial and relaive posiion sensors Posiioning algorihm GPS/GSM/RDS Posiion Esimae Daabase handling Map Daabase MAP Figure 2. Block diagram descripion of MAP. similar, relaive sensors, inerial sensors, and digial map daabase informaion. This posiion esimae can be used by he navigaion rouines as described above. The inernal srucure of he sysem consiss of wo main funcions/modules: he posiioning algorihm and he rouines for he map daabase handling. The former will be described nex, he laer furher down. Here we can noe ha hese wo modules, alhough separaed here for pedagogical reasons, are very closely coupled and inerac in each ieraion of he recursive MAP algorihm. 2.4 Concepual Descripion of he Posiioning Algorihm The underlying idea in MAP springs from a saisical viewpoin, where he knowledge abou he vehicles posiion a each ime insan is compleely summarized by he condiional probabiliy densiy funcion (PDF). The PDF evolves wih ime (using dynamical models) and informaion conained in observaions made on he sysem. Le us consider a simple example: Assume ha we know ha he vehicle we wan o posiion has been raveling along a sraigh road for quie some ime. The PDF will hen ell us ha he vehicle, mos likely, is locaed somewhere on ha road. The PDF is illusraed in Figure 3. When he vehicle laer makes a righ-hand urn, informaion abou he movemen and he spaial configuraion of he map is fused ino he PDF (Figure 4). The resul is ha he mass of he PDF is more concenraed o he rue posiion, i.e. he uncerainy abou where he vehicle is locaed has been reduced. Figure 3. The PDF before he urn is disribued along he sraigh ahead road wih no well defined peak.
Figure 4. Afer he righ-hand urn he PDF is concenraed wih a significan peak a he rue posiion. The MAP approach is o recursively esimae he condiional PDF using informaion from vehicle-mouned sensors measuring relaive movemens (e.g. wheel-speed sensors) and a digial road map. This ype of signal processing problem is ofen referred o as sensor fusion and is usually ackled by saisical mehods. Here, he recursive esimaion of he condiional probabiliy for he vehicle's posiion will be formulaed using a Bayesian framework, aiming a a nonlinear filering algorihm. Sensors, which provide relaive posiion informaion, e.g. inerial sensors (acceleromeers, yaw rae gyros ec.) and wheel speed sensors, will in he sequel be referred o as a dead reckoning sysem (DRS). 3. The Posiioning Algorihm 3.1 Idea Now we will describe he posiioning algorihm presened briefly above using a mahemaical approach. To illusrae he basic ideas, he following simple sae space model can be used: x = x + u + w +1 (1) y = h( x ) + e (2) The sae x represens he vehicle s posiion on he map. A each ieraion, his is updaed using u, which is he relaive movemen obained from he DRS. Drif in he DRS is modeled by addiive i.i.d. (independen idenically disribued) noise, w, wih probabiliy disribuion p ( ). The measuremen,, consiss of he nonlinear funcion w h( ), evaluaed a he curren posiion, plus i.i.d. addiive measuremen noise, e, wih probabiliy disribuion p ( ). e is assumed independen of w. I should be noed ha h ( ) is used o inroduce map informaion in he model. Since i is assumed ha he vehicle is driving on a road in he road nework, h ( ) can e.g. represen he shores disance o he neares road a ime, while y represens a ficiious measuremen of his disance a ime. This measuremen of course always equals zero, according o he assumpion made above. Le f (x) denoe he condiional probabiliy densiy funcion (PDF) for he sae, given he measuremens up o x Y he ime. In a Bayesian framework he PDF can be recursively updaed in wo seps [2]: Measuremen updae: 1 f ( x) = f ( y h( x)) f ( x) e (3) x Y x Y 1 c y x e
Time updae: f x Y + 1 ( x) = f ( χ) f ( x χ u ) dχ 2 R x Y w (4) However, due o he non-linear naure of he esimaion problem, hese expressions canno be evaluaed analyically. Therefore, some kind of discreizaion of he sae space is necessary. Below we will ouline wo possible implemenaions: one poin mass filer (PMF) and one sequenial Mone Carlo filer. The laer is ofen also referred o as a paricle filer (PF). In boh cases he algorihm has a recursive srucure as illusraed in Figure 5. Measuremen Updae Calculae Esimae and Covariance Time Updae Figure 5. The recursive srucure of he nonlinear filer The algorihm consiss of wo basic seps: A ime updae sep, in which he soluion is propagaed according o he sae ransiion equaion (1), and a measuremen updae sep, in which new informaion is fused ino he soluion according o (2). The soluion o he esimaion problem is compleely specified by he probabiliy densiy funcion (PDF). Our primary ineres, however, is o esimae he posiion (he sae ). Thus a calculaion of such an esimae along wih he associaed error covariance forms he hird sep in he ieraion. 3.2 Implemenaion Here we will describe wo ways of implemening he MAP algorihm, firs using a PMF and second using a PF. The PMF will be described only briefly, while focus will be on he PF. 3.2.1 Poin Mass Filer (PMF) Wih he PMF one discreizes he sae space (he map) using a grid, see Figure 6. x Figure 6. The PDF discreized by a homogenous grid (deerminisic sampling).
The Bayesian soluion, obained in Secion 3.1, is hen applied o each grid poin, i.e. he PDF is represened by a se of poin-masses, or weighs. Measuremen updae The measuremen updae re-compues he weighs using he informaion in he new measuremens according o (3). The normalizing consan is obained by calculaing he sum of all new weighs. Time updae The ime updae consiss of a ranslaion of he grid poins plus convoluion of he probabiliy densiy funcion wih he uncerainy in he relaive movemen (obained from dynamic sensor models). Mahemaically his is a resul of numerical inegraion of (4). More deails abou he PMF can be found in [1]. 3.2.2 Paricle Filer (PF) In boh he PMF and he PF he Bayesian problem is ackled using quanizaion of he sae space. In he PF case his means ha he PDF is represened by a number of i.i.d. (independen idenically disribued) samples, referred o as paricles (see e.g. [1],[2]). The sampling echnique used in he PF is called Mone Carlo (MC) sampling. The main advanage of his mehod is ha he samples of he PDF are auomaically chosen in pars of he sae space ha are imporan for he inegraion resul, i.e. more samples are drawn from regions conaining mos of he PDF mass. This is illusraed in Figure 7. Figure 7. The PDF sampled by Mone Carlo simulaion. The basic algorihm consiss of wo seps: A ime updae sep, in which he soluion is propagaed according o he sae ransiion equaion, and a measuremen updae sep, in which new informaion is fused ino he soluion. Time updae In he ime updae he paricles (which can be considered as candidaes o he rue posiion) are ranslaed using relaive displacemen, measured by he DRS, and realizaions of he process noise. Measuremen updae The measuremen updae calculaes a new, so called imporance weigh for each paricle based on he oucome of he measuremen equaions, when applied o he curren paricle. The weigh can be considered as a sampled value of he poserior PDF. In order o keep he calculaions sound in a probabilisic sense, he weighs need o be normalized.
Resampling (Boosrap) To make sure ha he paricles remain i.i.d. samples from he PDF, resampling has o be performed on a regular basis. This is done by drawing samples wih replacemen unil a cerain number of new paricles are obained. The probabiliy of resampling a specific paricle a each draw is equal o is weigh. Calculaion of esimaes and confidence parameers The (approximae) soluion o he esimaion problem is compleely specified by he paricle swarm. However, he primary ineres is usually o obain various esimaes and confidence parameers. There are mainly wo ways of calculaing esimaes, hrough expecaion or hrough maximum a poseriori (m.a.p.). Expecaion is easily performed by calculaing a weighed sum over all paricles. The m.a.p. esimae is also very sraighforward jus pick he paricle wih he larges weigh. The covariance marix is obained by calculaing he second momen, if he esimae was obained by expecaion. The same procedure applied o a m.a.p. esimae yields a correlaion marix, since he m.a.p. esimae is no necessarily unbiased. For a more deailed presenaion of he PF see [1],[2].
4. Example In order o illusrae he PF funcionaliy we will look a an example where wheel speed measuremens were colleced during a es drive in a suburban area. Four snapshos showing he propagaing paricle cloud for his paricular scenario are depiced in Figure 8. Iniially he paricles are randomly disribued along he srees in he area. When he movemens of he vehicle (i.e. he wheel speed signals) are insered ino he filer he paricles sar revealing he acual posiion of he vehicle. The speed of convergence of he filer is of course dependen on he informaion conens in he measuremens and he sree nework. If he vehicle rajecory includes several heading angle changes (urns) his will improve he convergence, wih respec o he spaial configuraion of he map. Figure 8. Four snapshos showing he propagaion of he paricle cloud from an auhenic es drive. A firs he paricles are disribued on he major roads. Then, as he vehicle moves, he paricles sar racking he rue posiion. 5. Map Daabase Handling Since he map daabase is one of he mos fundamenal componens in map-aided posiioning, he map daa handling is a vial par of he sysem. These rouines should be able o exrac he informaion needed by he posiioning algorihm from he daabase as quickly as possible. The measuremen equaions in he somewha simplified model presened in Secion 3 use a single quaniy provided by he map: The shores disance from an arbirary poin o he road nework. This can be obained hrough a limiing search operaion and minimizaion. Figure 9 illusraes how map informaion is rerieved in he measuremen updae sep.
Apar from performing he daa access operaions discussed above he map handling rouines should handle he updae of acual map daabase. I is obvious ha he size of he map ha is sored in he inernal memory and operaed on by he algorihm is limied by several facors. Therefore he sysem should be able o ell when he curren map needs a refresh and perform such an updae wihou disurbing he real-ime performance. Call from measuremen updae Limi he size of he search area For each paricle Calculae minimum disance o all road segmens in search area Pick he minimum disance Done Reurn o measuremen updae Figure 9. The map daa updae 7. Exensions 7.1 Heading Angle Esimaion The model sudied above does no include esimaion of he heading angle of he vehicle. However, in order o make he MAP sysem auonomous, ha has o be handled by he algorihm. Deails how he model should be exended o solve his can be found in [2]. 7.2 Sensor Error Modeling The sensors used in he MAP sysem are associaed wih several kinds of errors. Here, hese have been considered as whie noise processes. This is, however, no an appropriae assumpion, and he error models migh need o be refined in order o achieve decen performance. We will no presen any deails here, bu beer error models can be insered ino he filer srucure discussed so far, or, since errors ofen are addiive, i is also possible o esimae hese parameers using convenional linear filers, such as he Kalman Filer. 8. Summary and Conclusions The MAP sysem implemens an auonomous posiioning echnique, uilizing exising vehicle mouned sensors and digial road maps. I also eleganly solves he map-maching problem by adoping sensor fusion ideas. The MAP sysem can be inegraed in convenional saellie-based sysems in order o improve he accuracy and reliabiliy. References [1] N. Bergman. Recursive Bayesian Esimaion: Navigaion and Tracking Applicaions. PhD Thesis. Linköping Universiy. May 1999. [2] P. Hall. A Bayesian Approach o Map-Aided Vehicle Posiioning. Maser Thesis. Linköping Universiy. January 2001.
[3] N. Gordon, D. Salmond, A. Smih. Novel approach o nonlinear/non-gaussian Bayesian sae esimaion. IEE Proceedings on Radar and Signal Processing, 140:107-113, 1993. [4] P. Fearnhead. Sequenial Mone Carlo Mehods in filer heory. PhD hesis. Universiy of Oxford. 1998. [5] M. Pi and N. Shephard. Filering via simulaion: Auxiliary paricle filer. Journal of he American Saisical Associaion, 94(446):590-599. [6] F. Gusafsson, F. Gunnarsson, N. Bergman, U. Forssell, J. Jansson, R. Karlsson, P.J. Nordlund. Paricle filers for posiioning, navigaion and racking. IEEE Transacions on Signal Processing. February 2002.