AUTONOMOUS SYSTEMS. Probabilistic Robotics Basics Kalman Filters Particle Filters. Sebastian Thrun

Transcription

1 AUTONOMOUS SYSTEMS robabilisic Roboics Basics Kalman Filers aricle Filers Sebasian Thrun slides based on maerial from hp://robos.sanford.edu/probabilisic-roboics/pp/ Revisions and Add-Ins by edro U. Lima and Rodrigo Venura DEEC/Insiuo Superior Técnico Sepember 204 MEEC/MEAero Course Handous Auonomous Sysems robabilisic Roboics

2 robabilisic Roboics Key idea: Eplici represenaion of uncerainy using he calculus of probabiliy heory ercepion sae esimaion Acion uiliy opimiaion Auonomous Sysems robabilisic Roboics

3 Aioms of robabiliy Theory ra denoes probabiliy ha proposiion A is rue. 0 r A r True r False 0 r A B r A + r B r A B Auonomous Sysems robabilisic Roboics

4 A Closer Look a Aiom 3 r A B r A + r B r A B True A A B B B Auonomous Sysems robabilisic Roboics

5 Auonomous Sysems robabilisic Roboics Using he Aioms r r 0 r r r r r r r r r r A A A A False A A True A A A A A A + + +

6 Discree Random Variables X denoes a random variable. X can ake on a counable number of values in { 2 n }. X i or i is he probabiliy ha he random variable X akes on value i.. is called probabiliy mass funcion. E.g. Room Auonomous Sysems robabilisic Roboics

7 Coninuous Random Variables X akes on values in he coninuum. px or p is a probabiliy densiy funcion. r a b p d b a e.g. p Auonomous Sysems robabilisic Roboics

8 X and Yy y Join and Condiional robabiliy If X and Y are independen hen y y y is he probabiliy of given y y y / y y y y If X and Y are independen hen y Auonomous Sysems robabilisic Roboics

9 Law of Toal robabiliy Marginals Discree case Coninuous case y y y y y p d p p y dy p p y p y dy Auonomous Sysems robabilisic Roboics

10 Coninuous random variables Le p be a probabiliy densiy funcion of a coninuous random variable X - noe ha p>0 bu i is no necessarily rue ha p Normaliaion Relaion of densiy wih probabiliy Auonomous Sysems robabilisic Roboics

11 Epecaion Epeced value of a random variable X Epeced value of a funcion of a random variable X if X is a random variable so is a funcion of i Auonomous Sysems robabilisic Roboics

12 Variance and Covariance Variance of a random variable Le X and Y be wo random variables Covariance beween X and Y - Noe ha if X and Y are independen hen CovXY0 Auonomous Sysems robabilisic Roboics

13 Normal random variable Normal or Gaussian random variable probabiliy densiy funcion Nµ σ 2 parameers: µmean σ 2 variance Sandard normal disribuion is a ero mean uni variance normal N0 Auonomous Sysems robabilisic Roboics

14 Mulivariae random variables A mulivariae random variable is a vecor of random variables Epeced value Auonomous Sysems robabilisic Roboics

15 Mulivariae random variables Covariance Auonomous Sysems robabilisic Roboics

16 Mulivariae random variables Componens of a covariance mari Useful mari form If all componens are independen hen covariance mari is diagonal Auonomous Sysems robabilisic Roboics

17 Mulivariae normal random variable A mulivariae normal or gaussian random variable is parameried by is mean vecor µ and covariance mari Σ and is denoed Nµ Σ Auonomous Sysems robabilisic Roboics

18 Mulivariae normal random variable Auonomous Sysems robabilisic Roboics

19 Mulivariae normal random variable Le X be an arbirary mulivariae normal random variable Consider epansion of Σ in eigenvalues and eigenvecors specral decomp. where U is orhonormal and Λ is a diagonal of eigenvalues non-negaive Le Y be a sandard mulivariae normal N0I hen Auonomous Sysems robabilisic Roboics

20 Mulivariae normal random variable In 2D noe ha U can always be pu in a form of a roaion mari Therefore he ranformaion boils down o:. scaling each componen of y by a posiive eigenvalue 2. roaing he disribuion by an angle θ such ha URθ In oher words: - eigenvalues of Σ are he variances along he principal aes - eigenvecors of Σ are he aes along which he principal aes are aligned Auonomous Sysems robabilisic Roboics

21 Auonomous Sysems robabilisic Roboics Bayes Formula evidence prior likelihood y y y y y y y

22 y Algorihm: y y η y Normaliaion η y y ' ' ' : au y y η au y : y η au y Auonomous Sysems robabilisic Roboics

23 Auonomous Sysems robabilisic Roboics Condiioning Law of oal probabiliy for wo condiioning r.v.s: d y y y d d

24 Bayes Rule wih Background Knowledge y y y Auonomous Sysems robabilisic Roboics

25 Condiional Independence y y equivalen o and y y y Auonomous Sysems robabilisic Roboics

26 Simple Eample of Sae Esimaion Suppose a robo obains measuremen Wha is open? Auonomous Sysems robabilisic Roboics

27 Causal vs. Diagnosic Reasoning open is diagnosic. open is causal. Ofen causal knowledge is easier o obain. Bayes rule allows us o use causal knowledge: coun frequencies! open open open Auonomous Sysems robabilisic Roboics

28 Eample open 0.6 open 0.3 open open 0.5 open open open open p open + open p open open raises he probabiliy ha he door is open. Auonomous Sysems robabilisic Roboics

29 Combining Evidence Suppose our robo obains anoher observaion 2. How can we inegrae his new informaion? More generally how can we esimae... n? Auonomous Sysems robabilisic Roboics

30 Auonomous Sysems robabilisic Roboics Recursive Bayesian Updaing n n n n n n Markov assumpion: n is independen of... n- if we know n i i n n n n n n n n η η

31 Auonomous Sysems robabilisic Roboics Eample: Second Measuremen 2 open open 0.6 open 2/ open open open open open open open 2 lowers he probabiliy ha he door is open.

32 Acions Ofen he world is dynamic since acions carried ou by he robo acions carried ou by oher agens or jus he ime passing by change he world. How can we incorporae such acions? Auonomous Sysems robabilisic Roboics

33 Typical Acions The robo urns is wheels o move The robo uses is manipulaor o grasp an objec lans grow over ime Acions are never carried ou wih absolue cerainy. In conras o measuremens acions generally increase he uncerainy. Auonomous Sysems robabilisic Roboics

34 Modeling Acions To incorporae he oucome of an acion u ino he curren belief we use he condiional pdf u This erm specifies he pdf ha eecuing u changes he sae from o. Auonomous Sysems robabilisic Roboics

35 Eample: Closing he door Auonomous Sysems robabilisic Roboics

36 Sae Transiions u for u close door : open closed If he door is open he acion close door succeeds in 90% of all cases. 0 Auonomous Sysems robabilisic Roboics

37 Inegraing he Oucome of Acions Coninuous case: ' u ' ud Discree case: ' u ' u Auonomous Sysems robabilisic Roboics

38 Auonomous Sysems closed u open u Eample: The Resuling Belief closed u closed uopenopen + closed uclosedclosed open u open uopenopen + open uclosedclosed closed u robabilisic Roboics

39 Bayes Filers: Framework Given: Sream of observaions and acion daa u: Sensor model. Acion model u. rior probabiliy of he sysem sae. Waned: d { u u Esimae of he sae X of a dynamical sysem. The poserior of he sae is also called Belief: Bel u } u Auonomous Sysems robabilisic Roboics

40 Markov Assumpion p 0 : : u : p p : : u: p u Underlying Assumpions Saic world Independen noise erfec model no approimaion errors Auonomous Sysems robabilisic Roboics

41 Auonomous Sysems robabilisic Roboics d Bel u η Bayes Filers u u u u η Bayes u u Bel Markov u u η Markov d u u u η d u u u u η Toal prob. Markov d u u η

42 Bayes Filer Algorihm. Algorihm Bayes_filer Beld : 2. η0 3. If d is a percepual daa iem hen 4. For all do For all do Else if d is an acion daa iem u hen 0. For all do. 2. Reurn Bel Bel Auonomous Sysems Bel ' Bel η η + Bel' Bel' η Bel' Bel' u ' Bel' d' η u Bel d robabilisic Roboics

43 Bayes Filers are Familiar! Bel η u Bel d Kalman filers aricle filers Hidden Markov models Dynamic Bayesian neworks arially Observable Markov Decision rocesses OMDs Auonomous Sysems robabilisic Roboics

44 Summary Bayes Filers Bayes rule allows us o compue probabiliies ha are hard o assess oherwise. Under he Markov assumpion recursive Bayesian updaing can be used o efficienly combine evidence. Bayes filers are a probabilisic ool for esimaing he sae of dynamic sysems. Auonomous Sysems robabilisic Roboics

45 Discree Kalman Filer Esimaes he sae of a discree-ime conrolled process ha is governed by he linear sochasic difference equaion A + B u + ε wih a measuremen C + δ Auonomous Sysems robabilisic Roboics

46 Componens of a Kalman Filer A Mari nn ha describes how he sae evolves from o - wihou conrols or noise. B Mari nl ha describes how he conrol u changes he sae from o -. C ε δ Mari kn ha describes how o map he sae o an observaion. Random variables represening he process and measuremen noise ha are assumed o be independen and normally disribued wih covariance R and Q respecively. Auonomous Sysems robabilisic Roboics

47 Kalman Filer Updaes in D Auonomous Sysems robabilisic Roboics

48 Auonomous Sysems robabilisic Roboics Kalman Filer Updaes in D wih + Σ Σ Σ Σ + T T Q C C C K K C I C K bel µ µ µ wih obs K K K bel σ σ σ σ σ µ µ µ + +

49 Kalman Filer redicion D bel bel µ a µ + b u σ 2 a 2 σ 2 2 +σ ac µ A µ Σ A Σ + A T B u + R Auonomous Sysems robabilisic Roboics

50 Kalman Filer Updaes Auonomous Sysems robabilisic Roboics

51 Linear Gaussian Sysems: Iniialiaion Iniial belief is normally disribued: bel Σ N ; µ Auonomous Sysems robabilisic Roboics

52 Auonomous Sysems robabilisic Roboics Linear Gaussian Sysems: Dynamics Dynamics are linear funcion of sae and conrol plus addiive noise: u B A ε + + R B u A N u p ; + ; ~ ; ~ Σ + N R B u A N d bel u p bel µ

53 Auonomous Sysems robabilisic Roboics Linear Gaussian Sysems: Dynamics + Σ Σ + Σ Σ + T T T R A A B u A bel d B u A R B u A bel N R B u A N d bel u p bel 2 ep 2 ep ; ~ ; ~ µ µ µ µ η µ

54 Auonomous Sysems robabilisic Roboics Linear Gaussian Sysems: Observaions Observaions are linear funcion of sae plus addiive noise: C δ + Q C N p ; N Q C N bel p bel Σ ; ~ ; ~ µ η

55 Auonomous Sysems robabilisic Roboics Linear Gaussian Sysems: Observaions wih 2 ep 2 ep ; ~ ; ~ + Σ Σ Σ Σ + Σ Σ T T T T Q C C C K K C I C K bel C Q C bel N Q C N bel p bel µ µ µ µ µ η µ η

56 Kalman Filer Algorihm. Algorihm Kalman_filer µ - Σ - u : 2. redicion: 3. µ A µ + Bu T 4. Σ A Σ A + R 5. Correcion/Updae: T T 6. K ΣC C ΣC + Q 7. µ µ + K C µ 8. Σ I K C Σ 9. Reurn µ Σ Auonomous Sysems robabilisic Roboics

57 EKF Localiaion Eended Kalman Filer localiaion is a paricular case of Markov Localiaion Auonomous Sysems robabilisic Roboics 57

58 The redicion-correcion-cycle redicion bel µ a µ + b u σ 2 a 2 σ 2 2 +σ ac µ A µ + Bu bel T Σ A Σ A + R Auonomous Sysems robabilisic Roboics

59 Auonomous Sysems robabilisic Roboics The redicion-correcion-cycle + Σ Σ Σ Σ + T T Q C C C K K C I C K bel µ µ µ obs K K K bel σ σ σ σ σ µ µ µ + + Correcion

60 Auonomous Sysems robabilisic Roboics The redicion-correcion-cycle + Σ Σ Σ Σ + T T Q C C C K K C I C K bel µ µ µ obs K K K bel σ σ σ σ σ µ µ µ Σ Σ + T R A A B u A bel µ µ bel µ a µ + b u σ 2 a 2 σ 2 +σ ac 2 Correcion redicion

61 Lineariy Assumpion Revisied Auonomous Sysems robabilisic Roboics 6

62 Non-linear Funcion Auonomous Sysems robabilisic Roboics 62

63 EKF Lineariaion dependence on uncerainy Auonomous Sysems robabilisic Roboics 63

64 EKF Lineariaion 2 dependence on uncerainy Auonomous Sysems robabilisic Roboics 64

65 EKF Lineariaion 3 dependence on uncerainy Auonomous Sysems robabilisic Roboics 65

66 EKF and nonlinear dynamical sysems Eample: polar coordinaes y r cos θ r sin θ wih f [ cos θ ] r sin θ sin θ r cos θ wih r and θ are boh Gaussian r 2cm 5 y bu y are no! Auonomous Sysems True mean: EKF mean: o robabilisic Roboics 66

67 Unscened ransform Acual sampling Linearied EKF UT covariance sigma poins mean weighed sample mean and covariance rue mean ransformed sigma poins rue covariance UT mean UT covariance Auonomous Sysems Figure : Eample of he UT for mean and covariance robabilisic Roboics prop- 67

68 Unscened ransform Nonlinear Transformaion X 0 W 0 κ/n + κ n X i + + κ W i /2n + κ n i + κ W i+n /2n + κ X i+n S. J. Julier J. K. Uhlmann A New Eension of he Kalman Filer o Nonlinear Sysems In. Symp. Aerospace/Defense Sensing Simul. and Conrols 997. i Y i f [X i ] ȳ yy 2n i0 W i Y i. 2n i0 W i {Y i ȳ} {Y i ȳ} T Auonomous Sysems robabilisic Roboics 68

69 Unscened ransform Eample: polar coordinaes y r cos θ r sin θ wih f [ cos θ ] r sin θ sin θ r cos θ wih r 2cm True mean: EKF mean: o Kappa mean: + Auonomous Sysems robabilisic Roboics 69

70 Calculae sigma poins: For In he esimaion problem he noisy-ime series is h only observed inpu o eiher he EKF or UKF algorihm Calculae sigma he poins: boh uilie known neural nework model. Noe h for his sae-space formulaion boh he EKF and UKF ar order compleiy. Figure 2 shows a sub-segmen of h esimaes by boh he EKF and he UKF he orig a.k.a. predicion Time updae:generaed inal noisy ime-series has a 3dB SNR. The superior perfo mance of he UKF is clearly visible. Ne whie Gaussian noise was added o he clean MackeyGlass series o generae ime-series. Esimaionaofnoisy MackeyGlass ime series : EKF 5 The corresponding sae-space represenaion is given by: clean Unscened Kalman Filer UKF Time updae: Iniialie wih: k noisy EKF 0... Measuremen updae equaions: k For 20 clean noisy UKF In he esimaion problem he noisy-ime series is he only 0observed inpu o eiher he EKF or UKF algorihms boh uilie he known neural nework model. Noe ha for his sae-space formulaion boh he EKF and UKF are order compleiy. Figure 2 shows a sub-segmen of he by and 260he esimaes generaed boh 240 he EKF UKF he origk inal noisy ime-series has a 3dB SNR. The superior perforesimaion Error : EKF vs UKF on MackeyGlass mance of he UKF is clearly visible. EKF where composie scaling parameer dimension of augmened sae process noise cov. measuremen noise cov. weighs as calculaed in Eqn. 5. Auonomous Sysems Algorihm 3.: Unscened Kalman Filer UKF equaions UKF 0.8 Esimaion of MackeyGlass ime series : EKF where clean noisysae composie scaling parameer dimension of augmened 0.4 EKF process noise cov. measuremen noise cov. weighs 70 robabilisic Roboics 0.2 as calculaed in Eqn. 5. k Time updae: normalied MSE k Calculae sigma poins: Esimaion of MackeyGlass ime series : UKF a.k.a. updae Measuremen updae equaions: 5

71 Kalman Filer Summary Highly efficien: olynomial in measuremen dimensionaliy k and sae dimensionaliy n: Ok n 2 Opimal for linear Gaussian sysems! Mos roboics sysems are nonlinear! Auonomous Sysems robabilisic Roboics

72 aricle Filers Non-parameric filer Represen belief by random samples Esimaion of non-gaussian nonlinear processes Mone Carlo filer Survival of he fies Condensaion Boosrap filer aricle filer Filering: [Rubin 88] [Gordon e al. 93] [Kiagawa 96] Compuer vision: [Isard and Blake 96 98] Dynamic Bayesian Neworks: [Kanaawa e al. 95] Auonomous Sysems robabilisic Roboics

73 Imporance Sampling Targe disribuion roposal disribuion Bel Bel Weigh samples: w f / g Auonomous Sysems robabilisic Roboics

74 aricle Filers p Auonomous Sysems robabilisic Roboics

75 Sensor Informaion: Imporance Sampling Bel α p Bel w α p Bel Bel α p p p p Auonomous Sysems robabilisic Roboics

76 Robo Moion Bel p' u Bel d p p u Auonomous Sysems robabilisic Roboics

77 Sensor Informaion: Imporance Sampling Bel α p Bel w α p Bel Bel α p p p p Auonomous Sysems robabilisic Roboics

78 Robo Moion Bel p' u Bel d p p u Auonomous Sysems robabilisic Roboics

79 aricle Filer Algorihm wih < [i] w [i] > ℵ i... m Auonomous Sysems robabilisic Roboics

80 aricle Filer Algorihm Bel η p p u Bel d Imporance facor for i : w i draw i - from Bel - draw i from p i - u arge disribuion proposal disribuion η p p u Bel p u Bel p Auonomous Sysems robabilisic Roboics

81 Resampling Given: Se χ of weighed samples. Waned : Random sample where he probabiliy of drawing i is given by w i Typically done M imes wih replacemen o generae new sample se Even hough he variance of he paricle se iself decreases he variance of he paricle se as an esimaor of he rue belief increases over ime χ Auonomous Sysems robabilisic Roboics

82 Re-sampling: variance reducion When he sae is known o be saic X X - do no re-sample o avoid paricle depleion Even when he sae changes i is ofen a good idea o reduce he re-sampling frequency and inegrae muliple measuremens beween resamplings as non-normalied weighs Auonomous Sysems robabilisic Roboics

83 Selecive Re-sampling Re-sampling is dangerous since imporan samples migh ge los paricle depleion problem In case of subopimal proposal disribuions re-sampling is necessary o achieve convergence. Key quesion: When should we re-sample? All weighs equal ero esimaor variance: don re-sample A few paricles have he whole weigh high es. var.: re-sample Auonomous Sysems robabilisic Roboics

84 Selecive Re-sampling Number of effecive paricles: n eff M m w 2 m assuming normalied weighs Empirical measure of how well he goal disribuion is approimaed by samples drawn from he proposal n eff describes he inverse of he variance in erms of esimaor of he paricle weighs n eff is maimal M for equal weighs. In his case he arge disribuion is close o he proposal disribuion Only re-sample when n eff drops below a given hreshold M/2 Auonomous Sysems robabilisic Roboics

85 Re-sampling: variance reducion 2 W n- w n w w 2 W n- w n w w 2 w 3 w 3 Roulee wheel Binary search Compleiy OM log M Sochasic universal sampling Sysemaic resampling Linear ime compleiy OM Easy o implemen low variance Auonomous Sysems robabilisic Roboics

86 Low Variance Re-sampling Auonomous Sysems robabilisic Roboics