Localization, Mapping, SLAM and The Kalman Filter ...

1 RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxLocalization, mapping , slam and The Kalman Filter according to GeorgeRobotics Institute 16-735 ~motionHowie ~chosetRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxThe Problem What is the world around me ( mapping ) sense from various positions integrate measurements to produce map assumes perfect knowledge of position Where am I in the world ( localization ) sense relate sensor readings to a world model compute location relative to model assumes a perfect world model Together, these are slam (Simultaneous localization and mapping )RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D.

2 FoxLocalizationTracking: Known initial positionGlobal localization : Unknown initial positionRe- localization : Incorrect known position(kidnapped robot problem)Challenges Sensor processing Position estimation Control Scheme Exploration Scheme Cycle Closure Autonomy Tractability ScalabilitySLAMM apping while tracking locally and globallyRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxRepresentations for Robot LocalizationDiscrete approaches ( 95) Topological representation ( 95) uncertainty handling (POMDPs) occas.

3 Global localization , recovery Grid-based, metric representation ( 96) global localization , recoveryMulti-hypothesis ( 00) multiple Kalman filters global localization , recoveryParticle filters ( 99) sample-based representation global localization , recoveryKalman filters (late-80s?) Gaussians approximately linear models position trackingAIRoboticsRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxThree Major Map ModelsTopological:Collection of nodes and their interconnectionsGrid-Based:Collection of discretized obstacle/free-space pixelsFeature-Based:Collection of landmark locations and correlated uncertaintyElfes, Moravec, Thrun, Burgard, Fox, Simmons, Koenig, Konolige, , Durrant Whyte, Leonard, Nebot, Christensen, , Chong/Kleeman, Dudek, Choset,Howard, Mataric, 16-735, Howie Choset, with slides from george Kantor, Hager, and D.

4 FoxThree Major Map ModelsLocalize to nodesArbitrary localizationDiscrete localizationResolution vs. ScaleFrontier-based explorationGrid size and resolutionGrid-BasedGraph explorationNo inherent explorationExploration StrategiesMinimal complexityLandmark covariance (N2)Computational ComplexityTopologicalFeature-BasedRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxAtlas Framework Hybrid Solution: Local features extracted from local grid map. Local map frames created at complexity limit.

5 Topology consists of connected local map :Chong, Kleeman; Bosse, Newman, Leonard, Soika, Feiten, TellerRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxH-SLAMRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxWhat does a Kalman Filter do, anyway?xkFk xk Gkuk vkykHk xk wk()()()()() ()()()()()+=++=+1xknukmykpFk Gk Hkvk wkQk Rk()()()(), (), ()(), ()(), () is the - dimensional state vector (unknown) is the - dimensional input vector (known) is the - dimensional output vector (known, measured) are appropriately dimensioned system matrices (known) are zero - mean, white Gaussian noise with (known) covariance matrices Given the linear dynamical system.

6 The Kalman Filter is a recursion that provides the best estimate of the state 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxWhat s so great about that? noise smoothing (improve noisy measurements) state estimation (for state feedback) recursive (computes next estimate using only most recent measurement)xkFk xk Gkuk vkykHk xk wk()()()()() ()()()()()+=++=+1RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxHow does it work?xkFk xk Gkuk vkykHk xk wk()()()()() ()()()()()+=++=+1)|1( )()( )()()|( )()|1( kkxkHkykukGkkxkFkkx+=+=+ based on last correction based on prediction and current measurement:())|1( ),1(kkxkyfx++= prediction:xkkxkkx ++=++)|1( )1|1( RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D.

7 FoxFinding the correction (no noise!)Hxy={}yHxx== | best estimate comes from shortest x (k+1|k)x Want the best estimate to be consistent with sensor best"" theis)|1( that so find ,output and )|1( predictionGiven xxkkxxxykkx ++= +RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxFinding the correction (no noise!).ofestimate best"" theis)1|1( that so find ,output and )1|1( predictionGiven xxkxxxykx ++= +Hxy={}yHxx== |)|1( kkx+ best estimate comes from shortest x shortest is perpendicular to x x RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D.

8 FoxSome linear algebra{}yHxx== |a is parallel to if Ha = 0}0|0{)(= =HaaHNulla is parallel to if it lies in the null space of H)( if ),( allfor THcolumnbbvHNullv Weighted sum of columns means columns of sum weighted the, Hb=RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxFinding the correction (no noise!). of estimate best"" theis)|1( that so find ,output and )|1( predictionGiven xxkkxxxykkx ++= +Hxy={}yHxx== | best estimate comes from shortest x shortest is perpendicular to x () Hxnull()THxcolumn THx= {}yHxx== |)|1( kkx+ x RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D.

9 FoxFinding the correction (no noise!). of estimate best"" theis)|1( that so find ,output and )|1( predictionGiven xxkkxxxykkx ++= +Hxy={}yHxx== | best estimate comes from shortest x shortest is perpendicular to x () Hxnull()THxcolumn THx= {}yHxx== |x )|1( kkx+ ))|1( ())|1( (kkxxHkkxHy+ =+ = innovationReal output estimated outputRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxFinding the correction (no noise!). of estimate best"" theis)|1( that so find ,output and )|1( predictionGiven xxkkxxxykkx ++= +Hxy={}yHxx== | best estimate comes from shortest x shortest is perpendicular to x () Hxnull()THxcolumn THx= assume is a linear function of KHxT= Kmmmatrix somefor {}yHxx== |x )|1( kkx+ Guess, hope, lets face it, it has to be some function of the innovationRI 16-735, Howie Choset, with slides from george Kantor, Hager, and D.

10 FoxFinding the correction (no noise!).ofestimate best"" theis)|1( that so find ,output and predictionGiven xxkkxxxyx ++= Hxy={}yHxx== |we requireyxkkxH= ++))|1( ({}yHxx== |x )|1( kkx+ RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D. FoxFinding the correction (no noise!)Hxy={}yHxx== |we require =+ =+ = ))|1( ()|1( kkxxHkkxHyxH{}yHxx== |x )|1( kkx+ yxkkxH= ++))|1( (.ofestimate best"" theis)|1( that so find ,output and predictionGiven xxkkxxxyx ++= RI 16-735, Howie Choset, with slides from george Kantor, Hager, and D.