CSE-571 Probabilistic Robotics

Transcription

1 CSE-571 Proalstc Rootcs Rao-Blacwelzed Partcle Flters and Applcatons Ball Tracng n RooCup Extreely nosy nonlnear oton of oserver Inaccurate sensng, lted processng power Interactons etween target and Goal: envronent Unfed fraewor for odelng the all Interactons and etween ts nteractons. roots and target Deter Fox CSE-571: Proalstc Rootcs 2 Tracng Technques Dynac Bayes Networ for Ball Tracng Kalan Flter Hghly effcent, roust even for nonlnear Un-odal, lted handlng of nonlneartes Partcle Flter Less effcent, hghly roust Mult-odal, nonlnear, non-gaussan Rao-Blacwellsed Partcle Flter, MHT Cones PF wth KF Mult-odal, hghly effcent l l l z -2 z -1 z r -2 r -1 r -2 u -2 u z -2 z -1 z Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball oservaton Ball tracng Root localzaton Deter Fox CSE-571: Proalstc Rootcs 3 Deter Fox CSE-571: Proalstc Rootcs 1

2 Root Locaton Root and Ball Locaton and velocty l l l z -2 z -1 z r -2 r -1 r -2 u -2 u Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton l l l z -2 z -1 z r -2 r -1 r -2 u -2 u Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton z -2 z -1 z Ball oservaton z -2 z -1 z Ball oservaton Deter Fox CSE-571: Proalstc Rootcs 5 Deter Fox CSE-571: Proalstc Rootcs 6 Ball-Envronent Interactons Ball-Envronent Interactons 0. 8 resdual pro. Root loses gra 0. 2 None Graed C o l l s o n w t h o j e c t s o n a p None Wthn gra range and root gras pro. fro odel 1. 0 Graed Kc fals 0. 1 R o o t c s a l l 0. 9 Bounced Deflected Kced Bounced Deflected Kced Deter Fox CSE-571: Proalstc Rootcs 7 Deter Fox CSE-571: Proalstc Rootcs 8 2

3 Integratng Dscrete Ball Moton Mode Gra Exaple 1 l l l z -2 z -1 z r -2 r -1 r -2 u -2 u Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton l l l z -2 z -1 z r -2 r -1 r -2 u -2 u Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton z -2 z -1 z Ball oservaton z -2 z -1 z Ball oservaton Deter Fox CSE-571: Proalstc Rootcs 9 Deter Fox CSE-571: Proalstc Rootcs 10 Gra Exaple 2 Inference: Posteror Estaton l l l z -2 z -1 z r -2 r -1 r -2 u -2 u Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton l l l z -2 z -1 z r -2 r -1 r -2 u -2 u Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton z -2 z -1 z Ball oservaton z -2 z -1 z Ball oservaton Deter Fox CSE-571: Proalstc Rootcs 11 l p,, r z1 :, z1:, u1: 1 Deter Fox 12 3

4 Rao-Blacwellsed PF for Inference Represent posteror y rando saples Each saple s = r,, = x, y, θ,, µ, Σ contans root locaton, all ode, all Kalan flter Generate ndvdual coponents of a partcle stepwse usng the factorzaton p, p 1: 1:, r 1:, r 1: z, z 1: 1:, u, u 1: 1 1: 1 = p 1: r 1:, z 1:, u 1: 1 p r 1: z 1:, u 1: 1 Deter Fox CSE-571: Proalstc Rootcs 13 Rao-Blacwellsed Partcle Flter for Inference r r Map and root locaton Ball oton ode Ball locaton and velocty Draw a saple fro the prevous saple set: r 1, 1, 1 Deter Fox CSE-571: Proalstc Rootcs 1 Ball tracng Root localzaton Generate Root Locaton Generate Ball Moton Model r u -1 l z r Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton r u -1 l z r Landar detecton Map and root locaton Root control Ball oton ode Ball locaton and velocty Ball tracng Root localzaton r ~ p r r,,, z, u,_, _ Deter Fox CSE-571: Proalstc Rootcs 15 r ~ p r,,, z, u, Deter Fox CSE-571: Proalstc Rootcs 16 r,_

5 5 Update Ball Locaton and Velocty -1 r -1 r u -1 z -1 Ball locaton and velocty Ball oton ode Map and root locaton Root control Landar detecton Ball tracng Root localzaton l z 1,,,,, ~ r z r p Deter Fox 17 CSE-571: Proalstc Rootcs Iportance Resaplng Weght saple y f oservaton s landar detecton and y f oservaton s all detecton. Resaple l r z w p r p r z p r z p w d,,,,,, 1 1 = Deter Fox 18 CSE-571: Proalstc Rootcs Ball-Envronent Interacton Deter Fox 19 CSE73: Introducton to AI Ball-Envronent Interacton Deter Fox 20 CSE73: Introducton to AI

6 Tracng and Fndng the Ball Cluster all saples y dscretzng pan / tlt angles Uses negatve nforaton Deter Fox CSE-571: Proalstc Rootcs 21 Experent: Real Root Root cs all 100 tes, tres to fnd t afterwards Fnds all n 1.5 seconds on average Percentage of all lost Wth Map Wthout Map Deter Fox Nuer of all saples 22 Sulaton Runs Reference * Oservatons Coparson to KF* optzed for straght oton RBPF KF* Reference * Oservatons Deter Fox CSE-571: Proalstc Rootcs 23 Deter Fox CSE-571: Proalstc Rootcs 2 6

7 Coparson to KF nflated predcton nose Error vs. Predcton Te RBPF KF Reference * Oservatons RMS Error [c] RBPF KF' KF* Predcton te [sec] Deter Fox CSE-571: Proalstc Rootcs 25 Deter Fox CSE-571: Proalstc Rootcs 26 Orentaton Errors Goale Orentaton Error [degrees] RBPF KF* KF' Te [sec] Deter Fox CSE-571: Proalstc Rootcs 27 Deter Fox CSE-571: Proalstc Rootcs 28 7

8 [Lao-Fox-Kautz: AAAI-0, AIJ-07] STREET MAP Source: Tger / Lne data BUS ROUTES / STOPS Source: Metro GIS RESTAURANTS / STORES Source: MS MapPont Gven data strea fro a wearale GPS unt Infer the user s locaton and ode of transportaton foot, car, us, e,... Predct where user wll go Detect novel ehavor / user errors Deter Fox CSE-571: Proalstc Rootcs 29 Deter Fox CSE-571: Proalstc Rootcs 30 Dead and se-dead zones near uldngs, trees, etc. Sparse easureents nsde vehcles, especally us Mult-path propagaton Inaccurate street ap Map s drected graph Locaton: Edge e Dstance d fro start of edge Predcton: Move along edges accordng to velocty odel Correcton: Update estate ased on GPS readng Deter Fox CSE-571: Proalstc Rootcs 31 Deter Fox CSE-571: Proalstc Rootcs 32 8

9 e 1 e 1 x -1 e 3 x z e 0 e 2 e 3 e 2 Prole: Predcted locaton s ult-odal Deter Fox CSE-571: Proalstc Rootcs 33 Prole: GPS readng s not on the graph Deter Fox CSE-571: Proalstc Rootcs 3 x e 1 e 1 f q =e 1 e 3 z e 2 e 3 x f q =e 2 z e 2 Proalstcally snap GPS readng to the graph Perfor A* search to copute nnovaton Proalstcally snap GPS readng to the graph Perfor A* search to copute nnovaton Deter Fox CSE-571: Proalstc Rootcs 35 Deter Fox CSE-571: Proalstc Rootcs 36 9

10 Rao-Blacwellsed partcle flter represents posteror y sets of weghted partcles: S = { < s, w >, = 1,..., n} Each partcle contans Kalan flter for locaton: s = e, v, θ, N 2 µ, σ GPS easureents Partcles Kalan flters Edge transtons, veloctes, edge assocatons Gaussan for poston Deter Fox CSE-571: Proalstc Rootcs 37 Deter Fox CSE-571: Proalstc Rootcs 38 Encode pror nowledge nto the odel Modes have dfferent velocty dstrutons Buses run on us routes Get on/off the us near us stops -1 Transportaton ode x -1 x z -1 z Te -1 Te Edge, velocty, poston GPS readng Swtch to car near car locaton Partcles: 2 s =, e, v, θ, N µ, σ Deter Fox CSE-571: Proalstc Rootcs 39 Deter Fox CSE-571: Proalstc Rootcs 0 10

11 Measureents Projectons A B Worplace Green Red Blue Bus ode Car ode Foot ode Goal destnaton: worplace hoe, frends, restaurant,... Trp segents: <start, end, transportaton> Hoe to Bus stop A on Foot Bus stop A to Bus stop B on Bus Bus stop B to worplace on Foot Deter Fox CSE-571: Proalstc Rootcs 1 Deter Fox CSE-571: Proalstc Rootcs 2 g -1 g Goal t -1 t Trp segent -1 Transportaton ode x -1 x z -1 z Te -1 Te Partcles: Edge, velocty, poston GPS readng 2 s = g, t,, e, v, θ, N µ, σ Deter Fox CSE-571: Proalstc Rootcs 3 Key to goal / path predcton and error detecton Custozed odel for each user Unsupervsed odel learnng Learn varale doans goals, trp segents Learn transton paraeters goals, trps, edges Tranng data 30 days GPS readngs of one user, logged every second when outdoors Deter Fox CSE-571: Proalstc Rootcs 11

12 The prole of fndng laels for unlaeled data In nature, tes often do not coe wth laels. How can we learn laels wthout a teacher? Iteratve ethod for fndng axu lelhood estates of paraeters n statstcal odels, where the odel depends on unoserved unlaeled latent varales. x Unlaeled data Laeled data x Deter Fox CSE-571: Proalstc Rootcs 5 Fro Shadehr & Dedrchsen Raw Proxty Sensor Data. ANEMIA PATIENTS AND CONTROLS Measured dstances for expected dstance of 300 c. Sonar Laser Red Blood Cell Heoglon Concentraton Red Blood Cell Volue 12

13 Coponent 1 Coponent Coponent 1 Coponent 2 px px Mxture Model Mxture Model px px D. Weld and D. Fox 9 x D. Weld and D. Fox 50 x px Coponent Models Mxtures If our data s not laeled, we can hypothesze that: 1. There are exactly classes n the data: 2. Each class y occurs wth a specfc frequency: 3. Exaples of class y are governed y a specfc dstruton: y { 1, 2, L, } x y P y p px Mxture Model D. Weld and D. Fox 51 x Accordng to our hypothess, each exaple x ust have een generated fro a specfc xture dstruton: x = = x = p P y j p y j We ght hypothesze that the dstrutons are Gaussan: Paraeters of the dstrutons j= 1 θ = { P y = 1,µ 1, 1,!, P y =,µ, } = P y = j N x µ j, j p x θ j=1 Mxng proportons Noral dstruton 13

14 Learnng Mxtures fro Data Consder fxed K = 2 e.g., Unnown paraeters Q = { 1, s 1, 2, s 2, a 1 } Gven data D = {x 1,.x N }, we want to fnd the paraeters Q that est ft the data 1977: The EM Algorth Depster, Lard, and Run General fraewor for lelhood-ased paraeter estaton wth ssng data start wth ntal guesses of paraeters E-step: estate eershps gven paras M-step: estate paras gven eershps Repeat untl convergence Converges to a local axu of lelhood E-step and M-step are often coputatonally sple Generalzes to axu a posteror wth prors EM for Mxture of Gaussans E-step: Copute proalty that pont x j was generated y coponent : p j = α Px j C = PC = p = j p j M-step: Copute new ean, covarance, and coponent weghts: µ 2 σ w p j j p x / p j j j p x µ j 2 D. Weld and D. Fox 55 / p Red Blood Cell Heoglon Concentraton ANEMIA PATIENTS AND CONTROLS Red Blood Cell Volue 1

15 . EM ITERATION 1. EM ITERATION 3 Red Blood Cell Heoglon Concentraton Red Blood Cell Heoglon Concentraton Red Blood Cell Volue Red Blood Cell Volue. EM ITERATION 5. EM ITERATION 10 Red Blood Cell Heoglon Concentraton Red Blood Cell Heoglon Concentraton Red Blood Cell Volue Red Blood Cell Volue 15

16 . EM ITERATION 15. EM ITERATION 25 Red Blood Cell Heoglon Concentraton Red Blood Cell Heoglon Concentraton Red Blood Cell Volue Red Blood Cell Volue 90 LOG-LIKELIHOOD AS A FUNCTION OF EM ITERATIONS. ANEMIA DATA WITH LABELS Log-Lelhood Red Blood Cell Heoglon Concentraton Anea Group Control Group EM Iteraton Red Blood Cell Volue 16

17 Mxture Densty Raw Sensor Data Measured dstances for expected dstance of 300 c. α α P z x, = α α ht unexp ax rand T P P P P ht unexp ax rand z x, z x, z x, z x, Sonar Laser Approxaton Results Laser Sonar E-Step Infer the transportaton ehavor gven the odel Sooth our nference Infer the data forward n te Infer the data acward n te 300c 00c 68 17

18 Infer Forward Infer Bacward 18

19 Soothed Inference M-Step Update the odel paraeters to etter explan the soothed nference Stochastc verson of the Bau-Welch Algorth Count how any partcles ove fro one edge to the next Update the transton proaltes to reflect the counts 75 19

20 Re-estate Model Predcted goal Predcted path GOING TO THE WORKPLACE GOING HOME Hgh proalty transtons: us car foot Deter Fox CSE-571: Proalstc Rootcs 79 Deter Fox CSE-571: Proalstc Rootcs 80 20

21 [Patterson-Lao-etAl: Ucop-0] -1 Behavor ode g -1 g Goal noral / unnown / error t -1 t Trp segent -1 Transportaton ode x -1 x Edge, velocty, poston z -1 z Te -1 Te GPS readng Deter Fox CSE-571: Proalstc Rootcs 81 Deter Fox CSE-571: Proalstc Rootcs 82 Deter Fox CSE-571: Proalstc Rootcs 83 Deter Fox CSE-571: Proalstc Rootcs 8 21

22 Dscusson Partcle flters are ntutve and sple Support pont-wse thnng reduced uncertanty It s an art to ae the wor Good for test pleentaton f syste ehavor s not well nown Ineffcent copared to Kalan flter Rao-Blacwellzaton Only saple dscrete / hghly non-lnear parts of state space Solve reanng part analytcally KF,dscrete Deter Fox CSE-571: Proalstc Rootcs 85 22