Download presentation
Presentation is loading. Please wait.
Published byMaximillian Nichols Modified over 8 years ago
1
Probabilistic Robotics Bayes Filter Implementations Gaussian filters
2
Prediction (Action) Correction (Measurement) Bayes Filter Reminder
3
Gaussians -- Univariate Multivariate
4
Properties of Gaussians
5
We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations. Multivariate Gaussians
6
6 Discrete Kalman Filter Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation with a measurement
7
7 Components of a Kalman Filter Matrix (nxn) that describes how the state evolves from t-1 to t without controls or noise. Matrix (nxl) that describes how the control u t changes the state from t-1 to t. Matrix (kxn) that describes how to map the state x t to an observation z t. Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance R t and Q t respectively.
8
8 Kalman Filter Updates in 1D Measurement Initial belief Measurement with associated uncertainty Integrating measurement into belief via KF
9
9 Kalman Filter Updates in 1D Kalman gain
10
10 Kalman Filter Updates in 1D Motion to the right, == Introduces uncertainty
11
11 Kalman Filter Updates New measurement Resulting belief Belief after motion to right
12
12 Linear Gaussian Systems: Initialization Initial belief is normally distributed:
13
13 Dynamics are linear function of state and control plus additive noise: Linear Gaussian Systems: Dynamics State transition probability
14
14 Linear Gaussian Systems: Dynamics
15
15 Observations are linear function of state plus additive noise: Linear Gaussian Systems: Observations Measurement probability
16
16 Linear Gaussian Systems: Observations
17
17 Kalman Filter Algorithm 1. Algorithm Kalman_filter( t-1, t-1, u t, z t ): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return t, t
18
18 The Prediction-Correction-Cycle Prediction
19
19 The Prediction-Correction-Cycle Correction
20
20 The Prediction-Correction-Cycle Correction Prediction
21
21 Kalman Filter Summary Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k 2.376 + n 2 ) Optimal for linear Gaussian systems! Most robotics systems are nonlinear!
22
22 Nonlinear Dynamic Systems Most realistic robotic problems involve nonlinear functions
23
23 Linearity Assumption Revisited
24
24 Non-linear Function
25
25 EKF Linearization (1)
26
26 EKF Linearization (2)
27
27 EKF Linearization (3)
28
28 Prediction: Correction: EKF Linearization: First Order Taylor Series Expansion
29
29 EKF Algorithm 1.Extended_Kalman_filter ( t-1, t-1, u t, z t ): 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. 9. Return t, t
30
30 EKF Summary Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k 2.376 + n 2 ) Not optimal! Can diverge if nonlinearities are large! Works surprisingly well even when all assumptions are violated!
31
31 Linearization via Unscented Transform EKF UKF
32
32 UKF Sigma-Point Estimate (2) EKF UKF
33
33 UKF Sigma-Point Estimate (3) EKF UKF
34
34 Unscented Transform Sigma points Weights Pass sigma points through nonlinear function Recover mean and covariance For n-dimensional Gaussian λ is scaling parameter that determine how far the sigma points are spread from the mean If the distribution is an exact Gaussian, β=2 is the optimal choice.
35
35 UKF Summary Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term) Derivative-free: No Jacobians needed Still not optimal!
36
36 Thank You
37
37 Localization Given Map of the environment. Sequence of sensor measurements. Wanted Estimate of the robot’s position. Problem classes Position tracking (initial robot pose is known) Global localization (initial robot pose is unknown) Kidnapped robot problem (recovery) “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]
38
38 Markov Localization Markov Localization: The straightforward application of Bayes filters to the localization problem
39
39 1.EKF_localization ( t-1, t-1, u t, z t, m): Prediction: 6. 7. 8. Motion noise covariance Matrix from the control Jacobian of g w.r.t location Predicted mean Predicted covariance Jacobian of g w.r.t control
40
40 1.EKF_localization ( t-1, t-1, u t, z t, m): Correction: 6. 7. 8. 9. 10. Predicted measurement mean Pred. measurement covariance Kalman gain Updated mean Updated covariance Jacobian of h w.r.t location
41
41 EKF Prediction Step
42
42 EKF Observation Prediction Step
43
43 EKF Correction Step
44
44 Estimation Sequence (1)
45
45 Estimation Sequence (2)
46
46 Comparison to GroundTruth
47
47 UKF_localization ( t-1, t-1, u t, z t, m): Prediction: Motion noise Measurement noise Augmented state mean Augmented covariance Sigma points Prediction of sigma points Predicted mean Predicted covariance
48
48 UKF_localization ( t-1, t-1, u t, z t, m): Correction: Measurement sigma points Predicted measurement mean Pred. measurement covariance Cross-covariance Kalman gain Updated mean Updated covariance
49
49 1.EKF_localization ( t-1, t-1, u t, z t, m): Correction: 6. 7. 8. 9. 10. Predicted measurement mean Pred. measurement covariance Kalman gain Updated mean Updated covariance Jacobian of h w.r.t location
50
50 UKF Prediction Step
51
51 UKF Observation Prediction Step
52
52 UKF Correction Step
53
53 EKF Correction Step
54
54 Estimation Sequence EKF PF UKF
55
55 Estimation Sequence EKF UKF
56
56 Prediction Quality EKF UKF
57
57 [Arras et al. 98]: Laser range-finder and vision High precision (<1cm accuracy) Kalman Filter-based System [Courtesy of Kai Arras]
58
58 Multi- hypothesis Tracking
59
59 Belief is represented by multiple hypotheses Each hypothesis is tracked by a Kalman filter Additional problems: Data association: Which observation corresponds to which hypothesis? Hypothesis management: When to add / delete hypotheses? Huge body of literature on target tracking, motion correspondence etc. Localization With MHT
60
60 Hypotheses are extracted from LRF scans Each hypothesis has probability of being the correct one: Hypothesis probability is computed using Bayes’ rule Hypotheses with low probability are deleted. New candidates are extracted from LRF scans. MHT: Implemented System (1) [Jensfelt et al. ’00]
61
61 MHT: Implemented System (2) Courtesy of P. Jensfelt and S. Kristensen
62
62 MHT: Implemented System (3) Example run Map and trajectory # hypotheses #hypotheses vs. time P(H best ) Courtesy of P. Jensfelt and S. Kristensen
Similar presentations
© 2024 SlidePlayer.com Inc.
All rights reserved.