View

136Download

2

Embed Size (px)

DESCRIPTION

SEIF, EnKF , EKF SLAM, Fast SLAM, Graph SLAM Pieter Abbeel UC Berkeley EECS Many slides adapted from Thrun , Burgard and Fox, Probabilistic Robotics. TexPoint fonts used in EMF. Read the TexPoint manual before you delete this box.: A A A A A A A A A A A A A. Information Filter. - PowerPoint PPT Presentation

Transcript

Apprenticeship Learning via Inverse Reinforcement Learning

SEIF, EnKF, EKF SLAM, Fast SLAM, Graph SLAM

Pieter AbbeelUC Berkeley EECS

Many slides adapted from Thrun, Burgard and Fox, Probabilistic Robotics

TexPoint fonts used in EMF. Read the TexPoint manual before you delete this box.: AAAAAAAAAAAAAPage #From an analytical point of view == Kalman filterDifference: keep track of the inverse covariance rather than the covariance matrix [matter of some linear algebra manipulations to get into this form]Why interesting?Inverse covariance matrix = 0 is easier to work with than covariance matrix = infinity (case of complete uncertainty)Inverse covariance matrix is often sparser than the covariance matrix --- for the insiders: inverse covariance matrix entry (i,j) = 0 if xi is conditionally independent of xj given some set {xk, xl, }Downside: when extended to non-linear setting, need to solve a linear system to find the mean (around which one can then linearize)See Probabilistic Robotics pp. 78-79 for more in-depth pros/cons and Probabilistic Robotics Chapter 12 for its relevance to SLAM (then often referred to as the sparse extended information filter (SEIF))

Information FilterPage #Represent the Gaussian distribution by samplesEmpirically: even 40 samples can track the atmospheric state with high accuracy with enKF UKF: 2 * n sigma-points, n = 106 + then still forms covariance matrices for updatesThe intellectual innovation:Transforming the Kalman filter updates into updates which can be computed based upon samples and which produce samples while never explicitly representing the covariance matrixEnsemble Kalman filter (enKF)Page # Prediction:

Correction: Return mt, St

Can update the ensemble by simply propagating through the dynamics model + adding sampled noiseKFenKFKeep track of , Keep track of ensemble [x1, , xN]?4Page #KF:

Current ensemble X = [x1, , xN]Build observations matrix Z = [zt+v1 zt+vN] where vi are sampled according to the observation noise modelThen the columns ofX + Kt(Z Ct X)form a set of random samples from the posteriorNote: when computing Kt, leave t in the format t = [x1-t xN-t] [x1-t xN-t]T enKF correction step

Can also compute C_t from samplesPage #Indeed, would be expensive to build up C.However: careful inspection shows that C only appears as in:C XC CT = C X XT CT can simply compute h(x) for all columns x of X and compute the empirical covariance matrices required

[details left as exercise]

How about C?Page #

Page #Mandel, 2007 A brief tutorial on the Ensemble Kalman Filter Evensen, 2009, The ensemble Kalman filter for combined state and parameter estimationReferences for enKFPage #Kalman filter exact under linear Gaussian assumptionsExtension to non-linear setting:Extended Kalman filterUnscented Kalman filterExtension to extremely large scale settings:Ensemble Kalman filterSparse Information filterMain limitation: restricted to unimodal / Gaussian looking distributionsCan alleviate by running multiple XKFs + keeping track of the likelihood; but this is still limited in terms of representational power unless we allow a very large number of themKF SummaryPage #If system is observable (=dual of controllable!) then Kalman filter will converge to the true state.System is observable iffO = [C ; CA ; CA2 ; ; CAn-1] is full column rank (1)Intuition: if no noise, we observe y0, y1, and we have that the unknown initial state x0 satisfies:y0 = C x0y1 = CA x0...yK = CAK x0This system of equations has a unique solution x0 iff the matrix [C; CA; CAK] has full column rank. B/c any power of a matrix higher than n can be written in terms of lower powers of the same matrix, condition (1) is sufficient to check (i.e., the column rank will not grow anymore after having reached K=n-1).Kalman filter propertyPage #The previous slide assumed zero control inputs at all times. Does the same result apply with non-zero control inputs? What changes in the derivation?Simple self-quizPage #EKF/UKF SLAMState: (nR, eR, R, nA, eA, nB, eB, nC, eC, nD, eD, nE, eE, nF, eF, nG, eG, nH, eH)Now map = location of landmarks (vs. gridmaps)Transition model: Robot motion model; Landmarks stay in placeBEFCHAGDR12Page #Simultaneous Localization and Mapping (SLAM)In practice: robot is not aware of all landmarks from the beginningMoreover: no use in keeping track of landmarks the robot has not received any measurements about Incrementally grow the state when new landmarks get encountered.13Page #Simultaneous Localization and Mapping (SLAM)Landmark measurement model: robot measures [ xk; yk ], the position of landmark k expressed in coordinate frame attached to the robot: h(nR, eR, R, nk, ek) = [xk; yk] = R() ( [nk; ek] - [nR; eR] )

Often also some odometry measurements E.g., wheel encodersAs they measure the control input being applied, they are often incorporated directly as control inputs (why?)14Page #Victoria Park Data Set

[courtesy by E. Nebot]15Page #Victoria Park Data Set Vehicle

[courtesy by E. Nebot]16Page #Data Acquisition[courtesy by E. Nebot]

17Page #18Estimated Trajectory

[courtesy by E. Nebot]18Page #19EKF SLAM Application

[courtesy by J. Leonard]19Page #20EKF SLAM Application

odometryestimated trajectory[courtesy by John Leonard]20Page #21Landmark-based Localization

21Page #EKF-SLAM: practical challengesDefining landmarksLaser range finder: Distinct geometric features (e.g. use RANSAC to find lines, then use corners as features)Camera: interest point detectors, textures, color, Often need to track multiple hypothesesData association/Correspondence problem: when seeing features that constitute a landmark --- Which landmark is it? Closing the loop problem: how to know you are closing a loop?Can split off multiple EKFs whenever there is ambiguity; Keep track of the likelihood score of each EKF and discard the ones with low likelihood scoreComputational complexity with large numbers of landmarks.

22Page #High-dimensional ocean and atmospheric circulation models (106 dimensional state space)SLAM with 106 landmarksBecomes computationally very challenging to work with the 106 x 106 covariance matrix (terabytes!)In SLAM community: information filter which keeps tracks of the inverse covariance matrix, which can often be well approximated by a sparse matrixIn civil engineering community: ensemble Kalman filter, with applications often being in tracking systems described by partial differential equations

KF over very large state spacesPage #24EKF_localization ( mt-1, St-1, ut, zt, m):

Correction:

Predicted measurement meanPred. measurement covarianceKalman gainUpdated meanUpdated covarianceJacobian of h w.r.t location24Page #25EKF Prediction Step

25Page #26EKF Observation Prediction Step

26Page #27EKF Correction Step

27Page #28Estimation Sequence (1)

28Page #29Estimation Sequence (2)

29Page #30

Comparison to GroundTruth

30Page #Fast SLAMRao-Blackwellized particle filterRobot state = x, y, (just like gMapping)Map = Landmark based (vs. map = gridmap for gMapping)

Key observation (why Rao Blackwellization is so useful):Location of landmark i is independent of location of landmark j given the entire robot pose sequenceInstead of joint Gaussian over poses of all landmarks, can just keep track of Gaussian for each landmark separatelyLinear scaling with number of landmarks (rather than quadratic)Landmark based vs. occupancy gridProbability distribution representation:EKF vs. particle filter vs. Rao-Blackwellized particle filter

EKF, SEIF, FastSLAM are all online

Currently popular 4th alternative: GraphSLAMSLAM thus farPage #Graph-based FormulationUse a graph to represent the problemEvery node in the graph corresponds to a pose of the robot during mappingEvery edge between two nodes corresponds to the spatial constraints between them

Goal: Find a configuration of the nodes that minimize the error introduced by the constraints

33Page #Problem FormulationThe problem can be described by a graph

Goal:Find the assignment of poses to the nodes of the graph which minimizes the negative log likelihood of the observations:nodesObservation of fromerror

34Page #Approaches2D approaches:Lu and Milios, 97Montemerlo et al., 03Howard et al., 03Dellaert et al., 03Frese and Duckett, 05Olson et al., 06Grisetti et al., 07Tipaldi et al., 07

3D approaches:Nuechter et al., 05Dellaert et al., 05Triebel et al., 06Grisetti et al., 08/09

35Say more on 3D approachesPage #Graph-Based SLAM in a NutshellProblem described as a graphEvery node corresponds to a robot position and to a laser measurementAn edge between two nodes represents a data-dependent spatial constraint between the nodes

[KUKA Hall 22, courtesy P. Pfaff & G. Grisetti]36Page #

Graph-Based SLAM in a NutshellProblem described as a graphEvery node corresponds to a robot position and to a laser measurementAn edge between two nodes represents a data-dependent spatial constraint between the nodes[KUKA Hall 22, courtesy P. Pfaff & G. Grisetti]37Page #

Graph-Based SLAM in a NutshellOnce we have the graph, we determine the most likely map by moving the nodes[KUKA Hall 22, courtesy P. Pfaff & G. Grisetti]38Page #Graph-Based SLAM in a NutshellOnce we have the graph, we determine the most likely map by moving the nodes like this.

[KUKA Hall 22, courtesy P. Pfaff & G. Grisetti]39Page #

Once we have the graph, we determine the most likely map by moving the nodes like this.Then we render a map based on the known posesGraph-Based SLAM in a Nutshell[KUKA Hall 22, courtesy P. Pfaff & G. Grisetti]40Page #Graph-based Visual SLAM

Visual odometryLoop Closi