Sebastian Thrun Carnegie Mellon University University of Pittsburgh Particle Filters In Robotics or: How the World Became To Be One Big Bayes Network

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, Open Problems Particle Filters In Robotics Robotics Research Today

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Robotics Yesterday

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Robotics Today

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Robotics Tomorrow?

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, Open Problems Robotics Research Today Particle Filters In Robotics

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 CMU, 1997 with W. Burgard, A.B. Cremers, D. Fox, D. Hähnel, G. Lakemeyer, D. Schulz, W. Steiner

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 CMU, 1998 with M. Beetz, M. Bennewitz, W. Burgard, A.B. Cremers, F. Dellaert, D. Fox, D. Hähnel, C. Rosenberg, N. Roy, J. Schulte, D. Schulz

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 The Localization Problem n Objects n Robots n Other Agents local (tracking) global kidnapped ambiguous identity uniquely identifiable fast-moving static non-statio- nary one object few objects many objects

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Probabilistic Localization... observations robot poses controls x1x1 x2x2 xtxt u2u2 utut m z1z1 z2z2 x3x3 u3u3 z3z3 map z3z3 laser data u2u2 u3u3 map m controls x1x1 x2x2 u2u2 utut u3u3 robot poses x1x1 x2x2 u2u2 utut u3u3 “Bayes filter” HMMs DBNs POMDPs Kalman filters Particle filters Condensation etc

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Bayes Filter Localization [Nourbakhsh et al 94] [Simmons/Koenig 95] [Kaelbling et al 96]

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 What is the Right Representation? Multi-hypothesis [Weckesser et al. 98], [Jensfelt et al. 99] Particles [Kanazawa et al 95] [de Freitas 98] [Isard/Blake 98] [Doucet 98] Kalman filter [Schiele et al. 94], [Weiß et al. 94], [Borenstein 96], [Gutmann et al. 96, 98], [Arras 98] [Nourbakhsh et al. 95], [Simmons et al. 95], [Kaelbling et al. 96], [Burgard et al. 96], [Konolige et al. 99] Histograms (metric, topological)

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particle Filters For Localization

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Monte Carlo Localization (MCL) With: Wolfram Burgard, Dieter Fox, Frank Dellaert

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002

Monte Carlo Localization With: Frank Dellaert

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002

fast-moving kidnapped global Particle Filter in High Dimensions local (tracking) ambiguous identity uniquely identifiable one object few objects many objects/features static non-statio- nary

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Learning Maps aka Simultaneous Localization and Mapping (SLAM) 70 m

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 The SLAM Problem with known data association

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 EKF Approach [Smith, Self, Cheeseman, 1985] O(N2)O(N2)

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Kalman Filter Mapping: O(N 2 )

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 EKS-SLAM for Underwater Mapping Courtesy of Stefan Williams and Hugh Durrant-Whyte, Univ of Sydney

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particle Filtering in Low Dimensions! robot poses sample pose

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particle Filtering in High Dimensions? maps sample map 

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Factorization first developed by Murphy & Russell, 1999 Insight: Conditional Independence... Landmark 1 observations Robot poses controls x1x1 x2x2 xtxt u2u2 utut 22 11 z1z1 z2z2 x3x3 u3u3 z3z3 ztzt Landmark 2

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Rao-Blackwellized Particle Filters landmark n=2 … landmark n=N landmark n=1 landmark n=2 … landmark n=N landmark n=1 robot poses [Murphy 99, Montemerlo 02]

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 The FastSLAM Algorithm.7.1.2

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 FastSLAM - O(MN) n Update robot particles based on control u t n Incorporate observation z t into Kalman filters n Resample particle set M = Number of particles N = Number of map features O(M) Constant time per particle O(M) Constant time per particle O(MN) Linear time per particle

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Ben Wegbreit’s Log-Trick 3,33,3 n  3 ? FT n  2 ? F T n  4 ? F T [i] new particle old particle

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 FastSLAM - O(M logN) n Update robot particles based on control u t n Incorporate observation z t into Kalman filters n Resample particle set M = Number of particles N = Number of map features O(M) Constant time per particle O(M logN) Log time per particle O(M logN) Log time per particle

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Advantage of Structured PF Solution Kalman: O(N 2 ) 500 features Moore’s Theorem: logN  30 M: discussed later FastSLAM: O(MlogN) + global uncertainty, multi-modal + non-linear systems + sampling over data associations

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particles + Kalman filters 3 Examples Particles + Point Estimators Particles + Particles

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Outdoor Mapping (no GPS) With Juan Nieto, Eduardo Nebot, Univ of Sydney 4 km excursion

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 With Juan Nieto, Eduardo Nebot, Univ of Sydney

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particles + Point Estimators 3 Examples Particles + Kalman filters Particles + Particles

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Indoor Mapping n Map: point estimators (no uncertainty) n Lazy

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Importance of Particle Filters Non-probabilisticProbabilistic, with samples

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Multi-Robot Mapping

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Multi-Robot Exploration DARPA TMR MarylandDARPA TMR Texas With: Reid Simmons and Dieter Fox

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particles + Particles 3 Examples Particles + Kalman filters Particles + Point Estimators

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Tracking Moving Features With: Michael Montemerlo

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Tracking Moving Entities Through Map Differencing

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Map-Based People Tracking With: Michael Montemerlo

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Autonomous People Following With: Michael Montemerlo

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Advantage of Structured PF Solution Kalman: O(N 2 ) 500 features Moore’s Theorem: logN  30 M: discussed now! FastSLAM: O(MlogN) + global uncertainty, multi-modal + non-linear systems + sampling over data associations

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Worst-Case Environment ? … robot path N landmarks … … … Kalman filters: Maps (relative information) converges for linear-Gaussian case

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Relative Map Error (Simulation) error steps Kalman Filter 250 particles

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Robot-To-Map Error (Simulation) Kalman Filter 250 particles error steps

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Relative Map Error (Simulation) 100 particles Kalman Filter 250 particles 2 particles 100 particles Kalman Filter 250 particles error steps

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Robot-To-Map Error (Simulation) error steps Kalman Filter 250 particles 100 particles 2 particles

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Summary Results n O(N 2 )  O(MN)  O(M logN)  O(logN) O(N2)O(N2) O(logN) n Scalable(?) solution to data association problem

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Particle Filters In Robotics Robotics Research Today Robotics Research Today 4 Open Problems

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Can We Factorize Better? Static Factorization Dynamic Factorization

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Example: Multi-Robot Localization [Fox et al, 99]

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Example: Multi-Robot Localization... observations Robot 3 poses x1x1 x2x2 xtxt z1z1 x3x3 z3z3 x1x1 x2x2 xtxt x3x3 x1x1 x2x2 xtxt x3x3 z2z2 map Robot 1 poses Robot 2 poses observations z1z1 m z2z2 [Fox et al, 99]

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Dynamic Factorization ?? # samples error always use joint Task: calculate E[y|x] from samples always factorize factorize dynamically optimal Robot x Robot y

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Can We Learn Control? n Not an MDP n Not discrete or low-dimensional n Not knowledge-free n Only thing that matters in robotics Sondik 71,Littman/Kaelbling/Cassandra 96, …

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Implications for Planning & Control MDP PlannerPOMDP Planner N. Roy et al

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Can we Exploit Procedural Knowledge? LearningProgramming See David Andre’s and Stuart Russell’s AAAI paper this year! prob x = {{10, 0.2}, {11, 0.8}}; prob y = {{20, 0.5}, {21, 0.5}}; prob z = x + y; prob f = neuroNet(y); with Frank Pfenning, CMU

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 …And Can We Actually Do Something Useful? See poster by Anguelov et al.

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 The Nursebot Project University of Pittsburgh School of Nursing Prof. Jackie Dunbar-Jacob Prof. Sandy Engberg Prof. Margo Holm Prof. Deb Lewis Prof. Judy Matthews Prof. Barbara Spier School of Medicine Prof. Neil Resnick Prof. Joan Rogers Intelligent Systems Prof. Don Chiarulli University of Pittsburgh Computer Science Prof. Martha Pollack Carnegie Mellon University Computer Science, Robotics Prof. Sebastian Thrun Prof. Geoff Gordon Human Computer Interaction Prof. Sara Kiesler Financial Support National Science Foundation $1.4M ITR Grant $3.2M ITR Grant

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 The Nursebot Project

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Haptic Interface (In Development)

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Wizard of Oz Studies By Sara Kiesler, Jenn Goetz

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Truly Useful….?

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002

Stanford CS329 What Can Go Wrong? Model limitations And false assumptions n Map false, robot outside map n Independence assumption in sensor measurement noise n Robot goes through wall n Presence of people n Kidnapped robot problem n Invisible hazards? Approximation (Samples) n Small number of samples (eg, n=1) ignores measurements n Perfect sensors n Resampling without robot motion n Room full of chairs (discontinuities)

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Error as Function of Sensor Noise sensor noise level (in %) error (in cm) 1,000 samples assume higher noise 1,000 samples

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Probabilistic Localization... observations robot poses controls x1x1 x2x2 xtxt u2u2 utut m z1z1 z2z2 x3x3 u3u3 z3z3 map z3z3 Kanazawa et al, 95 x1x1 x2x2 u2u2 m z1z1 z2z2 x3x3 u3u3

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Practical problem: sampling ~measurement desired output: samplesinput: range scan

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Approach: Learn Tree of Joint f 1 (scan)<.5  (robot)>.62 f 2 (scan)>.5 ………… x(robot)<45 p=0.002 p=0.038 Algorithm:  gather scan  localize scan globally (with resamplig)  Add f(scan)+surviving poses to ‘training` set  Construct kd-tree

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 dual MCL Error as Function of Sensor Noise sensor noise level (in %) error (in cm) Reversed arc PF

Sebastian Thrun Carnegie Mellon UniversityUAI Aug 2, 2002 Results – Smithsonian Museum Combined With random samples Plain particle filers