L11. EKF SLAM: PART I. NA568 Mobile Robotics: Methods & Algorithms

Transcription

1 L11. EKF SLAM: PART I NA568 Mobile Robotics: Methods & Algorithms

2 Today s Topic EKF Feature-Based SLAM State Representation Process / Observation Models Landmark Initialization Robot-Landmark Correlation

3 The SLAM Problem 3 A robot is exploring an unknown, static environment. Given: The robot s controls Observations of nearby features Estimate: Map of features Path of the robot

4 SLAM Applications 4 Indoors Undersea Space Underground

5 Representations 5 Grid maps or scans [Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01; ] Landmark-based [Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;

6 Illustration of SLAM without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard

7 Illustration of SLAM without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard

8 Illustration of SLAM without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard

9 Illustration of SLAM without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard

10 Illustration of SLAM without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard

11 Illustration of SLAM without Landmarks With only dead reckoning, vehicle pose uncertainty grows without bound Courtesy J. Leonard

12 Repeat, with Measurements of Landmarks First position: two features observed Courtesy J. Leonard

13 Illustration of SLAM with Landmarks Second position: two new features observed Courtesy J. Leonard

14 Illustration of SLAM with Landmarks Re-observation of first two features results in improved estimates for both vehicle and feature Courtesy J. Leonard

15 Illustration of SLAM with Landmarks Third position: two additional features added to map Courtesy J. Leonard

16 Illustration of SLAM with Landmarks Re-observation of first four features results in improved location estimates for vehicle and all features Courtesy J. Leonard

17 Illustration of SLAM with Landmarks Process continues as the vehicle moves through the environment Courtesy J. Leonard

18 SLAM Using Landmarks MIT Indoor Track Courtesy J. Leonard

19 Test Environment (Point Landmarks) Courtesy J. Leonard

20 View from Vehicle Courtesy J. Leonard

21 SLAM Using Landmarks 1. Move 2. Sense 3. Associate measurements with known features 4. Update state estimates for robot and previously mapped features 5. Find new features from unassociated measurements 6. Initialize new features 7. Repeat MIT Indoor Track

22 Comparison with Ground Truth odometry SLAM result Courtesy J. Leonard

23 Why is SLAM a hard problem? 23 SLAM: robot path and map are both unknown Robot path error correlates errors in the map

24 Why is SLAM a hard problem? 24 Robot pose uncertainty In the real world, the mapping between observations and landmarks is unknown Picking wrong data associations can have catastrophic consequences EKF SLAM is brittle in this regard Pose error correlates data associations

25 Simultaneous Localization and Mapping (SLAM) Building a map and locating the robot in the map at the same time Chicken-and-egg problem map localize Courtesy: Cyrill Stachniss

26 Definition of the SLAM Problem Given The robot s controls Observations Wanted Map of the environment Trajectory of the robot Courtesy: Cyrill Stachniss

27 Three Main Paradigms Kalman filter Particle filter Graphbased Courtesy: Cyrill Stachniss

28 Bayes Filter Recursive filter with prediction and correction step Prediction Correction Courtesy: Cyrill Stachniss

29 EKF for Online SLAM We consider here the Kalman filter as a solution to the online SLAM problem Courtesy: Thrun, Burgard, Fox

30 Extended Kalman Filter Algorithm Courtesy: Cyrill Stachniss

31 EKF SLAM Application of the EKF to SLAM Estimate robot s pose and locations of landmarks in the environment Assumption: known correspondences State space (for the 2D plane) is Courtesy: Cyrill Stachniss

32 EKF SLAM: State Representation Map with n landmarks: (3+2n)-dimensional Gaussian Belief is represented by Courtesy: Cyrill Stachniss

33 EKF SLAM: State Representation More compactly Courtesy: Cyrill Stachniss

34 EKF SLAM: State Representation Even more compactly (note: ) Courtesy: Cyrill Stachniss

35 EKF SLAM: Filter Cycle 1. State prediction 2. Measurement prediction 3. Measurement 4. Data association 5. Update Courtesy: Cyrill Stachniss

36 EKF SLAM: State Prediction Courtesy: Cyrill Stachniss

37 EKF SLAM: Measurement Prediction Courtesy: Cyrill Stachniss

38 EKF SLAM: Obtained Measurement Courtesy: Cyrill Stachniss

39 EKF SLAM: Data Association and Difference Between h(x) and z Courtesy: Cyrill Stachniss

40 EKF SLAM: Update Step Courtesy: Cyrill Stachniss

41 EKF SLAM: Concrete Example Setup Robot moves in the 2D plane Velocity-based motion model Robot observes point landmarks Range-bearing sensor Known data association Known number of landmarks Courtesy: Cyrill Stachniss

42 Initialization Robot starts in its own reference frame (all landmarks unknown) 2n+3 dimensions Courtesy: Cyrill Stachniss

43 Extended Kalman Filter Algorithm Courtesy: Cyrill Stachniss

44 Prediction Step (Motion) Goal: Update state space based on the robot s motion Robot motion in the plane How to map that to the 2n+3 dim space? Courtesy: Cyrill Stachniss

45 Update the State Space From the motion in the plane to the 2n+3 dimensional space Courtesy: Cyrill Stachniss

46 Extended Kalman Filter Algorithm DONE Courtesy: Cyrill Stachniss

47 Update Covariance The function only affects the robot s motion and not the landmarks Jacobian of the motion (3 3) Identity (2n 2n) Courtesy: Cyrill Stachniss

48 Jacobian of the Motion Courtesy: Cyrill Stachniss

49 Jacobian of the Motion Courtesy: Cyrill Stachniss

50 Jacobian of the Motion Courtesy: Cyrill Stachniss

51 Jacobian of the Motion Courtesy: Cyrill Stachniss

52 This Leads to the Time Propagation Apply & DONE Courtesy: Cyrill Stachniss

53 EKF SLAM: Motion Prediction Step Courtesy: Cyrill Stachniss

54 Extended Kalman Filter Algorithm DONE DONE Courtesy: Cyrill Stachniss

55 Extended Kalman Filter Algorithm DONE DONE Courtesy: Cyrill Stachniss

56 EKF SLAM: Correction Step Known data association : i-th measurement at time t observes the landmark with index j Initialize landmark if unobserved Compute the expected observation Compute the Jacobian of Proceed with computing the Kalman gain Courtesy: Cyrill Stachniss

57 Range-Bearing Observation Range-Bearing observation If landmark has not been observed observed location of landmark j estimated robot s location relative measurement Can initialize from a single measurement b/c the observation function is bijective for range/bearing. If using either range or bearing only, would need to accumulate observations over a time window and triangulate to initialize. Courtesy: Cyrill Stachniss

58 Expected Observation Compute expected observation according to the current estimate Courtesy: Cyrill Stachniss

59 Jacobian for the Observation Based on Compute the Jacobian low-dim space Courtesy: Cyrill Stachniss

60 Jacobian for the Observation Based on Compute the Jacobian Courtesy: Cyrill Stachniss

61 The First Component Based on We obtain (by applying the chain rule) Courtesy: Cyrill Stachniss

62 Jacobian for the Observation Based on Compute the Jacobian Courtesy: Cyrill Stachniss

63 Jacobian for the Observation Use the computed Jacobian map it to the high dimensional space Courtesy: Cyrill Stachniss

64 In other words, what that projection matrix F x,j does r w.r.t. robot w.r.t. other landmarks w.r.t. m i w.r.t. other landmarks

65 Next Steps as Specified DONE DONE Courtesy: Cyrill Stachniss

66 Extended Kalman Filter Algorithm DONE DONE Apply & DONE Apply & DONE Apply & DONE Courtesy: Cyrill Stachniss

67 EKF SLAM Correction (1/2) Courtesy: Cyrill Stachniss

68 EKF SLAM Correction (2/2) Courtesy: Cyrill Stachniss

69 EKF-SLAM Stacked (Batch) Update Update K t is a (3+2n) (2k) gain matrix composed of a weighted linear combination of the columns of t associated with R and m i indices Hence, in general, because K t is full, it non-trivially changes all of the elements in t during the covariance update

70 EKF-SLAM Sequential Update for i=1:k Think of this loop as a zero motion prediction + update step endfor Return the updated state

71 Batch vs Sequential Updates When can we do this? When sensor measurements are independent, i.e. Why? Because of conditional independence Are Batch vs. Sequential Updates Equivalent? Yes when observation models are linear (KF) Not quite when observation models are nonlinear (EKF) Why? because our linearization point for each H t i changes as we perform the updates sequentially

72 Implementation Notes Measurement update in a single step requires only one full belief update Always normalize the angular components You do not need to create the F projection matrices explicitly (e.g., in Matlab), they are shown for illustration of dimensionality. Prediction and update expressions can be more efficiently implemented accounting for the sparsity that exists in the Jacobians. Courtesy: Cyrill Stachniss

73 Done! Courtesy: Cyrill Stachniss

74 Loop-Closing Loop-closing means recognizing an already mapped area Data association under high ambiguity possible environment symmetries Uncertainties collapse after a loop-closure (whether the closure was correct or not) Courtesy: Cyrill Stachniss

75 Before the Loop-Closure Courtesy: K. Arras

76 After the Loop-Closure Courtesy: K. Arras

77 Loop-Closures in SLAM Loop-closing reduces the uncertainty in robot and landmark estimates This can be exploited when exploring an environment for the sake of better (e.g., more accurate) maps Wrong loop-closures lead to filter divergence Courtesy: Cyrill Stachniss

78 EKF SLAM Correlations In the limit, the landmark estimates become fully correlated Courtesy: Dissanayake

79 EKF SLAM Correlations 79 Blue path = true path Red path = estimated path Black path = odometry Approximate the SLAM posterior with a high-dimensional Gaussian [Smith & Cheesman, 1986] Single hypothesis data association Courtesy: M. Montemerlo

80 EKF SLAM Correlations Map Correlation matrix Courtesy: M. Montemerlo

81 EKF SLAM Correlations Map Correlation matrix Courtesy: M. Montemerlo

82 EKF SLAM Correlations Map Correlation matrix Courtesy: M. Montemerlo

83 EKF SLAM Correlations Covariance Matrix in the Linear Gaussian SLAM solution Pros : Enables us to transfer information from one part of the environment to the other parts Prevents overconfidence and divergence Cons : Increases computational complexity Maintain Slow Consistent Cross correlations Ignore Fast Inconsistent

84 EKF SLAM Correlations The correlation between the robot s pose and the landmarks cannot be ignored Assuming independence generates too optimistic estimates of the uncertainty Courtesy: J.M. Castellanos

85 EKF SLAM Uncertainties The determinant of any sub-matrix of the map covariance matrix decreases monotonically New landmarks are initialized with maximum uncertainty Courtesy: Dissanayake

86 EKF SLAM in the Limit In the limit, the covariance associated with any single landmark location estimate is determined only by the initial covariance in the vehicle location estimate. Courtesy: Dissanayake

87 Example: Victoria Park Dataset Courtesy: E. Nebot

88 Victoria Park: Data Acquisition Courtesy: E. Nebot

89 Victoria Park: EKF Estimate Courtesy: E. Nebot

90 Victoria Park: EKF Estimate Courtesy: E. Nebot

91 Victoria Park: Landmarks Courtesy: E. Nebot

92 92 Victoria Park: Landmark Covariance Courtesy: E. Nebot

93 Andrew Davison: MonoSLAM

94 EKF SLAM Complexity Cubic complexity depends only on the measurement dimensionality Cost per step: dominated by the number of landmarks: Memory consumption: The EKF becomes computationally intractable for large maps! Courtesy: Cyrill Stachniss

95 EKF SLAM Summary The first SLAM solution Convergence proof for the linear Gaussian case Can diverge if non-linearities are large (and the reality is non-linear...) Can deal only with a single mode Successful in medium-scale scenes Approximations exists to reduce the computational complexity Courtesy: Cyrill Stachniss

96 EKF SLAM Summary 96 Quadratic in the number of landmarks: O(n 2 ) Convergence results for the linear case. Can diverge if nonlinearities are large! Have been applied successfully in large-scale environments. Approximations reduce the computational complexity.

97 Next Lecture Data Association Incremental Maximum Likelihood (i.e., Nearest Neighbor) Joint Compatibility Branch and Bound

98 Literature EKF SLAM Thrun et al.: Probabilistic Robotics, Chapter 10 Smith, Self, & Cheeseman: Estimating Uncertain Spatial Relationships in Robotics Dissanayake et al.: A Solution to the Simultaneous Localization and Map Building (SLAM) Problem Durrant-Whyte & Bailey: SLAM Part 1 and SLAM Part 2 tutorials Courtesy: Cyrill Stachniss