Probabilistic Fundamentals in Robotics

Size: px

Start display at page:

Download "Probabilistic Fundamentals in Robotics"

Kelly Wilcox
5 years ago
Views:

1 Probabilisic Fundamenals in Roboics Probabilisic Models of Mobile Robos Robo localizaion Basilio Bona DAUIN Poliecnico di Torino Course Ouline Basic mahemaical framework Probabilisic models of mobile robos Mobile robo localizaion problem Roboic mapping Probabilisic planning and conrol Reference exbook Thrun, Burgard, Fox, Probabilisic Roboics, MIT Press, 2006 hp:// roboics.org/ Basilio Bona 2 1

2 Mobile robo localizaion problem Mobile robo localizaion: Markov and Gaussian Inroducion Markov localizaion EKF localizaion Muli hypohesis racking UKF localizaion Mobile robo localizaion: Grid and Mone Carlo Grid localizaion Mone Carlo localizaion Localizaion in dynamic environmens Basilio Bona 3 Inroducion Localizaion or posiion esimaion is he problem of deermining he pose of a rover relaive o a given map of he environmen Localizaion is he process of esablishing he correspondence beween eenhe map reference frame and he local rover reference frame Hence coordinae ransformaion is necessary: from robo cenered local coordinae sysem o global one The pose x =(x y q) of he robo is no known and canno be measured direcly wih a sensor The pose mus be inferred from environmen daa (and odomery) A single measuremen is no sufficien o recover he pose Localizaion echniques depend on he map ype The algorihms presened are varians of he basic Bayes filer Basilio Bona 4 2

3 Example: localizaion wih known landmarks Basilio Bona 5 Localizaion as a sae sysem A graphical sae descripion of he localizaion problem The map m, he measuremens z and he conrols u are known Basilio Bona 6 3

Maps A 2D meric map A opological map (graph like) An occupancy

localizaion problems Posiion racking The iniial robo pose is

ofen assumed o be unimodal; he problem is essenially a local

iniial error canno be assumed o be small; global localizaion

he robo ge kidnapped and suddenly reappears in anoher locaion;

4 Maps A 2D meric map A opological map (graph like) An occupancy grid map A mosaic map of a ceiling Basilio Bona 7 A axonomy of localizaion problems Posiion racking The iniial robo pose is known; he iniial error is assumed small; he pose uncerainy is ofen assumed o be unimodal; he problem is essenially a local one Global localizaion The iniial robo pose in unknown; he iniial error canno be assumed o be small; global localizaion includes posiion racking Kidnapped robo problem During operaion he robo ge kidnapped and suddenly reappears in anoher locaion; he robo believes o be able o esimae is posiion, bu his is no rue Basilio Bona 8 4

Saic vs dynamic environmens Saic environmens Environmens where he only moving objec is he robo. All oher objecs (feaures, landmarks, ec.) are fixed wr he map Dynamic environmens Oher objecs move, i.e., change heir locaion over ime.

5 Saic vs dynamic environmens Saic environmens Environmens where he only moving objec is he robo. All oher objecs (feaures, landmarks, ec.) are fixed wr he map Dynamic environmens Oher objecs move, i.e., change heir locaion over ime. Changes usually are no episodic, i.e., hey persis over ime and are recognized by sensor readings. Changes ha affec only a single or few measuremens are reaed as noise. Examples are: people moving around he robo, doors ha open and close, movable furniure, dayligh illuminaion (for robo wih vision sensors) Two approaches: moving objec are included ino he sae Sensor daa are filered o eliminae he effec of unmodeled dynamics Basilio Bona 9 Passive vs acive localizaion Passive localizaion The localizaion module only observes he robo operaing; he localizaion resuls do no conrol he robo moion. Moion is random or deermined by oher ask accomplishmen Acive localizaion The localizaion module conrols he robo moion o maximize a performance crierion on localizaion error Basilio Bona 10 5

6 Single robo vs muli robo Single robo The localizaion is performed by a single robo ha moves (acively or passively) in he environmen. No communicaion issues are presen (excep ha wih he supervisor) Muli robo The localizaion is joinly performed by a eam or robos. In principle each one can perform single robo localizaion, bu if hey can deec each oher, his informaion can be shared and each robo belief can influence oher robos belief. Deecion and communicaion issues are imporan, as well as relaive pose measuremen. Basilio Bona 11 Markov localizaion Probabilisic localizaion algorihms are varians of he Bayes filer The simples localizaion filer (pure Bayesian) is called Markov localizaion Predicion bel( x ) = ò p( x x, u ) bel( x )dx Correcion bel( x ) = h p( z x ) bel( x ) Basilio Bona 12 6

7 Iniial belief Posiion racking bel( x ) de(2 p ) - ì ï ü T - ï = S exp í- ( x -x ) S ( x -x ) ý ï î 2 ï þ Nxx (;, S) 0 Normal disribuion Nxm (;, S) bel( x ) = Global localizaion 0 1 X Volume of he poses space Basilio Bona 13 KF summary x = Ax + Bu + e -1 ( ) px ( x, u) = N x; Ax + Bu, R -1-1 bel( x ) = ò p( x x, u ) bel( x )dx ß ß ( ; +, ) ( ;, S ) ~ N x Ax Bu R ~ N x m Basilio Bona 14 7

8 ò KF summary bel( x ) = p( x x, u ) bel( x ) dx ß ( ; + B u, R ) N ( x ; m, S ) ~ N x Ax B ~ N x, ß ì 1 ü T -1 bel( x ) = h exp ï ( x Ax B u ) R ( x Ax B u ) ï ò í ý ïî 2 ïþ ì 1 T -1 exp ï ü í- ( x -m ) S ( x -m ) ï dx ý - 1 ïî 2 ï ïþ ìï m = Am - + Bu 1 bel( x ) = ï í ï T S = AS A + R ïî -1 ß Basilio Bona 15 KF summary z = C x + d ( ) pz ( x ) = N z ; Cx, Q bel( x ) = h p( z x ) bel( x ) ß ß ( ;, ) N ( x ; m, S ) ~ N z ; C x Q ~ Basilio Bona 16 8

9 KF summary bel( x ) = h p( z x ) bel( x ) ß ß ( ;, Q ) N ( x ; m, S ) ~ N z C x ~ ; ß ì 1 belx = h ï- z -Cx Q z -Cx ïî 2 ì 1 ü T -1 exp ï í- ( x -m ) S ( x -m ) ï ý ïî 2 ïþ T -1 ( ) exp ( ) ( ) í ìï m = m + K ( z -C m ) bel( x ) = ï í wih K =SC ïs = ( I -KC ) S ïî T ü ï ý ïþ T ( C S C + Q ) - 1 Basilio Bona 17 EKF localizaion We assume ha he map is represened by a collecion of feaures (landmark based localizaion) We assume he velociy model and he measuremen model presened before All feaures are uniquely idenifiable (correspondence variables are known) = known landmarks Range and heading measuremens are available pz ( x, mc, ) Door 1 Door 2 Door 3 One dimensional environmen wih hree landmarks (doors) Correspondence is known Basilio Bona 18 9

Iniial belief (uniform) Measuremen model

model Correcion/updae sep Moion model (predicion)

(,, u, z, c, m) m - S 1-1 Predicion æ x x x ö ç ç m m

of g wr locaion = = x 1 m m m - -1, x -1, y -1, q q q

m -1 y y 2: V = = Jacobian of g wr conrol u v w q q ç

2 ç 0 ( a v a w çè + 3 4 ) ø 4: m = gu (, m ) Prediced

10 Iniial belief (uniform) Measuremen model Correcion/updae sep Moion model (predicion) Measuremen model Correcion/updae sep Moion model (predicion) Basilio Bona 19 EKF_known_correspondence_localizaion (,, u, z, c, m) m - S 1-1 Predicion æ x x x ö ç ç m m m -1, x -1, y -1, q gu (, m ) -1 y y y 1: G Jacobian of g wr locaion = = x 1 m m m - -1, x -1, y -1, q q q q ç ç m m m çè -1, x -1, y -1, q ø æ x x ö v w (, ) gu m -1 y y 2: V = = Jacobian of g wr conrol u v w q q ç ç v w è ø æ 2 ö ( a v a w 1 2 ) 0 3: M + Moion noise = 2 ç 0 ( a v a w çè ) ø 4: m = gu (, m ) Prediced mean -1 5: S = GS G T + VMV T Prediced covariance -1 Basilio Bona 20 10

EKF_known_correspondence_localizaion (,, u, z, c, m) Correcion æ 2 2 ö ( m m ) ( m m x, x y, y) (, ) m - S 1-1 1: zˆ = - + - ç m m m m m èçèaan2 - - - y, y x, x, q ø

H S H + Q Prediced measuremen covariance T -1 5: K = S H S Kalman gain 6: m = m + K ( z -zˆ ) Updaed mean 7: S = I -K H S Updaed covariance ( ) Basilio Bona 21 EKF

11 EKF_known_correspondence_localizaion (,, u, z, c, m) Correcion æ 2 2 ö ( m m ) ( m m x, x y, y) (, ) m - S 1-1 1: zˆ = ç m m m m m èçèaan y, y x, x, q ø Prediced measuremen mean 2: æ r r r ö h( m, m) m m m x, y,, q H = = x f f f ç m m m çè x, y,, q ø Jacobian of h wr locaion 3: æ 2 s 0 ö r Q = 2 ç 0 s çè r ø T 4: S = H S H + Q Prediced measuremen covariance T -1 5: K = S H S Kalman gain 6: m = m + K ( z -zˆ ) Updaed mean 7: S = I -K H S Updaed covariance ( ) Basilio Bona 21 EKF predicion sep differen moion noise parameers Small roaion and ranslaion noise High roaion noise High ranslaion noise High roaion and ranslaion noise Basilio Bona 22 11

12 EKF measuremen predicion sep innovaion z - zˆ innovaion Basilio Bona 23 EKF correcion sep measuremen uncerainy uncerainy in robo locaion Predicion uncerainy Basilio Bona 24 12

13 Esimaion Sequence (1) Accurae deecion sensor rajecories esimaed from he moion conrol uncerainy ground ruh rajecories before correced rajecories afer Basilio Bona 25 Esimaion Sequence (2) Less accurae deecion sensor Basilio Bona 26 13

14 Esimaion wih unknown correspondences When landmark correspondence is no cerain (as i is ofen he case) one should adop a sraegy o deermine he ideniy of he landmarks during localizaion The mos simple and popular echnique e is he maximum m likelihood correspondence One deermines he mos likely value of he correspondence variable and use i as he rue one This sraegy is fragile when differen landmarks have equally likely values How o reduce he risk Choose landmarks ha are far apar, so no o be confused Make sure ha he robo pose uncerainy remains small Basilio Bona 27 ML daa associaion The maximum likelihood esimaor (MLE) deermines he correspondence c ha maximizes he likelihood cˆ = arg max p( z c, z, u, m) 1: 1: -1 1: c This expression is condiioned on all prior correspondences ha are reaed as always correc c1: - 1 When he number of landmarks is high, he number of possible correspondence grows oo much and he problem becomes inracable A soluion is o maximize each erm singularly and hen proceed as in cˆ = arg max p( z c, z, u, m) i i 1: 1: -1 1: i c i i» Nz hm c m HS H T + Q i c arg max ( ; (,, ), ) Basilio Bona 28 14

Muli hypohesis racking filer Each color represens a differen hypohesis Basilio Bona 29 Localizaion wih MHTF Curren belief is represened by muliple hypoheses Each hypohesis is racked by a Kalman filer

15 Muli hypohesis racking filer Each color represens a differen hypohesis Basilio Bona 29 Localizaion wih MHTF Curren belief is represened by muliple hypoheses Each hypohesis is racked by a Kalman filer Addiional problems: Daa associaion: Which observaion corresponds o which hypohesis? Hypohesis managemen: When o add/delee hypoheses? Large body of lieraure on arge racking, moion correspondence ec. Basilio Bona 30 15

16 MHT example The blue robo sees he door, bu canno resolve is posiion since also he oher hypohesis (in red) are likely Basilio Bona 31 MHT (1) Hypoheses are exraced from laser range finder (LRF) scans Each hypohesis is given by pose esimae, covariance marix and probabiliy of being he correc one: H = { xˆ, S, PH ( )} i i i i Hypohesis probabiliy is compued using Bayes rule sensor repor Pr ( H) PH ( ) i i PH ( r) = i Pr () Hypoheses wih low probabiliy bbl are dl deleedd New candidaes are exraced from LRF scans C = {, z R} j j j Basilio Bona 32 16

17 MHT (2) Basilio Bona 33 MHT (3) Basilio Bona 34 17

he moion and he measuremen models Insead of compuing Jacobians of hese models i compues Gaussians using

18 MHT: Implemened Sysem (3) Example run # hypoheses P(H bes ) Map and rajecory #hypoheses vs. ime Basilio Bona 35 UKF localizaion The UKF localizaion algorihm uses he unscened ransform o linearize he moion and he measuremen models Insead of compuing Jacobians of hese models i compues Gaussians using sigma poins and ransforms hem hrough he model The landmark associaion (correspondence) is cerain Basilio Bona 36 18

19 UKF Algorihm par a) Sigma poins Basilio Bona 37 UKF Algorihm par b) Cross covariance Basilio Bona 38 19

20 UKF_localizaion (,, u, z, m) æ 2 ö ( a v a w 1 2 ) 0 ç + 2 m - S 1-1 ( ) c = m m + g S m -g S a a a a a a Predicion M = 0 ( a v a w Moion noise ç çè ) ø æ 2 s 0 ö r Q = Measuremen noise 2 ç 0 s çè r ø T T a æ T ö m = m 1 1 ( 00) ( ) ç çè Augmened sae mean ø æs 0 0ö -1 a S = 0 M 0-1 Augmened covariance ç çè 0 0 Q ø c x = g( c) Predicion of sigma poins Sigma poins 2L i x m = åw c m i, Prediced mean i= 0 2L T i x x S = w å c ( c -m i, ) ( c -m i, ) Prediced covariance i= 0 Basilio Bona 39 UKF_localizaion (,, u, z, m) m - S 1-1 Correcion 1 x ( ) Z = h c + c z Measuremen sigma poins 2L i zˆ = åw Z m i, Prediced measuremen mean i= 0 ( Z ˆ)( Z ˆ) 2L T i = å - - c i, i, i= 0 S w z z ( )( zˆ,, ) 2L T xz, i x = åw - - c i i i= 0 S c m Z Pred. measuremen covariance Cross covariance K = S S xz, -1 Kalman gain m = m + K ( z -zˆ ) S = S -KSK T Updaed mean Updaed covariance Basilio Bona 40 20

measuremen covariance Kalman gain Updaed mean Updaed covariance Basilio Bona 41 UKF Predicion Sep 3) High noise in roaion small in ranslaion 1) Small noise in

21 UKF_localizaion (,, u, z, m) m - S 1-1 Correcion 2 zˆ æ 2 2 ö ( m m ) ( m m x, x y, y) ( ) = Prediced measuremen mean ç aan 2 m m, m m m çè y, y x, x, q ø æ r r r ö h( m, m) m m m x, y,, q H = = x f f f Jacobian of h w.r.. locaion ç m m m çè x, y,, q ø æ 2 s 0 ö r Q = 2 ç 0 s çè r ø S = H S H + Q T K m = m + K ( z -zˆ ) S = S H S T -1 ( I K H ) S = - Pred. measuremen covariance Kalman gain Updaed mean Updaed covariance Basilio Bona 41 UKF Predicion Sep 3) High noise in roaion small in ranslaion 1) Small noise in ranslaion and roaion Iniial esimae 2) High noise in ranslaion small in roaion 4) High noise in ranslaion and in roaion Basilio Bona 42 21

22 UKF Observaion Predicion Sep The lef plos show he sigma poins prediced from wo moion updaes along wih he resuling uncerainy ellipses. The rue robo and he observaion are indicaed by he whie circle and he bold line, respecively The righ plos show he resuling li measuremen predicion sigma poins. The whie arrows indicae he innovaions, he differences beween observed and prediced measuremens Basilio Bona 43 UKF Correcion Sep The panels on he lef show he measuremen predicion The panels on he righ he resuling correcions, which updae he mean esimae and reduce he posiion uncerainy ellipses Basilio Bona 44 22

23 EKF Correcion Sep Basilio Bona 45 Robo rajecory according o he moion conrol (dashed lines) and he resuling rue rajecory (solid lines) Landmark deecions are indicaed by hin lines Esimaion Sequence EKF PF UKF Basilio Bona 46 23

Predicion Qualiy EKF predicion UKF predicion Approximaion error due o linearizaion The robo moves on a circle The reference covariances are exraced from an accurae, sample based predicion Basilio

24 Predicion Qualiy EKF predicion UKF predicion Approximaion error due o linearizaion The robo moves on a circle The reference covariances are exraced from an accurae, sample based predicion Basilio Bona 47 Mobile robo localizaion Grid and Mone Carlo mehods These algorihms can process raw sensor measuremen, i.e., here is no need o exrac feaures from measuremens These mehods are non parameric, e.g., hey are no limied o unimodal disribuions ions as wih he EKF localizaion aion mehod They can solve global localizaion problem and kidnapped robo problems (in some cases); he EKF algorihm is no able o solve such problems The firs mehod is called grid localizaion The second mehod is called Mone Carlo localizaion Basilio Bona 48 24

Grid localizaion Grid localizaion uses a hisogram

dimension is a key facor for he performances of

algorihm can be exremely slow When he cell grid

loss ha makes he algorihm no working in some

Correcion/updae sep Moion model (predicion)

25 Grid localizaion Grid localizaion uses a hisogram filer o represen poserior belief The grid dimension is a key facor for he performances of he mehod When he cell grid dimension is small, he algorihm can be exremely slow When he cell grid dimension is large, here can be an informaion loss ha makes he algorihm no working in some cases Basilio Bona 49 Example Measuremen model Correcion/updae sep Moion model (predicion) Measuremen model Correcion/updae sep Moion model (predicion) Basilio Bona 50 25

Topological grid map A opological map is a graph annoaion of he environmen Topological maps assign nodes o

spaial knowledge primarily by opological informaion This informaion is used o consruc a hierarchical

represenaion: Coarse gridding Cells of varying size Resoluion influenced by he srucure of he environmen

26 Topological grid map A opological map is a graph annoaion of he environmen Topological maps assign nodes o paricular places and edges as pahs if direc passage beween pairs of places (end nodes) exis Humans manage spaial knowledge primarily by opological informaion This informaion is used o consruc a hierarchical opological map ha describes he environmen Basilio Bona 51 Topological grid map Topological grid map represenaion: Coarse gridding Cells of varying size Resoluion influenced by he srucure of he environmen (significan places and landmarks as doors, corridors windows, T juncions, dead ends, ac as grid elemens) Grid elemens Basilio Bona 52 26

27 Meric grid map Meric grid represenaion: Finer gridding Cells of uniform size Resoluion no influenced by he srucure of he environmen Usually cell sizes of 15 cm x 15 cm up o 1 m x 1m Moion model affeced by robo velociy and cell size q Basilio Bona 53 Cell size influence Cell size influence he performance of he grid localizaion algorihm Average localizaion error as a funcion of grid cell size, for ulrasound sensors and laser range finders Average CPU ime needed for global localizaion as a funcion of grid resoluion, shown for boh ulrasound sensors and laser range finders Basilio Bona 54 27

28 Example wih meric grids 1 Basilio Bona 55 Example wih meric grids 2 Basilio Bona 56 28

29 Example wih meric grids 3 Basilio Bona 57 Example wih sonar daa 1 Basilio Bona 58 29

30 Example wih sonar daa 2 Basilio Bona 59 Example wih sonar daa 3 Basilio Bona 60 30

31 Example wih sonar daa 4 Basilio Bona 61 Example wih sonar daa 5 Basilio Bona 62 31

32 Mone Carlo localizaion Mone Carlo localizaion (MCL) is a relaively new, ye very popular algorihm I is a versaile mehod, where he belief is represened by a se of paricles (i.e., a paricle filer) I can be used boh for local and for global localizaion problems In he conex of localizaion, he paricles are propagaed according o he moion model They are hen weighed according o he likelihood of he observaions In a re sampling sep, new paricles are drawn wih a probabiliy proporional o he likelihood of he observaion The mehod firs appeared in 70 s, and was re discovered by Kiagawa and Isard & Blake in compuer vision Basilio Bona 63 Paricle filer localizaion (MCL) Paricle filers based localizaion (Mone Carlo Localizaion MCL) uses a se of weighed random samples o approximae he robo pose belief Paricle se size n ( ) [] i ( ˆ [] i» å w d - ) bel p p p i= 1 n wih å i= 1 w [] i = 1 Pose of he paricle Weigh of he paricle Basilio Bona 64 32

Paricle filer localizaion (MCL) Paricle based Represenaion of posiion belief Paricle based approximaion Gaussian approximaion (ellipse: 95% accepance

he robo iniial belief a ime zero) 1. PREDICTION: Generae a new se of paricles given he moion model and he applied conrols 2.

33 Paricle filer localizaion (MCL) Paricle based Represenaion of posiion belief Paricle based approximaion Gaussian approximaion (ellipse: 95% accepance region) Basilio Bona 65 Paricle filer localizaion (MCL) Using paricle filers approximaion, he Bayes Filer can be reformulaed as follows (saring from he robo iniial belief a ime zero) 1. PREDICTION: Generae a new se of paricles given he moion model and he applied conrols 2. UPDATE: Assign o each paricle an imporance weigh according o he sensor measuremens 3. RESAMPLING: Randomly resample paricles in funcion of heir imporance weigh Basilio Bona 66 33

Predicion Predicion Generae a new se of paricles given he moion model and he applied conrols For each paricle: Given he paricle pose a ime sep -1 and he commands, he

prediced paricles Basilio Bona 67 Updae Updae Assign o each paricle an imporance weigh according o he sensor measuremens For each paricle: Compare paricle s predicion of

34 Predicion Predicion Generae a new se of paricles given he moion model and he applied conrols For each paricle: Given he paricle pose a ime sep -1 and he commands, he paricle pose a ime is prediced using he moion model i ( -1 ) [] i [] pˆ = f pˆ, u, w The variable is randomly exraced according o he noise disribuion We obain a se of prediced paricles Basilio Bona 67 Updae Updae Assign o each paricle an imporance weigh according o he sensor measuremens For each paricle: Compare paricle s predicion of measuremens wih acual [ i ] measuremens z vs h( pˆ ) Paricles whose predicions mach he measuremens are given a high weigh In red he paricles wih high weighs Basilio Bona 68 34

35 Resampling Resampling Randomly resample paricles in funcion of heir imporance weigh For each paricle: For n imes draw (wih replacemen) a paricle from G - wih probabiliy bili given by he imporance weighs ih and pu i in he se G Paricles whose predicions mach he measuremens are given a high weigh G The new se provides he paricle based approximaion of he robo pose a ime Basilio Bona 69 Example Measuremen model Correcion/updae sep Moion model (predicion) Measuremen model Correcion/updae sep Moion model (predicion) Basilio Bona 70 35

x m [ m] [ m] 5: c = c + x, w 4: (,, ) 6: endfor 7: for m = 1o M do 8: 9 : 10: endfor draw

36 Example Basilio Bona 71 MC_localizaion (, u, z, m) c - 1 1: c = c = Æ 2: for m = 1o M do = [ m] [ m] 3: x sample_moion_model moion model ( u, x ) -1 [ m] [ m] w = measuremen_model z x m [ m] [ m] 5: c = c + x, w 4: (,, ) 6: endfor 7: for m = 1o M do 8: 9 : 10: endfor draw i wih probabiliy add x 11 : reurn c [] i o c µ w [] i see previous slides Basilio Bona 72 36

37 Mone Carlo Localizaion wihin a sensor infrasrucure Fixed sensors deployed in known posiions in he environmen STEP 1: Acquire odomery Basilio Bona 73 STEP 1: Acquire odomery Filer Predicion Basilio Bona 74 37

38 STEP 1: Acquire odomery Filer Predicion Acquire meas. Basilio Bona 75 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Basilio Bona 76 38

39 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling Basilio Bona 77 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 1: Acquire odomery Basilio Bona 78 39

40 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 1: Acquire odomery Filer Predicion Basilio Bona 79 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Basilio Bona 80 40

41 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Basilio Bona 81 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling Basilio Bona 82 41

42 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 3: Acquire odomery Basilio Bona 83 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 3: Acquire odomery Filer Predicion Basilio Bona 84 42

43 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 3: Acquire odomery Filer Predicion Acquire meas. Basilio Bona 85 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 3: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Basilio Bona 86 43

Weighs Updae Resampling STEP 3: Acquire odomery Filer Predicion Acquire meas.

44 STEP 1: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 2: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling STEP 3: Acquire odomery Filer Predicion Acquire meas. Weighs Updae Resampling Basilio Bona 87 Example for landmark based localizaion Basilio Bona 88 44

Properies of MCL MCL can approximae any disribuion, as i can represen complex muli modal disribuions and blend hem wih Gaussian syle disribuions Increasing he oal number of paricles increases he

45 Properies of MCL MCL can approximae any disribuion, as i can represen complex muli modal disribuions and blend hem wih Gaussian syle disribuions Increasing he oal number of paricles increases he accuracy of he approximaion The number of paricles M is a rade off parameer beween accuracy and necessary compuaional resources Paricles number shall remain large enough o avoid filer divergence The paricles number may remain fixed or change adapively Basilio Bona 89 Adaping he paricle size In he example below, he number of paricles is very high (~ ) o allow an accurae represenaion of he belief during early sages of he algorihm, bu is unnecessarily high in laer sages, when he belief concenraes in smaller regions an adapive sraegy is required Basilio Bona 90 45

46 KLD sampling Kullback Leibler divergence (KLD) sampling is a varian of MCL ha adaps he number of paricles over ime KLD (also known as informaion divergence, informaion gain, relaive enropy)is a measure of he difference beween wo probabiliy disribuions For probabiliy disribuions P and Q of adiscree random variable heir KLD is defined as For disribuions P and Q of a coninuous random variable, he KLD is defined as an inegral Basilio Bona 91 KLD sampling The idea is o se adapively he number of paricles based on a saisical bound on he sample based approximaion qualiy A each ieraion of he PF, KLD sampling deermines he number of samples such ha, wih probabiliy 1- d, he error beween he rue poserior and he sample based approximaion is less han e To derive his bound, we assume ha he rue poserior is given by a discree, piecewise consan disribuion such as a discree densiy ree or a muli dimensional hisogram D. Fox, Adaping he sample size in paricle filers hrough KLDsampling, Inernaional Journal of Roboics Research, 2003 Basilio Bona 92 46

47 KLD sampling Given a discree disribuion wih k bins, and given he number n of samples o be chosen, we deermine n as follows 1 2 n = ck -1,1-d 2e 2 where c is he chi square disribuion wih k -1 d.o.f. and k-1,1-d probabiliy 1 - d we can guaranee ha wih probabiliy 1- dhe KL disance beween he MLE and he rue disribuion is less han e Basilio Bona 93 Approximaion formula n k -1 ì 2 2 ü ï 1 z ï í d ý 2e ï 9( k -1) 9( k -1) î ïþ 3 where z is he upper 1 - d quanile of he nomal N(0,1) disribuion 1-d Basilio Bona 94 47

48 Dynamic environmen Markov assumpions are good for a saic environmen Ofenhe he environmen where robos operae is full of people moving here and here People dynamics is no modeled by he sae x Probabilisic approaches are robus since hey incorporae sensor noise, bu... Sensor noise mus be independen a each ime sep... while people dynamics is highly dependen in ime Basilio Bona 95 Dynamic environmen: which soluion Sae Augmenaion include he hidden sae ino he sae esimaed by he filer i is more general, bu suffer from high compuaional complexiy: he pose of each subjec moving around he robo mus be esimaed, and he number of saes varies in ime Oulier rejecion pre process sensor measuremens o eliminae measuremens affeced by hidden sae i may work well when he people presence affecs he sensors (laser range finder, and, o lesser exen, vision sensors) reading Basilio Bona 96 48

49 Oulier rejecion The Expecaion maximizaion (EM) learning algorihm is used for oulier deecion and rejecion I comes from he beam model of range finders, where z shor and p shor parameers relaes o unexpeced objecs k we have o inroduce and compue a correspondence variable c ha can ake one of four values {hi, shor, max, rand} he desired probabiliy is compued as his inegral does no have a closed form soluion ò k p ( z x, m) z bel( x )dx k k shor shor pc ( = shor z, z, u, m) = 1: -1 1: k ò å p ( z x, m) z bel( x )dx c c c approximaion wih a represenaive sample of he poserior he measuremen is rejeced if he probabiliy exceeds a given hreshold Basilio Bona 97 Comparison of differen ML implemenaions EKF MHT Topological grid Meric grid MCL Measuremens landmarks landmarks landmarks Measuremen noise raw measuremens raw measuremens Gaussian Gaussian any any any Poserior Gaussian mixure of Gaussians hisogram hisogram paricles Efficiency (mem) Efficiency (ime) Ease of implemenaion i Resoluion Robusness Global localizaion no no yes yes yes Basilio Bona 98 49

50 Example Basilio Bona 99 Example Basilio Bona

51 Example Basilio Bona 101 Thank you. Any quesion? Basilio Bona

Probabilistic Robotics

Probabilistic Robotics Probabilisic Roboics Bayes Filer Implemenaions Gaussian filers Bayes Filer Reminder Predicion bel p u bel d Correcion bel η p z bel Gaussians : ~ π e p N p - Univariae / / : ~ μ μ μ e p Ν p d π Mulivariae