Probabilistic Robotics - Pomona

1 ProbabilisticRoboticsProbabilisticRoboti csSebastianThrunWolframBurgardDieterFoxT heMITP ressCambridge,MassachusettsLondon,Englan d 2006 MassachusettsInstituteofTechnologyAll rights reserved. No part of this book may be reproduced in any form by anyelectronic or mechanical means (including photocopying, recording, or informationstorageandretrieval) use. For information, please email orwrite to Special Sales Department, The MIT Press, 55 Hayward Street, Cambridge, . ,Sebastian,1967 Probabilisticrobotics/SebastianThrun,Wol framBurgard, (Intelligentroboticsandautonomousagentss eries) : 978-0-262-20162-9( )1. Robotics . 2. Probabilities. ,Wolfram. ,Dieter. 92 dc222005043346109876563 Brief ContentsI Basics 11 Introduction32 Recursive State Estimation133 Gaussian Filters394 Nonparametric Filters855 Robot Motion1176 Robot Perception149II Localization1897 Mobile Robot Localization: Markov and Gaussian1918 Mobile Robot Localization: Grid And Monte Carlo237 III Mapping 2799 Occupancy Grid Mapping28110 Simultaneous Localization and Mapping30911 The GraphSLAM Algorithm33712 The Sparse Extended Information Filter38513 The fastslam Algorithm437IV Planning and Control 48514 Markov Decision Processes48715 Partially Observable Markov Decision Processes513viBrief Contents16 Approximate POMDP Techniques54717 Exploration569 ContentsPrefacexviiAcknowledgmentsxixI Basics UncertaintyinRobotics ProbabilisticRobotics Implications RoadMap TeachingProbabilisticRobotics BibliographicalRemarks 112 Recursive State Introduction BasicConceptsinProbability RobotEnvironmentInteraction State EnvironmentInteraction ProbabilisticGenerativeLaws BeliefDistributions BayesFilters TheBayesFilterAlgorithm Example MathematicalDerivationoftheBayesFilter TheMarkovAssumption RepresentationandComputation Summary BibliographicalRemarks Exercises 363

2 Gaussian Introduction TheKalmanFilter LinearGaussianSystems TheKalmanFilterAlgorithm Illustration MathematicalDerivationoftheKF TheExtendedKalmanFilter WhyLinearize? LinearizationViaTaylorExpansion TheEKFA lgorithm MathematicalDerivationoftheEKF PracticalConsiderations TheUnscentedKalmanFilter LinearizationViatheUnscentedTransform TheUKFA lgorithm TheInformationFilter CanonicalParameterization TheInformationFilterAlgorithm MathematicalDerivationoftheInformationFi lter TheExtendedInformationFilterAlgorithm MathematicalDerivationoftheExtendedInfor mationFilter PracticalConsiderations Summary BibliographicalRemarks Exercises 814 Nonparametric TheHistogramFilter TheDiscreteBayesFilterAlgorithm ContinuousState MathematicalDerivationoftheHistogramAppr oximation DecompositionTechniques BinaryBayesFilterswithStaticState TheParticleFilter BasicAlgorithm ImportanceSampling MathematicalDerivationofthePF PracticalConsiderationsandPropertiesofPa rticleFilters Summary BibliographicalRemarks Exercises 1155 Robot Introduction Preliminaries

3 KinematicConfiguration ProbabilisticKinematics VelocityMotionModel ClosedFormCalculation SamplingAlgorithm MathematicalDerivationoftheVelocityMotio nModel OdometryMotionModel ClosedFormCalculation SamplingAlgorithm MathematicalDerivationoftheOdometryMotio nModel MotionandMaps Summary BibliographicalRemarks Exercises 1456 Robot Introduction Maps BeamModelsofRangeFinders TheBasicMeasurementAlgorithm AdjustingtheIntrinsicModelParameters MathematicalDerivationoftheBeamModel PracticalConsiderations LimitationsoftheBeamModel LikelihoodFieldsforRangeFinders BasicAlgorithm Extensions Correlation-BasedMeasurementModels Feature-BasedMeasurementModels FeatureExtraction LandmarkMeasurements SensorModelwithKnownCorrespondence SamplingPoses FurtherConsiderations PracticalConsiderations Summary BibliographicalRemarks Exercises 185II Localization1897 Mobile Robot Localization: Markov and ATaxonomyofLocalizationProblems MarkovLocalization IllustrationofMarkovLocalization EKFL ocalization Illustration TheEKFL ocalizationAlgorithm MathematicalDerivationofEKFL ocalization PhysicalImplementation EstimatingCorrespondences EKFL ocalizationwithUnknownCorrespondences MathematicalDerivationoftheMLDataAssocia tion Multi-HypothesisTracking UKFL ocalization MathematicalDerivationofUKFL ocalization Illustration PracticalConsiderations Summary BibliographicalRemarks Exercises 2348 Mobile Robot Localization: Grid And Monte Introduction GridLocalization BasicAlgorithm GridResolutions ComputationalConsiderations Illustration MonteCarloLocalization Illustration TheMCLA lgorithm PhysicalImplementations PropertiesofMCL RandomParticleMCL:RecoveryfromFailures ModifyingtheProposalDistribution KLD-Sampling.

4 AdaptingtheSizeofSampleSets LocalizationinDynamicEnvironments PracticalConsiderations Summary BibliographicalRemarks Exercises 276 III Mapping 2799 Occupancy Grid Introduction TheOccupancyGridMappingAlgorithm Multi-SensorFusion LearningInverseMeasurementModels InvertingtheMeasurementModel SamplingfromtheForwardModel TheErrorFunction ExamplesandFurtherConsiderations MaximumAPosterioriOccupancyMapping TheCaseforMaintainingDependencies OccupancyGridMappingwithForwardModels Summary BibliographicalRemarks Exercises 30710 Simultaneous Localization and Introduction SLAM withExtendedKalmanFilters SetupandAssumptions SLAM withKnownCorrespondence MathematicalDerivationofEKFSLAM EKFSLAM withUnknownCorrespondences TheGeneralEKFSLAMA lgorithm Examples FeatureSelectionandMapManagement Summary BibliographicalRemarks Exercises 33411 The GraphSLAM Introduction IntuitiveDescription BuildingUptheGraph Inference TheGraphSLAMA lgorithm MathematicalDerivationofGraphSLAM TheFullSLAMP osterior TheNegativeLogPosterior TaylorExpansion ConstructingtheInformationForm ReducingtheInformationForm RecoveringthePathandtheMap DataAssociationinGraphSLAM TheGraphSLAMA lgorithmwithUnknownCorrespondence MathematicalDerivationoftheCorrespondenc eTest EfficiencyConsideration EmpiricalImplementation AlternativeOptimizationTechniques Summary BibliographicalRemarks Exercises 38212 The Sparse Extended Information Introduction IntuitiveDescription TheSEIFSLAMA lgorithm MathematicalDerivationoftheSEIF MotionUpdate MeasurementUpdates Sparsification GeneralIdea SparsificationinSEIFs MathematicalDerivationoftheSparsificatio n AmortizedApproximateMapRecovery HowSparseShouldSEIFsBe?

5 IncrementalDataAssociation ComputingIncrementalDataAssociationProba bilities PracticalConsiderations Branch-and-BoundDataAssociation RecursiveSearch ComputingArbitraryDataAssociationProbabi lities EquivalenceConstraints PracticalConsiderations Multi-RobotSLAM IntegratingMaps MathematicalDerivationofMapIntegration EstablishingCorrespondence Example Summary BibliographicalRemarks Exercises 43513 The fastslam TheBasicAlgorithm FactoringtheSLAMP osterior MathematicalDerivationoftheFactoredSLAMP osterior fastslam withKnownDataAssociation ImprovingtheProposalDistribution ExtendingthePathPosteriorbySamplingaNewP ose UpdatingtheObservedFeatureEstimate CalculatingImportanceFactors UnknownDataAssociation MapManagement TheFastSLAMA lgorithms EfficientImplementation FastSLAMforFeature-BasedMaps EmpiricalInsights LoopClosure Grid-basedFastSLAM TheAlgorithm EmpiricalInsights Summary BibliographicalRemarks Exercises 482IV Planning and Control 48514 Markov Decision Motivation UncertaintyinActionSelection ValueIteration GoalsandPayoff FindingOptimalControlPoliciesfortheFully ObservableCase ComputingtheValueFunction ApplicationtoRobotControl Summary BibliographicalRemarks Exercises 510 Contentsxv15 Partially Observable Markov Decision Motivation AnIllustrativeExample Setup ControlChoice Sensing Prediction DeepHorizonsandPruning TheFiniteWorldPOMDPA lgorithm MathematicalDerivationofPOMDPs ValueIterationinBeliefSpace ValueFunctionRepresentation CalculatingtheValueFunction PracticalConsiderations Summary BibliographicalRemarks Exercises 54416 Approximate POMDP Motivation QMDPs AugmentedMarkovDecisionProcesses TheAugmentedStateSpace TheAMDPA lgorithm MathematicalDerivationofAMDPs ApplicationtoMobileRobotNavigation MonteCarloPOMDPs UsingParticleSets

6 TheMC-POMDPA lgorithm MathematicalDerivationofMC-POMDPs PracticalConsiderations Summary BibliographicalRemarks Exercises Introduction BasicExplorationAlgorithms InformationGain GreedyTechniques MonteCarloExploration Multi-StepTechniques ActiveLocalization ExplorationforLearningOccupancyGridMaps ComputingInformationGain PropagatingGain ExtensiontoMulti-RobotSystems ExplorationforSLAM EntropyDecompositioninSLAM ExplorationinFastSLAM EmpiricalCharacterization Summary BibliographicalRemarks Exercises 604 Bibliography607 Index639 PrefaceThis book provides a comprehensive introduction into the emerging fieldof Probabilistic Robotics . Probabilistic Robotics is a subfield of Robotics con-cerned with perception and control. It relies on statistical techniques forrepresenting information and making decisions. By doing so, it recent years, Probabilistic techniques have become one of the dominantparadigmsforalgorithmdesigninrob otics.

7 Book has a strong focus on algorithms. All algorithms in this bookarebasedonasingleoverarchingmathemat icalfoundation: Bayesrule,andits temporal extension known as Bayes filters. This unifying , wehavetriedtobeascompleteaspossiblewithr e-gards to technical detail. Each chapter describes one or more major algo-rithms. Foreachalgorithm,weprovidethefollowingfo urthings: (1)anex-ampleimplementationinpseudocode; (2)acompletemathematicalderiva-tionfromf irstprinciplesthatmakesthevariousassumpt ionsbehindeachal-gorithmexplicit;(3)empi ricalresultsinsofarastheyfurthertheunder stand-ingofthealgorithmspresentedinthebo ok;and(4)adetaileddiscussionofthestrengt hsandweaknessesofeachalgorithm fromapractitioner sper-spective. Developing all this for many different algorithms proved to be alaborious task. The result might at times be a bit difficult to digest for thecasualreader althoughskippingthemathematicalderivatio nsectionsisal-waysanoption! Wehopethatacarefulreaderemergeswithamuch deeperlevelofunderstandingthananysuperfi cial, ,theauthors,our students, and many of our colleagues in the field.

8 We began writing itin 1999, hoping that it would take not much more than a few months tocomplete this book. Five years have passed, and almost nothing from theoriginaldrafthassurvived. Throughworkingonthisbook,wehavelearnedmu chmoreaboutinformationanddecisiontheoryt hanwethoughtweeverwould. We are happy to report that much of what we learned has made monograph is written for students, researchers, and practitioners inrobotics. We believe everybody building robots has to develop the material in this book should be relevant to every roboticist. Itshouldalsobeofinteresttoappliedstatist icians,andpeopleconcernedwithreal-worlds ensordataoutsidetherealmofrobotics. Toserveawiderangeofreaderswithvaryingtec hnicalbackgrounds,wehaveattemptedtomaket hisbookasself-containedaspossible. Somepriorknowledgeoflinearalge-braandbas icprobabilityandstatisticswillbehelpful, butwehaveincludedaprimerforthebasiclawso fprobability, Eachchapteroffersanumberof exercises and suggests hands-on projects. When used in the classroom,each chapter should be covered in one or two lectures.

9 Chapters should beskippedorreorderedquitearbitrarily;inf act,inourownteachingweusuallystartrighti nthemiddleofthebook,withChapter7. Werecommendthatthestudyofthebookbeaccomp aniedbypractical,hands-onexperimentation asdirectedbytheexercisesattheendofeachch apter. Nothingmoreimportantinroboticsthandoingi tyourself!Despite our very best efforts, we believe there will still be techni-cal errors left in this book. Many of these errors have been correctedin this third printing of the book. We continue to post corrections onthe book s Web site, along with other materials relevant to this !SebastianThrunWolframBurgardDieterFoxAc knowledgmentsThisbookwouldnothavebeenpos siblewithoutthehelpandsupportfromsomanyf riends,familymembers,students,andcolleag uesinthefield. of the material in this book is the result of collaborations with ourcurrent and past students and post-docs. We specifically would like to ac-knowledge Rahul Biswas, Matthew Deans, Frank Dellaert, James Diebel,BrianGerkey, DirkH hnel, Johnathan Ko, CodyKwok, JohnLangford, LinLiao, DavidLieb, BensonLimketkai, MichaelLittman, YufengLiu, AndrewLookingbill, Dimitris Margaritis, Michael Montemerlo, Mark Moors, MarkPaskin, Joelle Pineau, Charles Rosenberg, Nicholas Roy, Aaron Shon, JamieSchulte,DirkSchulz,DavidStavens,Cyr illStachniss,andChieh-ChihWang,alongwith allotherpastandpresentmembersofourlabs.

10 GregArmstrong,GrinnellMore,TysonSawyer, of our research was conducted while we were with Carnegie Mel-lon University in Pittsburgh, PA, and we thank our former colleagues andfriends at CMU for many inspirational discussions. We also would like toexpress our gratitude to Armin Cremers from the University of Bonn are indebted to numerous colleagues whose comments and insightswere instrumental during the course of our research. We would specifi-cally like to thank Gary Bradski, Howie Choset, Henrik Christensen, HughDurrant-Whyte, Nando de Freitas, Zoubin Gharamani, Geoffrey Gordon,Steffen Gutmann, Andrew Howards, Leslie Kaelbling, Daphne Koller, KurtKonoligeBenKuipers,JohnLeonard,TomMi tchell,KevinMurphy,EduardoNebot, PaulNewman, , ReidSimmons, SatinderSingh, Gau-xxAcknowledgmentsravSukhatme,JuanTar d s,BenWegbreit, Araneda, Gal Elidan, Udo Frese, Gabe Hoffmann, John Leonard,BensonLimketkai,RudolphvanderMer we,AnnaPetrovskaya,BobWang,and Stefan Williams gave us extensive comments on earlier drafts of thisbook, which we gratefully acknowledge.