Uncertainty & Localization I

Transcription

1 Advanced Roboics Uncerain & Localiaion I

2 Moivaion Inrodcion basics represening ncerain Gassian Filers Kalman Filer eended Kalman Filer nscened Kalman Filer Agenda Localiaion Eample For Legged Leage Non-arameric Filers paricle filer Laser-Based Localiaion in ROS

3 robabilisic bili Roboics b Sebasian Thrn Wolfram Brgard and Dieer Fo MIT ress Lierare 3

4 Moivaion self-localiaion localiaion he mos fndamenal problem o providing a mobile robo wih aonomos capabiliies [Co 99] Localiaion i is Sae Esimaion i 4

5 RoboCp For-Legged Leage 5

6 RoboCp For-Legged Leage 6

7 Sae Esimaion roblem sae of a ssem which h can no be observed direcl observaions of pariclar aspecs of he ssem conrol acions which inflence he ssem ncerain in observaions and acions roboics: ncerain sensing e.g. wrong esimaions sensor noise and acing e.g. slip join clearance wha is he crren sae of a ssem if we know all pas conrol acions and observaions? 7

8 The Basis Baes Rle 8

9 Toal robabili and Baes Condiioned d d d d d d 9

10 Baes Filers: Framework given: sream of observaions and acion daa : d { } waned: sensor model acion model prior probabili of he ssem sae esimae of he sae of a dnamical ssem he poserior of he sae is also called belief: Bel 0

11 Baes Filers = observaion = acion = sae Bel Baes Markov d Toal prob. Recrsive Filer Markov d Markov d d Bel

12 Baes Filer Algorihm Bel Bel d Algorihm Baes_filer Beld : 0 if d is a percepal daa iem hen for all do for all do Bel ' Bel Bel' Bel' Bel' else if d is an acion daa iem hen for all do Bel' ' Bel ' d' rern Bel

13 Baes Filers are a Famil! Bel Bel d Kalman filers aricle filers Hidden Markov models Dnamic Baesian neworks ariall Observable Markov Decision rocesses OMDs 3

14 Represening Uncerain processes and measremens are nois and someimes ambigos represening he ncerain necessar for sae esimaion represening he probabili densi fncion of saes acion and measremens closed form represenaions grid-based represenaions sample-based represenaions 4

15 p ~ N : Closed Form - Gassians p e Univariae - p ~ Ν μ Σ : p d / Σ / e μ Σ μ Mlivariae 5

16 Grid-Based 6

17 iecewise Consan Represenaion Bel 7

18 Sample-Based Sample: <vale weigh> 8

19 Smmar Represenaions Closed Form Grid-Based Sample-Based Represenaion Formla Grid Samples Uncerain Gassian Arbirar Arbirar Ssems Linear Arbirar Arbirar Compaion Ver Efficien Ver High Demand High Demand Memor Ver Low Ver High High Demand 9

20 Discree Kalman Filer Esimaes he sae of a discree-ime ime conrolled process ha is governed b he linear sochasic difference eqaion A B wih a measremen C 0

21 D Robo Localiaion Eample

22 Kalman Filer Updaes redicion Measremen Updae

23 Measremen Updae in D ih K C K bl wih obs K K bel wih T T Q C C C K K C I C K bel Innovaion Kalman Gain 3

24 rocess Updaes in D b a bel B A ac a bel T R A A B A bel 4

25 Linear Gassian Ssems: Iniialiaion iniial belief is normall disribed: bel N ;

26 Linear Gassian Ssems: Dnamics dnamics are linear fncion of sae and conrol pls addiive noise: A B N ; A B R p bel p bel d ; A B R ~ N ; ~ N ; 6

27 Linear Gassian Ssems: Dnamics d bel p bel N R B A N p N R B A N ; ~ ; ~ T B A R B A bel ep T d ep T R A A B A bel 7 R A A

28 Linear Gassian Ssems: Observaions observaions are linear fncion of sae pls addiive noise: C N ; C Q p bel p bel ; C Q ~ N ; ~ N Q 8

29 Linear Gassian Ssems: Observaions bel p bel ; ~ ; ~ N Q C N ep ep T T C Q C bel ep ep C Q C bel wih T T Q C C C K K C I C K bel 9

30 Kalman Filer Algorihm Kalman Filer Algorihm Algorihm Kalman_filer - - : redicion: A B Correcion: A B T R A A Correcion: T T Q C C C K C K Rern C K I Rern 30

31 The redicion-correcion-ccle K b a redicion T T Q C C C K C K I C K bel obs K K K bel T R A A B A bel ac a b a bel Correcion 3

32 Kalman Filer Smmar highl efficien: polnomial in measremen dimensionali k and sae dimensionali n: Ok n closed form represenaion opimal for linear ssems wih Gassian noise! works qie good for oher cases bad news: mos roboics ssems are nonlinear! 3

33 Eample Odoor Localiaion Compass GS Seering Commands Where am I? 33

34 Eended Kalman Filer 34

35 Nonlinear Dnamic Ssems Mos realisic roboic problems involve nonlinear fncions g h 35

36 Lineari Assmpion Revisied 36

37 Non-linear Fncion 37

38 EKF Lineariaion 38

39 EKF Lineariaion 39

40 EKF Lineariaion 3 40

41 EKF Lineariaion: Firs Order Talor Series EKF Lineariaion: Firs Order Talor Series Epansion redicion: g g g G g g g g Correcion: Correcion: h h h Jacobian Mari H h h Mari 4

42 EKF Algorihm EKF Algorihm Eended Kalman filer : Eended_Kalman_filer - - : redicion: C i g T R G G A B T R A A Correcion: T T Q H H H K h K T T Q C C C K C K h K H K I C K C K I Rern g G h H 4

43 EKF Smmar highl efficien: polnomial in measremen dimensionali k and sae dimensionali n: Ok n no opimal anmore! can diverge if nonlineariies are large! works srprisingl well even when all assmpions are violaed! drawback: one have o know he Jacobians derivaion 43

44 Localiaion given informaion abo he environmen e.g. meric map landmarks seqence of sensor measremens waned esimae of he robo s posiion w.r.. environmen problem classes posiion racking global localiaion kidnapped robo problem recover 44

45 Landmark-based Localiaion Y Θ ω v 3 X Sae: =[XYΘ] Observaion: i =[r i φ i s i ] Conrol: =[vω] 45

46 Robo Moion robo moion is inherenl ncerain. how can we model his ncerain? real pah inegraed odomer 46

47 Reasons for Moion Errors ideal case differen wheel diameers bmp carpe and man more 47

48 robabilisic Moion Models o implemen he Baes Filer we need he ransiion model p. he erm p specifies a poserior probabili ha acion carries he robo from o. now we will specif how p can be modeled based on he moion eqaions. 48

49 Veloci Model Differenial Drive 49

50 Eqaion for he Veloci Model Cener of circle: Cener of circle: sin v cos v c c v r Moion Model: v v sin sin v v cos cos sin sin cos cos 50

51 Eamples Veloci Model small errors larger ranslaional errors larger roaional errors 5

52 Noise Model for Veloci The measred moion is given b he re moion corrped wih noise. ˆ v v v ˆ 3 v 4 5

53 Noise Veloci Model M v 0 3v are parameer of he robo 53

54 Odomer Model robo moves from o ' ' '. odomer informaion. ro ro rans rans ' ' aan ' ' ro ro ' ro rans ro ' ' ' ro 54

55 Eqaion for he Odomer Model Moion Model: rans cos ro rans sin ro ro ro 55

56 Noise Model for Odomer The measred moion is given b he re moion corrped wih noise. ˆ ˆ ˆ ro rans ro ro rans rans ro ro ro 3 rans 4 ro ro rans 56

57 Noise Odomer Model M ro 0 0 rans 3 rans ro ro ro 0 0 rans 4 are parameer of he robo 57

58 EKF_localiaion - - m: redicion: ' ' ' ' ' ' g J bi f l i ' ' ' g G Jacobian of g w.r. locaion ' ' v v g V ' ' ' ' Jacobian of g w.r. conrol v moion model 58

59 M v 0 0 v Conrol noise 3 4 g T G G V M V T rediced mean rediced covariance process noise conrol noise 59

60 EKF_localiaion - - m: Correcion: epeced disance o landmark h Correcion: m m di d p aan ˆ m m m m rediced measremen mean epeced bearing o landmark r r r m h H Jacobian of h w.r. locaion 0 0 r Q 0 measremen fncion measremen noise 60 measremen noise

61 measremen process measremen noise S K H H T H S T Q K ˆ I K H red. measremen covariance Kalman gain Updaed mean Updaed covariance innovaion 6

62 EKF redicion Sep sm mall moion noise la rge ransla ional noise old ncerain conrol ncerain process ncerain new ncerain larg ge roaiona l noise larg ge moion noise 6

63 EKF Observaion redicion Sep sm mall moion noise large moion noise measremen noise predicion ncerain combined ncerain 63

64 EKF Correcion Sep Innovaion small moion noise large moion noise 64

65 Esimaion Seqence observaions commanded pah ncerain afer measremen re pah ncerain before measremen small measremen noise 65

66 Esimaion Seqence larger measremen noise 66

67 Thank o! 67