Localization & Mapping

Transcription

1 Auonomous Moble Robos, Chaer 5 CSE360/ Inroducon o Moble Robocs Localaon & Mang Dearmen of Comuer Scence & Engneerng P.C. Rossn College of Engneerng and Aled Scence R. Segwar, I. Nourbahsh CSE360/460 Inro o Moble Robocs oday s Agenda leel Requremens Homewor 4 Preew Localaon I 4/3/008

2 Auonomous Moble Robos, Chaer 5 Localaon and Ma Buldng 5 Nose and alasng; odomerc oson esmaon o locale or no o locale Belef reresenaon Ma reresenaon Probablsc ma-based localaon Oher eamles of localaon sysems Auonomous ma buldng Localaon "Poson" Global Ma Cognon Enronmen Model Local Ma Perceon Real World Enronmen Pah Moon Conrol R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Localaon, Where am I?? 5. oson Poson Udae Esmaon? Encoder Predcon of Poson e.g. odomery YES mached obseraons Ma daa base redced oson Machng Odomery, Dead Reconng Localaon base on eernal sensors, beacons or landmars Probablsc Ma Based Localaon Perceon Obseraon raw sensor daa or eraced feaures R. Segwar, I. Nourbahsh 4/3/008

3 Auonomous Moble Robos, Chaer 5 Challenges of Localaon 5. Knowng he absolue oson e.g. GPS s no suffcen Localaon n human-scale n relaon wh enronmen Planng n he Cognon se requres more han only oson as nu Perceon and moon lays an moran role Sensor nose Sensor alasng Effecor nose Odomerc oson esmaon R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Sensor Nose 5.. Sensor nose n manly nfluenced by enronmen e.g. surface, llumnaon AND by he measuremen rncle self e.g. nerference beween ulrasonc sensors Sensor nose drascally reduces he useful nformaon of sensor readngs. he soluon s: o ae mulle readng no accoun emloy emoral and/or mul-sensor fuson R. Segwar, I. Nourbahsh 4/3/008 3

4 Auonomous Moble Robos, Chaer 5 Sensor Alasng 5.. In robos, non-unqueness of sensors readngs s he norm Een wh mulle sensors, here s a many-o-one mang from enronmenal saes o robo s erceual nus herefore he amoun of nformaon erceed by he sensors s generally nsuffcen o denfy he robo s oson from a sngle readng Robo s localaon s usually based on a seres of readngs Suffcen nformaon s recoered by he robo oer me R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Effecor Nose: Odomery, Dead Reconng 5..3 Odomery and dead reconng: Poson udae s based on rorocee sensors Odomery: wheel sensors only Dead reconng: also headng sensors he moemen of he robo, sensed wh wheel encoders and/or headng sensors s negraed o he oson. Pros: Sragh forward, easy Cons: Errors are negraed -> unbound Usng addonal headng sensors e.g. gyroscoe mgh hel o reduce he cumulaed errors, bu he man roblems reman he same. R. Segwar, I. Nourbahsh 4/3/008 4

5 Auonomous Moble Robos, Chaer 5 Odomery: Error sources 5..3 deermnsc sysemac non-deermnsc non-sysemac Deermnsc errors can be elmnaed by roer calbraon of he sysem. Non-deermnsc errors hae o be descrbed by error models and wll always lead o unceran oson esmae. Major Error Sources: Lmed resoluon durng negraon me ncremens, measuremen resoluon, ec. Msalgnmen of he wheels deermnsc Unequal wheel dameer deermnsc Varaon n he conac on of he wheel Unequal floor conac slng, no lanar, ec R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer Odomery: Classfcaon of Inegraon Errors Range error: negraed ah lengh dsance of he robos moemen Sum of he wheel moemens urn error: smlar o range error, bu for urns Dfference of he wheel moons Drf error: dfference n he error of he wheels leads o an error n he robos angular orenaon Oer long erods of me, urn and drf errors far ouwegh range errors! Consder mong forward on a sragh lne along he as. he error n he y-oson nroduced by a moe of d meers wll hae a comonen of dsn θ θ, whch can be que large as he angular error θ grows. R. Segwar, I. Nourbahsh 4/3/008 5

6 Auonomous Moble Robos, Chaer 5 Odomery: he Dfferenal Dre Robo 5..4 y θ y θ s R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Odomery: he Dfferenal Dre Robo 5..4 Knemacs R. Segwar, I. Nourbahsh 4/3/008 6

7 4/3/008 7 CSE360/460 Inro o Moble Robocs SA Refresher he eeced alue for a random arable X s.e. he mean defned as he arance of X abou he mean s defned as X d X E X X E X n connuous for dscree for µ µ X d E X E X n for connuous ] [ for dscree ] [ µ µ σ µ µ σ Auonomous Moble Robos, Chaer 5 R. Segwar, I. Nourbahsh When X s a ecor, he arance s eressed n erms of a coarance mar C where he resulng mar has he form whereρ j corresonds o he degree of correlaon beween arables X and X j ] [ j j j E c µ µ n n n n n n n n n C σ σ σ ρ σ σ ρ σ σ ρ σ σ σ ρ σ σ ρ σ σ ρ σ K M O M M L K SA Refresher

8 CSE360/460 Inro o Moble Robocs he Correlaon Coeffcen Correlaon s a means o esmae how wo funcons/seres are correlaed. For a dscree seres, s defned as ρ [ µ y µ y ] µ y µ y C C y C yy C y σ σ y where ρ denoes he correlaon coeffcen he denomnaor normales he correlaon coeffcen such ha ρ [,] CSE360/460 Inro o Moble Robocs Correlaon Eamles 4/3/008 8

9 CSE360/460 Inro o Moble Robocs Correlaon Eamles CSE360/460 Inro o Moble Robocs Correlaon Eamles 4/3/008 9

10 CSE360/460 Inro o Moble Robocs Proeres of he Coarance Mar he coarance mar C s symmerc and ose defne hrough a smlary ransform wh he roer roaon mar R, C can be decomosed as CRDR, where C [ EVE L EVE ] n σ O 0 0 EVE 0 M σ n EVE n ha s, he columns of R corresond o he egenecors of C, and he elemens of he dagonal mar D s egenalues hese also corresond o he rmary aes of he PDF and he arances, resecely hs means you can always defne a coordnae sysem where he alues wll be uncorrelaed! CSE360/460 Inro o Moble Robocs Eamle QUESION: Gen a normal dsrbuon wh coarance mar characere he PDF C SOLUION: Solng for he egenalues, we ge Wha hs means for us s ha we can always erac a nce D ellse o reflec our osonal uncerany on he lane. λ 5.47 λ λ 9, λ σ 9, σ 0 and solng for he frs EVE we oban σ σ an o 4.6 4/3/008 0

11 4/3/008 CSE360/460 Inro o Moble Robocs he Gaussan Nose Assumon A -D Gaussan dsrbuon s defned as In -D assumng uncorrelaed arables hs becomes In n dmensons, generales o σ µ πσ e σ µ σ µ σ πσ e r µ µ π C n e C r Gaussan he Normal Gaussan dsrbuon s comleely arameered by s frs and second momens. Auonomous Moble Robos, Chaer 5 R. Segwar, I. Nourbahsh Gen wo ndeenden random arables X,Y wh resece normal dsrbuons, hen Howeer, n many cases when we roagae he coarance, he underlyng funcon needs s NO lnear Q: How do you hn we could do hs? A: Lnearaon. Agan, our wonderful frend he aylor seres comes o he rescue Proeres of Gaussans... ' ε ε f f f f... ε ε J f f f, ~ N X σ µ, ~ y y N Y σ µ, ~ y y N Y X Z σ σ µ µ, ~ a b a N b ax σ µ

12 CSE360/460 Inro o Moble Robocs ransformng Uncerany Le s say we now he uncerany of a arable, and we wan o comue he uncerany of yf We now ha ε where s he dsrbuon mean and ε s ero mean nose We can hen use he Jacoban o lnearly aromae y y f f ε f Jε he mean of he dsrbuon would hen be y E[ y] E[ f Jε ] f herefore y y Jε CSE360/460 Inro o Moble Robocs ransformng Uncerany he coarance of he ransformed dsrbuon would hen be C y E[ y y y y ] E[ Jεε J ] hus, o ransform uncerany across a non-lnear ransformaon, we erform a smlary ransform wh he Jacoban Noe ha because of he symbols on he reous age, normal dsrbuons are NO resered JC J 4/3/008

13 Auonomous Moble Robos, Chaer 5 Odomery: he Dfferenal Dre Robo Error model f f f 3 R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer Odomery: Growh of Pose uncerany for Sragh Lne Moemen Noe: Errors erendcular o he drecon of moemen are growng much faser! R. Segwar, I. Nourbahsh 4/3/008 3

14 Auonomous Moble Robos, Chaer Odomery: Growh of Pose uncerany for Moemen on a Crcle Noe: Errors ellse n does no reman erendcular o he drecon of moemen! R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 o Locale or No o Locale? 5.3 How o nagae beween A and B nagaon whou hng obsacles deecon of goal locaon Possble by followng always he lef wall Howeer, how o deec ha he goal s reached R. Segwar, I. Nourbahsh 4/3/008 4

15 Auonomous Moble Robos, Chaer 5 Behaor Based Nagaon 5.3 R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer Model Based Nagaon R. Segwar, I. Nourbahsh 4/3/008 5

16 Auonomous Moble Robos, Chaer 5 Belef Reresenaon 5.4 a Connuous ma wh sngle hyohess b Connuous ma wh mulle hyohess c Dscreed ma wh robably dsrbuon d Dscreed oologcal ma wh robably dsrbuon R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Belef Reresenaon: Characerscs 5.4 Connuous Precson bound by sensor daa Sngle or mulle hyohess ose esmae Los when dergng for sngle hyohess Comac reresenaon and ycally reasonable n rocessng ower. Dscree Precson bound by resoluon of dscresaon ycally mulle hyohess ose esmae Neer los when derges conerges o anoher cell Imoran memory and rocessng ower needed. no he case for oologcal mas R. Segwar, I. Nourbahsh 4/3/008 6

17 Auonomous Moble Robos, Chaer 5 Sngle-hyohess Belef Connuous Lne-Ma 5.4. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Sngle-hyohess Belef Grd and oologcal Ma 5.4. R. Segwar, I. Nourbahsh 4/3/008 7

18 Auonomous Moble Robos, Chaer 5 Grd-base Reresenaon - Mul Hyohess 5.4. Grd se around 0 cm. Couresy of W. Burgard Plannng and Comuaonal Issues! R. Segwar, I. Nourbahsh CSE360/460 Inro o Moble Robocs Mul-Hyohess Issues 5.4 Can elcly manan uncerany esmae regardng he robo s oson Allows he use of aral nformaon Howeer: Where s he robo? How do you handle ah lannng? 4/3/008 8

19 CSE360/460 Inro o Moble Robocs 5.5 Ma Reresenaon Dual of reresenng he robo s ossble osons Effecs he choces aalable for robo oson reresenaon hree Fundamenal quesons:. Ma recson s. alcaon. Feaures recson s. ma recson 3. Precson s. comuaonal comley Connuous Reresenaon Eac decomoson of he enronmen Dscreaon Auonomous Moble Robos, Chaer Reresenaon of he Enronmen Enronmen Reresenaon Connuous Merc Dscree Merc Dscree oologcal Enronmen Modelng,y,θ merc grd oologcal grd Raw sensor daa, e.g. laser range daa, grayscale mages o large olume of daa, low dsnceness on he leel of nddual alues o maes use of all acqured nformaon Low leel feaures, e.g. lne oher geomerc feaures o medum olume of daa, aerage dsnceness o flers ou he useful nformaon, sll ambgues Hgh leel feaures, e.g. doors, a car, he Effel ower o low olume of daa, hgh dsnceness o flers ou he useful nformaon, few/no ambgues, no enough nformaon R. Segwar, I. Nourbahsh 4/3/008 9

20 Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Connuous Lne-Based 5.5. a Archecure ma b Reresenaon wh se of nfne lnes Key adanage s oenally hgh accuracy. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Decomoson 5.5. Eac cell decomoson R. Segwar, I. Nourbahsh 4/3/008 0

21 CSE360/460 Inro o Moble Robocs Decomoson Sraeges 5.4 Absracon he underlyng assumon s ha he eac oson of a robo whn each area of free sace does no maer essellae he enronmen no useful regons Loss of ma fdely can somemes be aaren Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Decomoson 5.5. Fed cell decomoson Narrow assages dsaear R. Segwar, I. Nourbahsh 4/3/008

22 Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Decomoson Adae cell decomoson R. Segwar, I. Nourbahsh CSE360/460 Inro o Moble Robocs Occuancy Grd 5.4 Enronmen reresened by a dscree grd Each cell classfed as eher free or occued by an obsacle Each cell has eher a couner or a PDF assocaed wh s lelhood of beng an obsacle or oen A hreshold on he number of hs/robably s used for decdng wha s/s no an obsacle If range measuremens ass hrough cell, couner/robably dscouned Lle Ben Vdeo 4/3/008

23 Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Decomoson Fed cell decomoson Eamle wh ery small cells Couresy of S. hrun R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer Ma Reresenaon: Decomoson 5 oologcal Decomoson R. Segwar, I. Nourbahsh 4/3/008 3

24 Auonomous Moble Robos, Chaer 5 oologcal Mas Grah reresenaon Nodes reresen areas n he world Edges reresen connecy beween adjacen areas Demarcaon of areas/ransons s uned o sensor caables Red room Inersecon deecor Doorway deecor An adanage of he oologcal reresenaon s ha embeds nongeomerc nformaon Color RFID R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Decomoson oologcal Decomoson node Connecy arc R. Segwar, I. Nourbahsh 4/3/008 4

25 Auonomous Moble Robos, Chaer 5 Ma Reresenaon: Decomoson oologcal Decomoson A lmaon s ha sensors are ofen uned o he enronmen and do no ranslae well from one o anoher. ~ 00 m ~ 400 m ~ m ~ 50 m ~ 0 m R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Curren Challenges n Ma Reresenaon Real world s dynamc Perceon s sll a major challenge Error rone Eracon of useful nformaon dffcul raersal of oen sace How o buld u oology boundares of nodes Sensor fuson R. Segwar, I. Nourbahsh 4/3/008 5

26 Auonomous Moble Robos, Chaer Probablsc, Ma-Based Localaon Consder a moble robo mong n a nown enronmen. As sars o moe, say from a recsely nown locaon, mgh ee rac of s locaon usng odomery. Howeer, afer a ceran moemen he robo wll ge ery unceran abou s oson. udae usng an obseraon of s enronmen. Obseraons also yeld an esmae of he robos oson whch can hen be fused wh he odomerc esmaon o ge he bes ossble udae of he robos acual oson. R. Segwar, I. Nourbahsh CSE360/460 Inro o Moble Robocs Bayes Law Reew A B: he robably of A gen ha B has occurred he condonal robably of A on B heorem of Comound Probably: Bayes Law A B A B B A A A B A B A B B B A B A B A A B B A A B 4/3/008 6

27 CSE360/460 Inro o Moble Robocs A Smle Eamle Suose our robo s ryng o deec obsacles from a measuremen s Wha s obsacles? s CSE360/460 Inro o Moble Robocs A Smle Eamle obsacles n racce s dffcul o measure elcly Snce we hae a sensor model, s ofen easer o ge sobsacle By alyng Bayes law we oban obs s s obs obs s 4/3/008 7

28 CSE360/460 Inro o Moble Robocs Normalaon Process obs s s obs obs s! obs s s! obs! obs s obs s! obs s s s obs obs s! obs! obs obs s s obs s obs obs obs s! obs! obs CSE360/460 Inro o Moble Robocs A Smle Eamle 3 Suose our robo s ryng o deec obsacles from a measuremen s Wha s obsacledeecon IF obsacle 0. deeconobsacle0.9 deeconno obsacle * 0. o s 0.9 * * s 4/3/008 8

29 4/3/008 9 CSE360/460 Inro o Moble Robocs Wha f Addonal Informaon s Aalable? Wh addonal nformaon C aalable, Bayes heorem becomes he recurse formula for Bayesan Udang s hen If B n s ndeenden from B,,B n GIVEN A hs reduces o,, B B B A B A B B B A,...,,...,,...,,,..., n n n n n n B B B B B A B B A B B B A,...,,...,,..., n n n n n B B B B B A A B B B A CSE360/460 Inro o Moble Robocs Suose our robo has a second deecor s deeconobsacle0.5 deeconno obsacle * * *!!,, 3, * * * 0. s o o s s o o s s o o s s s s o s o s s s o s o s o A Smle Eamle 4!! obs obs s obs obs s obs obs s s obs

30 CSE360/460 Inro o Moble Robocs Or Equalenly obs s s obs obs s obs obs s! obs! obs Suose our robo has a second deecor s deeconobsacle0.5 deeconno obsacle0.05 o s 0.5 * * * , o s 3 o s, s s s o, s s o s s s o o s o o s s! o! o s * * * Auonomous Moble Robos, Chaer 5 Probablsc, Ma-Based Localaon 5.6. Acon udae acon model AC wh o : Encoder Measuremen, ncreases uncerany Perceon udae erceon model SEE s - : ror belef sae wh : eerocee sensor nus, decreases uncerany s : udaed belef sae R. Segwar, I. Nourbahsh 4/3/008 30

31 Auonomous Moble Robos, Chaer Imrong belef sae by mong R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 he Fe Ses for Ma-Based Localaon 5.6. Encoder Predcon of Measuremen and Poson odomery oson esmae Esmaon fuson Ma daa base redced feaure obseraons YES Machng mached redcons and obseraons. Predcon based on reous esmae and odomery. Obseraon wh on-board sensors 3. Measuremen redcon based on redcon and ma 4. Machng of obseraon and ma 5. Esmaon -> oson udae oseror oson Perceon raw sensor daa or eraced feaures Obseraon on-board sensors R. Segwar, I. Nourbahsh 4/3/008 3

32 Auonomous Moble Robos, Chaer 5 Maro Kalman Fler Localaon 5.6. Maro localaon localaon sarng from any unnown oson recoers from ambguous suaon. Howeer, o udae he robably of all osons whn he whole sae sace a any me requres a dscree reresenaon of he sace grd. he requred memory and calculaon ower can hus become ery moran f a fne grd s used. Kalman fler localaon racs he robo and s nherenly ery recse and effcen. Howeer, f he uncerany of he robo becomes o large e.g. collson wh an objec he Kalman fler wll fal and he oson s defnely los. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon 5.6. Maro localaon uses an elc, dscree reresenaon for he robably of all oson n he sae sace. hs s usually done by reresenng he enronmen by a grd or a oologcal grah wh a fne number of ossble saes osons. Durng each udae, he robably for each sae elemen of he enre sace s udaed. R. Segwar, I. Nourbahsh 4/3/008 3

33 Auonomous Moble Robos, Chaer Maro Localaon : Alyng robably heory o robo localaon PA: Probably ha A s rue. e.g. r l : robably ha he robo r s a locaon l a me We wsh o comue he robably of each nddual robo oson gen acons and sensor measures. PAB: Condonal robably of A gen ha we now B. e.g. r l : robably ha he robo s a oson l gen he sensors nu. Produc rule: Bayes rule: R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer Maro Localaon 3: Alyng robably heory o robo localaon Bayes rule: Ma from a belef sae and a sensor nu o a refned belef sae SEE: l: belef sae before erceual udae rocess l: robably o ge measuremen when beng a oson l o consul robos ma, denfy he robably of a ceran sensor readng for each ossble oson n he ma : normalaon facor so ha sum oer all l for L equals. R. Segwar, I. Nourbahsh 4/3/008 33

34 Auonomous Moble Robos, Chaer Maro Localaon 4: Alyng robably heory o robo localaon Bayes rule: Ma from a belef sae and a acon o new belef sae AC: Summng oer all ossble ways n whch he robo may hae reached l. Maro assumon: Udae only deends on reous sae and s mos recen acons and erceon. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy - oologcal Ma he Dersh Robo oologcal Localaon wh Sonar 5.6. R. Segwar, I. Nourbahsh 4/3/008 34

35 Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy - oologcal Ma oologcal ma of offce-ye enronmen 5.6. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy - oologcal Ma 3 Udae of belee sae for oson n gen he erce-ar 5.6. n : new lelhood for beng n oson n n: curren belee sae n: robably of seeng n n see able No acon udae! Howeer, he robo s mong and herefore we can aly a combnaon of acon and erceon udae - s used nsead of - because he oologcal dsance beween n and n can ery deendng on he secfc oologcal ma R. Segwar, I. Nourbahsh 4/3/008 35

36 Auonomous Moble Robos, Chaer Maro Localaon: Case Sudy - oologcal Ma 4 he calculaon s calculaed by mullyng he robably of generang erceual een a oson n by he robably of hang faled o generae erceual eens a all nodes beween n and n. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer Maro Localaon: Case Sudy - oologcal Ma 5 Eamle calculaon Assume ha he robo has wo nonero belef saes o -.0 ; * a ha s facng eas wh cerany Sae -3 wll rogress oenally o 3 and 3-4 o 4. Sae 3 and 3-4 can be elmnaed because he lelhood of deecng an oen door s ero. he lelhood of reachng sae 4 s he roduc of he nal lelhood -3 0., a he lelhood of deecng anyhng a node 3 and he lelhood of deecng a hallway on he lef and a door on he rgh a node 4 and b he lelhood of deecng a hallway on he lef and a door on he rgh a node 4. for smlcy we assume ha he lelhood of deecng nohng a node 3-4 s.0 hs leads o: o 0. [ ] 0.7 [0.9 0.] o Smlar calculaon for rogress from * Noe ha he robables do no sum u o one. For smlcy normalaon was aoded n hs eamle R. Segwar, I. Nourbahsh 4/3/008 36

37 Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 5.6. Fne fed decomoson grd, y, θ, 5 cm 5 cm Acon and erceon udae Acon udae: Sum oer reous ossble osons and moon model Dscree erson of eq. 5. Perceon udae: Gen erceon, wha s he robably o be a locaon l Couresy of W. Burgard R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 5.6. he crcal challenge s he calculaon of l he number of ossble sensor readngs and geomerc cones s eremely large l s comued usng a model of he robo s sensor behaor, s oson l, and he local enronmen merc ma around l. Assumons o Measuremen error can be descrbed by a dsrbuon wh a mean o Non-ero chance for any measuremen Couresy of W. Burgard R. Segwar, I. Nourbahsh 4/3/008 37

38 Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma he D case. Sar No nowledge a sar, hus we hae an unform robably dsrbuon.. Robo ercees frs llar Seeng only one llar, he robably beng a llar, or 3 s equal. 3. Robo moes Acon model enables o esmae he new robably dsrbuon based on he reous one and he moon. 4. Robo ercees second llar Base on all ror nowledge he robably beng a llar becomes domnan R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma Eamle : Offce Buldng Poson 5 Couresy of W. Burgard Poson 3 Poson 4 R. Segwar, I. Nourbahsh 4/3/008 38

39 Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 5 Eamle : Museum Laser scan Couresy of W. Burgard 5.6. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 6 Eamle : Museum Laser scan Couresy of W. Burgard 5.6. R. Segwar, I. Nourbahsh 4/3/008 39

40 Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 7 Eamle : Museum Laser scan 3 Couresy of W. Burgard 5.6. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 8 Eamle : Museum Laser scan 3 Couresy of W. Burgard 5.6. R. Segwar, I. Nourbahsh 4/3/008 40

41 Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma 9 Eamle : Museum Laser scan Couresy of W. Burgard 5.6. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Maro Localaon: Case Sudy Grd Ma Fne fed decomoson grds resul n a huge sae sace Very moran rocessng ower needed Large memory requremen Reducng comley Varous aroached hae been roosed for reducng comley he man goal s o reduce he number of saes ha are udaed n each se Randomed Samlng / Parcle Fler Aromaed belef sae by reresenng only a reresenae subse of all saes ossble locaons E.g udae only 0% of all ossble locaons he samlng rocess s ycally weghed, e.g. u more samles around he local eas n he robably densy funcon Howeer, you hae o ensure some less lely locaons are sll raced, oherwse he robo mgh ge los R. Segwar, I. Nourbahsh 4/3/008 4

42 CSE360/460 Inro o Moble Robocs Parcle Fler Moaon 0 m A B 30 m he robo nows s oson o be eher A or B. I measures he dsance o he wall n fron of o be meers. Whch oson s he robo locaed? CSE360/460 Inro o Moble Robocs Parcle Fler Moaon 0 m A C B 30 m he robo nows s oson o be eher A or B. I measures he dsance o he wall n fron of o be meers. Whch oson s he robo locaed? Wha f here s a hrd ossble oson C? 4/3/008 4

43 CSE360/460 Inro o Moble Robocs Wha s a Parcle Fler? he unform dsrbuon s defned as b a 0 for [ a, b] oherwse a b /b-a Our frend, he Gaussan dsrbuon r e πσ σ Parcle dsrbuons µ µ σ σ CSE360/460 Inro o Moble Robocs Wha s a Parcle Fler? Parcle Flers PF are nown by a arey of names Imorance Samlng, he Merools Algorhm, Mone Carlo Mehods, CONDENSAION algorhm, ec. We wll use he PF moner from sascal leraure PFs reresen a robably densy funcon as a se of dscree samles or arcles, each reresenng a hyohecal esmae of he sae For eamle, o esmae he ose of a robo on he lane each arcle would corresond o a hyohecal oson and orenaon We can hen ae sascs oer hs se as necessary o esmae ose 4/3/008 43

44 CSE360/460 Inro o Moble Robocs Bayesan Flers PFs and Kalman Flers KF/EKF are eamle of Bayesan Flers Bayesan flers do no elcly esmae he sae Insead, hey roagae a oseror robably densy funcon for he sae from whch can be nferred In he KF, a gaussan dsrbuon P s roagaed a each mese wh mean µ and arance σ. he former s used as he sae esmae In he PF, a weghed arcle se corresonds o he oseror from whch an esmae for he sae can be nferred CSE360/460 Inro o Moble Robocs Bayesan Flers Le denoe he sae a me Bayesan flers esmae he condonal df for or he belef denoed by Bel d.. where d 0.. corresonds o all of he aalable daa ha he df has been condoned uon For robo localaon, d 0.. would corresond o sensor measuremens and robo moons. Denong hese as and u, resecely we oban Bel, u,, u,..., 0, u0 where he belef s condoned uon all aalable sensor measuremens and robo acons 0 4/3/008 44

45 4/3/ CSE360/460 Inro o Moble Robocs Bayesan Flers 3 Bayesan flers use redce correce models. hs recurse naure can be obaned by eandng he reous equaon a Bayes rule Snce he denomnaor s no a funcon of and seres o normale he dsrbuon, we can relace nsead wh a normalaon consan η,...,,,,...,,,,...,,,, u u u u u u u u u Bel,...,,,,...,,,, 0 0 u u u u u u Bel η CSE360/460 Inro o Moble Robocs Bayesan Flers 4 Bayes flers han rely uon a Maro assumon whch effecely means ha our belef s no deenden on as sensor daa or robo moons f one nows he curren sae. Mahemacally, hs mles ha Wh hs assumon, our reous equaon reduces o We can furher eand he rgh-hand erm based on he law of oal robably,...,,,, 0 u u u,...,,, 0 u u u Bel η 0 0,...,,,...,, d u u u u Bel η

46 CSE360/460 Inro o Moble Robocs Bayesan Flers 5 hs s followed by agan alyng a Maro assumon o oban Bel η u,, u..., u0 d Bu now he rgh hand erm s merely our belef a he reous me se. hus Bel η u, Bel d or our curren belef s based uon negrang ossble robo moons oer our reous belef, and hen condoned o our curren measuremen. CSE360/460 Inro o Moble Robocs Bayesan Flers 6 Bel η u, Bel d hs corresonds o boh he redcon and correcon hases of a Bayesan fler For he KF for eamle, he negraon s accomlshed n he redce se where we add he coarance mar Q assocaed wh our rocess nose o he ransformed coarance esmae P - o oban our ror belef P - hs s condoned o he new sensor readngs n he measuremen udae hase o oban P, our belef a me 4/3/008 46

47 CSE360/460 Inro o Moble Robocs Mone Carlo Localaon Homewor 5 CSE360/460 Inro o Moble Robocs Predcor-Correcor Eamle We hae a ror of unform weghed arcle Parcles are weghed based on he sensor measuremen and resamled accordng o wegh o generae our oseror. 3 Parcles are assed hrough our moon model o generae a new oseror A hs on, we hae m unque samles We sll hae m samles, bu hey are all equally weghed and no necessarly unque Parcles are agan unque and equally weghed 4/3/008 47

48 CSE360/460 Inro o Moble Robocs he Parcle Fler Unle he KF, whch reresens he df aramercally as a gaussan, he PF aromaes as a samle se Bel {, w } [.. m] m denoes he number of arcles n he samle se corresonds o a hyohecal sae esmae w corresonds o a wegh reflecng a confdence n how well he arcle reflecs he rue sae m w, so ha he samle se corresonds o a dscree robably densy funcon I has been shown ha as he number of samles aroaches nfny, he samle se conerges o he rue oseror [anner, ools for Sascal Inference, 996]. Howeer, no roofs for raes of conergence es CSE360/460 Inro o Moble Robocs Solng he Bayes Fler Equaon Parcle fler flers aromae he soluon o our belef equaon by a numercal negraon oer he arcle se Bel η u, Bel d. Choose a arcle a random from he ror dsrbuon. Projec ahead by generang a new samle from he moon model 3. Reea ses - m mes 4. Rewegh each samle based uon he new sensor measuremen 5. Normale he wegh facors o sum o 4/3/008 48

49 CSE360/460 Inro o Moble Robocs Predcor-Correcor Eamle We hae a ror of unform weghed arcle 3 Acual Robo Poson CSE360/460 Inro o Moble Robocs Predcor-Correcor Eamle 3 a Parcles are weghed based on he sensor measuremen b Parcles are resamled accordng o wegh o generae our oseror. s 0.33 s s w w 0.5, 3 S w 0. w 0. w 0.5 w We sll hae m samles, bu hey are all equally weghed and no necessarly unque /3/008 49

50 CSE360/460 Inro o Moble Robocs Predcor-Correcor Eamle 3 3 Parcles are assed hrough our moon model o generae a new oseror, 3 3 Parcles are agan unque and equally weghed 4 Ierae CSE360/460 Inro o Moble Robocs he Parcle Fler Algorhm funcon X X for : m generae random from X based on samle weghs; generae random ~ u w Inser,w X end 0/ ; Normale wegh facors w X reurn X ; runfler X ;,, u, ; ; 4/3/008 50

51 CSE360/460 Inro o Moble Robocs he Parcle Fler Algorhm er..0 funcon X X for : m generae random from Inser end for all X ~ u end for all X w ; Inser,w X end Normale wegh facors reurn 0; / X ; X X em em em runfler em 0; /, ; ; X X,, u based on samle w X ; weghs; hs erson wll be beer for your Malab mlemenaon on he PF assgnmen! CSE360/460 Inro o Moble Robocs he MCL Problem In he MCL roblem, our objece s o esmae oson and orenaon n a worsace We assume he aalably of a ma m hs allows us o condon our belef o no only he curren ose, bu consraned o le whn a ma. hus, our belef equaon becomes Bel η, m u,, m Bel d hus, we can nfer eeced measuremens from a gen ose hrough: Ray racng f we are dong an occuancy grd Lne nersecon f we are reresenng he ma as a se of lnes We can also combne ma nformaon wh our moon model o elo consrans n he worsace 4/3/008 5

52 CSE360/460 Inro o Moble Robocs Generang he Sensor Model Oeraon of he arcle fler hnges uon assocang a robably wh each sensor measuremen gen a sae so ha a roer wegh can be assocaed wh each samle Bel η, m u,, m Bel d hs s NO he same as samlng he robably densy funcon of For a connuous dsrbuon, he robably of measurng a secfc alue s ero Normally, sensors hae a resoluon whch a gen measuremen s rounded o e.g. a LRF may hae a cm leel resoluon Probables can hen be deermned by negrang he sensor df oer hs resoluon range CSE360/460 Inro o Moble Robocs Generang a MCL Secfc Sensor Model MCL s ycally erformed wh range sensors a nown bearng angles o he robo alhough cameras hae also been used As such, a sngle scan consss of numerous sensor measuremens e.g. from laser or sonar ulses If we assume ha hese n measuremens are ndeenden, he condonal robably can hen be eressed as, m n, m 4/3/008 5

53 CSE360/460 Inro o Moble Robocs MCL Sensor Model Issues A shorcomng of arcle flers s ha hey end o fal f he sensor models are oo accurae hs can resul from a no generang an nal samle close enough o he rue sae esmae One oenal soluon s o nflae he sensor model error. For eamle, he sandard deaon for he SICK LRF s modeled as σ 5cm when n realy s closer cm. hs olaes he bass from whch he PF was dered, bu has bass n acual measuremens and wors well n racce CSE360/460 Inro o Moble Robocs MCL Sensor Model Issues A second ssue usng he LRF s ha for many scans, here wll be no sensor daa aalable hs ycally resuls from wall feaures beng ousde he mamum range of he sensor as aboe, bu can also arse when he laser scan s absorbed, mul-ah error, ec. o address hs, he robably of obanng such a readng s elcly modeled. he weghng of hs s robably s a funcon of he range and he enronmen beng elored 4/3/008 53

54 CSE360/460 Inro o Moble Robocs MCL Sensor Model Issues 3 Recall ha he condonal robably for he sensor measuremen s eressed as he roduc of he nddual robables., m As a consequence, a sngle ouler can cause he robably o aroach ero Such errors can readly be caused by errors n our ma, furnure, ersons/robos mong hroughou he enronmen, ec. hs s handled by nroducng an eonenal based robably densy no he sensor model for unmodeled obsacles n, m Auonomous Moble Robos, Chaer 5 Samle Sensor Models 5.6. Couresy of W. Burgard R. Segwar, I. Nourbahsh 4/3/008 54

55 CSE360/460 Inro o Moble Robocs Smle MCL Eamles CSE360/460 Inro o Moble Robocs When can MCL Fal? MCL reles uon dfference n he enronmen o nduce corresondng dfferences n sensor measuremens Large oen areas, long feaureless corrdors, symmerc enronmens, ec. can cause MCL o be slow o conerge or o conerge o he wrong ose MCL can elo een mnor dfferences o oban a correc ose esmae Inconssen Conergence Conssen Conergence 4/3/008 55

56 CSE360/460 Inro o Moble Robocs Generang a Sae Esmae Snce each arcle has an assocaed sae and wegh, he mean of he dsrbuon can be esmaed usng sandard echnques and sere as our esmae of he sae. hs suffers when here are comeng dsrbuons when he fler has no ye conerged Alernaely, one could use he arcle wh he hghes wegh A drawbac o hs s ha a sngle samle s used o generae he sae esmae A hrd alernae s o dscree he sae sace, fnd he cell wh he hghes oal wegh, and calculae he mean oer hs arcle subse Oher mehods can be magned CSE360/460 Inro o Moble Robocs he Parcle Fler Pros & Cons here are seeral adanages o usng arcle flers Able o model non-lnear sysem dynamcs and sensor models No gaussan nose model assumons hey can use mlc as well as aramerc esmaors In racce, erforms well n he resence of large amouns of nose and assumon olaons e.g. Maro assumon, weghng model Smle mlemenaon Some dsadanages nclude Hgher comuaonal comley comared o he KF Comuaonal comley ncreases eonenally comared wh ncreases n sae dmenson ycally NO used oudoors In some alcaons, he fler s more lely o derge wh more accurae measuremens 4/3/008 56

57 CSE360/460 Inro o Moble Robocs Kalman Fler - Suorng References G. Welch & G. Bsho, An Inroducon o he Kalman Fler P. Maybec, Sochasc Models, Esmaon & Conrol, Chaer aached o W&B uoral CSE360/460 Inro o Moble Robocs Daa Fuson Moaon r α Le s say your robo aes 3 range measuremens of he dsance o a beacon as Z [000, 900, 00] Wha would be your esmae of he beacon dsance? Well, a good esmae mgh be he mean of he 3 sensor alues: r E Z /3/008 57

58 CSE360/460 Inro o Moble Robocs Daa Fuson Moaon r α Now le s say your robo aes 3 measuremens of he dsance o a beacon as Z [000, 900, 300] We could agan use he mean as he range esmae and oban r E Z Would you hae as much confdence n hs esmae as he frs? CSE360/460 Inro o Moble Robocs he Kalman Fler he man dea behnd he Kalman fler s ha you do no jus hae an esmae for a arameer bu also hae some esmae for he uncerany n your alue for hs s reresened by he arance/coarance of he esmae P here are many adanages o hs, as allows you a means for esmang he confdence n your robo s ably o eecue a as e.g. nagang hrough a gh doorway In he case of he KF, also rodes a nce mechansm for omally combnng daa oer me hs omaly condon assumes we hae lnear models, and he error characerscs of our sensors can be modeled as ero-mean, Gaussan nose 4/3/008 58

59 CSE360/460 Inro o Moble Robocs Noaon Reew. Marces are denoed by a caal leer. In e, hey wll be bold e.g. A. Vecors are denoed by a lowercase leer. In e, hey wll be bold e.g.. In Mcrosof Equaons, hey wll hae an oerscore e.g. 3. Scalars are lowercase leers whou emhass 4. - denoes he a ror esmae for he sae ecor a me se before he measuremen udae hase 5. denoes he esmae for he sae ecor a me se afer he measuremen udae hase r CSE360/460 Inro o Moble Robocs he Dscree Kalman Fler he Kalman fler addresses he roblem of esmang he sae R n of a dscree-me conrolled rocess goerned by he lnear dfference equaon r r r r A Bu w and wh a measuremen R m ha s r r r H w and reresen he rocess and measuremen nose. hey are assumed ndeenden, whe, and wh gaussan PDFs w ~ N 0, Q ~ N 0, R NOE: he marces A,B,H,Q & R may be me aryng 4/3/008 59

60 CSE360/460 Inro o Moble Robocs he Predcor-Correcor Aroach In hs eamle, redcon comes from usng nowledge of he ehcle dynamcs o esmae s change n oson he analogy would be negrang nformaon from he ehcle odomery or o esmae changed n oson he correcon s accomlshed hrough mang eerocee obseraons and hen fusng hs wh your curren esmae hs s an o udang oson esmaes usng landmar nformaon, ec. In racce, he redcon rae s ycally much hgher han he correcon CSE360/460 Inro o Moble Robocs he Dscree Kalman Fler A each me se, he KF roagaes boh a sae esmae and an esmae for he error coarance P. he laer rodes an ndcaon of he uncerany assocaed wh he sae esmae As menoned reously, he KF s a redcor-correcor algorhm. Predcon comes n he me udae hase, and correcon n he measuremen udae hase he - suerscr mles a redcon NO nerse! Correc, P me Udae, P Measuremen Udae, P In our case, redcon wll be from he robo nemacs X, Y, α Predc 4/3/008 60

61 CSE360/460 Inro o Moble Robocs he me Udae Phase. Predc he sae ahead r r A r B u. Projec he error coarance ahead P AP A Q CSE360/460 Inro o Moble Robocs he Measuremen Udae Phase. Comue he Kalman Gan K. Udae he esmae based on he new measuremen K H 3. Udae he error coarance P I K H P 4/3/008 6

62 CSE360/460 Inro o Moble Robocs Predcor-Correcor KF Eamle We hae a coarance mar P wh mean. s our ose esmae and he P s he uncerany assocaed wh ha ose esmae. We redc he ne oson from our moon model r r r A B P u AP A Q - Acual Robo Poson CSE360/460 Inro o Moble Robocs Predcor-Correcor KF Eamle 3 We ae a new measuremen n he MU hase and use hs o esmae our new oson and coarance P K P H HP H R K H P I K H P - 4/3/008 6

63 CSE360/460 Inro o Moble Robocs he Kalman Fler Se n he me udae hase s merely our redcon based uon he lnear sae udae equaon ha we hae r r A r B u Se of he me udae hase comes from rojecng our coarance mar forward where we merely add he rocess nose arance Q due o he normal sum dsrbuon roery where σ 3 σ σ N P µ µ N Predc, 0 P 0 me Udae. Projec he sae forward r r r A Bu. Projec he coarance forward P AP A Q Correc Measuremen Udae. Comue Kalman Gan K P H HP H R. Udae sae esmae wh measuremen K H 3. Udae error coarance P I K H P P P P P N N N [ A µ ][ A µ ] AP A Q N N N A µ µ µ µ Q Q A Q CSE360/460 Inro o Moble Robocs he Kalman Fler 3 Predc 0 0, P me Udae. Projec he sae forward r r r A Bu. Projec he coarance forward P AP A Q Measuremen Udae. Comue Kalman Gan K P H HP H R. Udae sae esmae wh measuremen K H 3. Udae error coarance P I K H P Predcon before Measuremen Udae Correc Esmae afer Measuremen Udae Measuremen /3/008 63

64 CSE360/460 Inro o Moble Robocs he Correcon: A Leas-Squares Aroach OK, le s say we hae wo ndeenden sensors and oban wo dfferen measuremens Z [, ] for he range r o a beacon Le us furher assume ha he arance n each of hese sensor measuremens s R and R, resecely Q: How should we fuse hese measuremens n order o oban he bes ossble resulng esmae for r? We ll defne bes from a les-squares ersece We hae measuremens ha are equal o r lus some adde ero-mean Gaussan nose and r N 0, R r r N 0, R r CSE360/460 Inro o Moble Robocs A Leas-Squares Aroach We wan o fuse hese measuremens o oban a new esmae for he range rˆ Usng a weghed leas-squares aroach, he resulng sum of squares error wll be Mnmng hs error wh resec o rˆ yelds r N 0, R r r N 0, R e n w rˆ n n e w ˆ r rˆ rˆ r w rˆ 0 4/3/008 64

65 4/3/ CSE360/460 Inro o Moble Robocs Rearrangng we hae If we choose he wegh o be we oban 0 ˆ n n w w r n n w w r ˆ R w σ ˆ R R R R R R R R R R r A Leas-Squares Aroach CSE360/460 Inro o Moble Robocs hs can be rewren as or f we hn of hs as addng a new measuremen o our curren esmae of he sae we would ge For mergng Gaussan dsrbuons, he udae rule s whch f we wre n our measuremen udae equaon form we ge ˆ R R R r 3 3 σ σ σ σ σ σ σ σ σ σ σ σ P K P R P R P P ˆ ˆ ˆ r R P P r r ˆ ˆ ˆ r K r r Kalman Gan A Leas-Squares Aroach. Comue Kalman Gan. Udae sae esmae wh measuremen 3. Udae error coarance Measuremen Udae R H HP H P K H K P H K I P

66 CSE360/460 Inro o Moble Robocs -D Eamle Esmang a Random Consan Suose we are ryng o esmae he alue of a D consan from corrued sensor measuremens. Our rocess model s hen he KF equaons hen are Varance of our sae esmae P P A Bu w w me Udae H Q Varance of our sgnal leel K P Measuremen Udae P P I K R K P Varance of our measuremen dece CSE360/460 Inro o Moble Robocs Smulaon Resuls, P 0 0 me Udae. Projec he sae forward r r r A Bu. Projec he coarance forward P AP A Q Predc Measuremen Udae. Comue Kalman Gan K P H HP H R. Udae sae esmae wh measuremen K H 3. Udae error coarance P I K H P Correc Le us assume ha * 7.5, Q0.0, R9 Wh erfec nowledge of he rocess and sensor coarance model, we oban 4/3/008 66

67 CSE360/460 Inro o Moble Robocs Smulaon Resuls 0 0, P me Udae. Projec he sae forward r r r A Bu. Projec he coarance forward P AP A Q Predc Measuremen Udae. Comue Kalman Gan K P H HP H R. Udae sae esmae wh measuremen K H 3. Udae error coarance P I K H P Correc Le us assume ha * 7.5, Q0.0, R9 Le us furher assume ha he user belees ha he sensor coarance R 0.09 CSE360/460 Inro o Moble Robocs Smulaon Resuls 3, P 0 0 me Udae. Projec he sae forward r r r A Bu. Projec he coarance forward P AP A Q Predc Measuremen Udae. Comue Kalman Gan K P H HP H R. Udae sae esmae wh measuremen K H 3. Udae error coarance P I K H P Correc Le us assume ha * 7.5, Q0.0, R9 Le us furher assume ha he user belees ha he sensor coarance R 900 4/3/008 67

68 CSE360/460 Inro o Moble Robocs Kalman Flers s. Parcle Flers Comac reresenaon Sngle sae hyohess Elcly model Gaussan PDF for sae / coarance esmaon Scales well comuaonally for hgher dmensonal reresenaons Derge n he dnaed robo roblem Lmed o lnear sysem models Omal KF PF Memory-nense reresenaon n hyoheses for each arcle Imlcly Aromaes any PDF for sae/coarance Lmed o ~3 dmensons on modern comuers Soles he dnaed robo roblem Wors for any sysem model Sub-omal CSE360/460 Inro o Moble Robocs Kalman Fler Localaon Le s say ha we are gong o use a Kalman fler o locale our robo from range measuremen o RF beacons wh nown locaon We could wre our sae udae equaon as y θ y cos θ sn θ θ ω Now le s loo a our measuremen equaons ang range o a beacon a b, y b r b y yb Houson, we hae a roblem 4/3/008 68

69 CSE360/460 Inro o Moble Robocs Kalman Fler Localaon For many alcaons, he me udae and measuremen equaons are NO lnear. As a consequence, he KF s no alcable Howeer, he KF s such a nce algorhm ha maybe f we lneare around he non-lneares, we can sll ge good erformance n racce hs lne of hough lead o he deelomen of he Eended Kalman Fler EKF By relang he lnear assumons, he use of he KF s eended dramacally Lfe Rule: here s no such hng as a free lunch We can no longer use he word omal wh he EKF CSE360/460 Inro o Moble Robocs he Eended Kalman Fler EKF he Eended Kalman EKF s a sub-omal eenson of he orgnal KF algorhm he EKF allows for esmaon of non-lnear rocesses or measuremen relaonshs hs s accomlshed by lnearng he curren mean and coarance esmaes smlar o a frs order aylor seres aromaon Suose our rocess and measuremen equaons are he non-lnear funcons r A Bu H Kalman Fler w r r r r f, u, w h, Eended Kalman Fler 4/3/008 69

70 CSE360/460 Inro o Moble Robocs EKF me Udae Phase For he sae udae equaon, we do no now he nose alues a each me se. So, we aromae he sae and whou hem ˆ r f ˆ, u,0 y θ y cos θ sn θ θ ω Predc 0 0, P me Udae. Projec he sae forward r ˆ f ˆ,,0 u. Projec he coarance forward Measuremen Udae. Comue Kalman Gan. Udae sae esmae wh measuremen 3. Udae error coarance Correc CSE360/460 Inro o Moble Robocs EKF me Udae Phase Howeer when we roagae he coarance ahead n me, he underlyng funcon needs o be lnear n order o roerly combne he Gaussan uncerany n our sae our coarance mar P - wh our rocess uncerany Q Q: How do you hn we could do hs? A: Lnearaon. Agan, our wonderful frend he aylor seres comes o he rescue f f ε f f '... ε f f ε f J... ε 4/3/008 70

71 CSE360/460 Inro o Moble Robocs ransformng Uncerany Le s say we now he uncerany of a arable, and we wan o comue he uncerany of yf We now ha ε where s he dsrbuon mean and ε s ero mean nose We can hen use he Jacoban o lnearly aromae y y f f ε f Jε he mean of he dsrbuon would hen be y E[ y] E[ f Jε ] f herefore y y Jε CSE360/460 Inro o Moble Robocs ransformng Uncerany he coarance of he ransformed dsrbuon would hen be C y E[ y y y y ] E[ Jεε J ] hus, o ransform uncerany across a non-lnear ransformaon, we erform a smlary ransform wh he Jacoban Noe ha because of he symbols on he reous age, normal dsrbuons are NO resered he omaly/robusness of he KF allows he EKF o wor well n racce JC J 4/3/008 7

72 CSE360/460 Inro o Moble Robocs EKF me Udae Phase 3 So he coarance s rojeced ahead as P AP A Q Kalman Fler P AP A WQW Eended Kalman Fler where A s now he Jacoban of f wh resec o and W s he Jacoban of f wh resec o w Predc 0 0, P me Udae. Projec he sae forward r ˆ f ˆ,,0 u. Projec he coarance forward P A P A W QW Measuremen Udae. Comue Kalman Gan. Udae sae esmae wh measuremen 3. Udae error coarance Correc CSE360/460 Inro o Moble Robocs EKF Robo Imlemenaon Eamle Assume ha we hae a moble robo usng odomery and range measuremens o landmar o esmae s oson and orenaon [, y, θ ] Assume ha he odomery rodes a elocy esmae V and an angular elocy esmae ω ha are boh corrued by gaussan nose We can wre he sae udae equaon as y r y θ y θ V cos θ V sn θ ω whch s obously non-lnear n he sae y θ r 4/3/008 7

73 4/3/ CSE360/460 Inro o Moble Robocs We calculae he Jacoban A as We calculae he Jacoban W from he sensor measuremens as EKF Robo Imlemenaon Eamle V V y y f f f f ω θ θ θ θ sn cos cos 0 sn 0 V V f A j θ θ w f W 0 0 sn 0 cos θ θ CSE360/460 Inro o Moble Robocs Agan, n he measuremen udae we can hae a non-lnear relaonsh beween our measuremens and sae and once agan we wll assume ha he nose s ero o roagae uncerany, we shall agan hae o calculae he arorae Jacobans H s he Jacoban relang changes n h o changes n our sae V s he Jacoban relang changes n h o changes n he measuremen nose hese are hen subsued no he orgnal KF as arorae EKF Measuremen Udae Phase, h,0 h b b y y r

74 4/3/ CSE360/460 Inro o Moble Robocs Comung he Kalman Gan: Sae Udae: Coarance Udae: R H HP H P K VRV H HP H P K H K P H K I P,0 h K P H K I P KF EKF NOE: Some deraons wll wre hs as H as well. EKF Measuremen Udae Phase CSE360/460 Inro o Moble Robocs Le s go bac o our beacon eamle and le s assume we hae a sngle beacon. From hs, our range measuremen can be wren as where b and y b are consans he Jacobans H and V can hen be calculaed as Noe ha f hs were he only measuremens aalable hen θ would be unobserable Q: How do we address hs? Kalman Fler Localaon Eamle Resed b b y y h 0 b b b b b b y y y y y y H V

75 CSE360/460 Inro o Moble Robocs he Dscree Eended Kalman Fler Predc 0 0, P me Udae. Projec he sae forward ˆ r f ˆ, u,0. Projec he coarance forward A P P A W Q W Measuremen Udae. Comue Kalman Gan K P H H P H V RV. Udae sae esmae wh measuremen ˆ ˆ ˆ K h,0 3. Udae error coarance P I K H P Correc CSE360/460 Inro o Moble Robocs Issues wh he EKF he EKF aromaes he KF usng lnearaon abou he curren sae esmae hese lnearaons can be arcularly roblemac n esmang robo orenaon Hybrd aroaches ofen use KF based soluons o esmae oson and a arcle fler based for esmang orenaon een able n 3D Lnearaons also end o yeld an oerconfden esmae of he coarance n he sae esmae meanng ha he robo hns s ose esmae s beer han acually s Can address hs o a on by arfcally nflang he Q & R marces In racce, he coarance marces hae o be uned emrcally o ome sysem erformance 4/3/008 75

76 Auonomous Moble Robos, Chaer 5 Smulaneous Localaon and Mang EPFL 5.8. R. Segwar, I. Nourbahsh Auonomous Moble Robos, Chaer 5 Cyclc Enronmens Small local error accumulae o arbrary large global errors! hs s usually rrelean for nagaon Howeer, when closng loos, global error does maer 5.8. Couresy of Sebasan hrun R. Segwar, I. Nourbahsh 4/3/008 76

77 CSE360/460 Inro o Moble Robocs Summary For localaon ndoors, arcle fler aroaches domnae Unle EKF aroaches, hey do no need a good nal esmae for localaon hey are also able o sole he dnaed robo roblem by connuously dsrbung a small number of addonal arcles durng each eraon For localaon off he lane, he EKF s he referred aroach Comuaonally hey scale cubcly n he dmenson of he roblem s. eonenally for he PF Boh arcle flers and EKF are used o aac he smulaneous localaon and mang SLAM roblem Agan PFs can wor well ndoors for hs EKF can also aac hs roblem snce he orgn of he ma can be assgned o he nal robo ose. CSE360/460 Inro o Moble Robocs Summary For ma buldng, EKFs rely on racng secfc feaures n he enronmen. hs requres solng he corresondence roblem machng n ror o runnng he EKF In racce, EKFs can rac feaures n real-me remember On 3 s sll no a nce comley bound hs consrans he se and fdely of he mas ha can be generaed he EKF sll remans he algorhm of choce for SLAM oudoors as you won oban nce lne scans of walls o mach for your PF wo rmary ssues wh mang: Cycles Dynamc enronmena 4/3/008 77