Introduction to Mobile Robotics -...
Transcript of Introduction to Mobile Robotics -...
![Page 1: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/1.jpg)
1
Bayes Filter – Extended Kalman Filter
Introduction to Mobile Robotics
Wolfram Burgard
![Page 2: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/2.jpg)
Bayes Filter Reminder
Prediction
Correction
![Page 3: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/3.jpg)
Discrete Kalman Filter
3
Estimates the state x of a discrete-time controlled process
with a measurement
![Page 4: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/4.jpg)
4
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 ut changes the state from t-1 to t.
Matrix (kxn) that describes how to map the state xt to an observation zt.
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Qt and Rt respectively.
![Page 5: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/5.jpg)
Kalman Filter Update Example
5
prediction measurement
correction
It's a weighted mean!
![Page 6: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/6.jpg)
Kalman Filter Update Example
prediction
correction measurement
![Page 7: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/7.jpg)
Kalman Filter Algorithm 1. Algorithm Kalman_filter( µt-1, Σt-1, ut, zt):
2. Prediction: 3. 4.
5. Correction: 6. 7. 8.
9. Return µt, Σt
![Page 8: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/8.jpg)
Nonlinear Dynamic Systems Most realistic robotic problems involve
nonlinear functions
![Page 9: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/9.jpg)
Linearity Assumption Revisited
![Page 10: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/10.jpg)
Non-Linear Function
Non-Gaussian!
![Page 11: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/11.jpg)
Non-Gaussian Distributions The non-linear functions lead to non-
Gaussian distributions Kalman filter is not applicable anymore!
What can be done to resolve this?
![Page 12: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/12.jpg)
Non-Gaussian Distributions The non-linear functions lead to non-
Gaussian distributions Kalman filter is not applicable anymore!
What can be done to resolve this? Local linearization!
![Page 13: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/13.jpg)
EKF Linearization: First Order Taylor Expansion
Prediction: Correction:
Jacobian matrices
![Page 14: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/14.jpg)
Reminder: Jacobian Matrix It is a non-square matrix in general
Given a vector-valued function
The Jacobian matrix is defined as
![Page 15: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/15.jpg)
Reminder: Jacobian Matrix It is the orientation of the tangent plane to
the vector-valued function at a given point
Generalizes the gradient of a scalar valued function
![Page 16: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/16.jpg)
EKF Linearization: First Order Taylor Expansion
Prediction: Correction:
Linear function!
![Page 17: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/17.jpg)
Linearity Assumption Revisited
![Page 18: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/18.jpg)
Non-Linear Function
![Page 19: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/19.jpg)
EKF Linearization (1)
![Page 20: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/20.jpg)
EKF Linearization (2)
![Page 21: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/21.jpg)
EKF Linearization (3)
![Page 22: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/22.jpg)
EKF Algorithm
1. Extended_Kalman_filter( µt-1, Σt-1, ut, zt):
2. Prediction: 3. 4.
5. Correction: 6. 7. 8.
9. Return µt, Σt
![Page 23: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/23.jpg)
Example: EKF Localization EKF localization with landmarks (point features)
![Page 24: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/24.jpg)
1. EKF_localization ( µt-1, Σt-1, ut, zt, m): Prediction:
3.
5.
1.
2. 3.
Motion noise
Jacobian of g w.r.t location
Predicted mean Predicted covariance (V maps Q into state space)
Jacobian of g w.r.t control
![Page 25: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/25.jpg)
1. EKF_localization ( µt-1, Σt-1, ut, zt, m): Correction:
3.
5.
6. 7. 8. 9. 10.
Predicted measurement mean (depends on observation type)
Innovation covariance
Kalman gain
Updated mean
Updated covariance
Jacobian of h w.r.t location
![Page 26: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/26.jpg)
EKF Prediction Step Examples
![Page 27: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/27.jpg)
EKF Observation Prediction Step
![Page 28: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/28.jpg)
EKF Correction Step
![Page 29: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/29.jpg)
Estimation Sequence (1)
![Page 30: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/30.jpg)
Estimation Sequence (2)
![Page 31: Introduction to Mobile Robotics - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ss17/robotics/...Introduction to Mobile Robotics Wolfram Burgard Bayes Filter Reminder Prediction](https://reader030.fdocuments.us/reader030/viewer/2022040116/5ed4ddaed593760db5615618/html5/thumbnails/31.jpg)
Extended Kalman Filter Summary Ad-hoc solution to deal with non-linearities Performs local linearization in each step Works well in practice for moderate non-
linearities Example: landmark localization There exist better ways for dealing with
non-linearities such as the unscented Kalman filter called UKF
31