robot mapping extended kalman filter -...
TRANSCRIPT
![Page 1: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/1.jpg)
1
Robot Mapping
Extended Kalman Filter
Cyrill Stachniss
![Page 2: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/2.jpg)
2
Goal: Simultaneous Localization and Mapping (SLAM) § Building a map and locating the robot
in the map at the same time § Chicken-or-egg problem
map
localize
![Page 3: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/3.jpg)
3
SLAM is a State Estimation Problem § Estimate the map and robot’s pose § Bayes filter is one tool for state
estimation § Prediction
§ Correction
![Page 4: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/4.jpg)
4
Kalman Filter
§ It is a Bayes filter § Estimator for the linear Gaussian case § Optimal solution for linear models and
Gaussian distributions
![Page 5: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/5.jpg)
5
Gaussians
§ Everything is Gaussian
1D 3D
![Page 6: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/6.jpg)
6
Properties: Marginalization and Conditioning § Given § The marginals are Gaussians
§ as well as the conditionals
![Page 7: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/7.jpg)
7
Linear Model
§ The Kalman filter assumes a linear transition and observation model
§ Zero mean Gaussian noise
![Page 8: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/8.jpg)
8
Components of a Kalman Filter
Matrix that describes how the state evolves from to without controls or noise. Matrix that describes how the control changes the state from to .
Matrix that describes how to map the state to an observation .
Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance and respectively.
![Page 9: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/9.jpg)
9
Linear Motion Model
§ Motion under Gaussian noise leads to
![Page 10: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/10.jpg)
10
Linear Motion Model
§ Motion under Gaussian noise leads to
§ describes the noise of the motion
![Page 11: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/11.jpg)
11
Linear Observation Model
§ Measuring under Gaussian noise leads to
![Page 12: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/12.jpg)
12
Linear Observation Model
§ Measuring under Gaussian noise leads to
§ describes the measurement noise
![Page 13: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/13.jpg)
13
Everything stays Gaussian
§ Given an initial Gaussian belief, the belief is always Gaussian
§ Proof is non-trivial (see Probabilistic Robotics, Sec. 3.2.4)
![Page 14: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/14.jpg)
14
Kalman Filter Algorithm
![Page 15: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/15.jpg)
15
1D Kalman Filter Example (1)
15
prediction measurement
correction
It's a weighted mean!
![Page 16: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/16.jpg)
16
1D Kalman Filter Example (2)
prediction
correction measurement
![Page 17: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/17.jpg)
17
Kalman Filter Assumptions
§ Gaussian distributions and noise § Linear motion and observation model
What if this is not the case?
![Page 18: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/18.jpg)
18
Non-linear Dynamic Systems
§ Most realistic problems (in robotics) involve nonlinear functions
![Page 19: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/19.jpg)
19
Linearity Assumption Revisited
![Page 20: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/20.jpg)
20
Non-Linear Function
Non-Gaussian!
![Page 21: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/21.jpg)
21
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 22: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/22.jpg)
22
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 23: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/23.jpg)
23
EKF Linearization: First Order Taylor Expansion § Prediction:
§ Correction: Jacobian matrices
![Page 24: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/24.jpg)
24
Reminder: Jacobian Matrix § It is a non-square matrix in general
§ Given a vector-valued function
§ The Jacobian matrix is defined as
![Page 25: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/25.jpg)
25
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 26: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/26.jpg)
26
EKF Linearization: First Order Taylor Expansion § Prediction:
§ Correction: Linear functions!
![Page 27: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/27.jpg)
27
Linearity Assumption Revisited
![Page 28: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/28.jpg)
28
Non-Linear Function
![Page 29: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/29.jpg)
29
EKF Linearization (1)
![Page 30: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/30.jpg)
30
EKF Linearization (2)
![Page 31: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/31.jpg)
31
EKF Linearization (3)
![Page 32: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/32.jpg)
32
Linearized Motion Model
§ The linearized model leads to
§ describes the noise of the motion
![Page 33: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/33.jpg)
33
Linearized Observation Model
§ The linearized model leads to
§ describes the measurement noise
![Page 34: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/34.jpg)
34
Extended Kalman Filter Algorithm
KF vs. EKF
![Page 35: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/35.jpg)
35
Extended Kalman Filter Summary § Extension of the Kalman filter § Ad-hoc solution to handle the non-
linearities § Performs local linearizations § Works well in practice for moderate
non-linearities § Complexity:
![Page 36: Robot Mapping Extended Kalman Filter - uni-freiburg.deais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/... · 2012-11-13 · Robot Mapping Extended Kalman Filter Cyrill Stachniss](https://reader030.vdocuments.mx/reader030/viewer/2022040402/5e7ec6f7366e8b313d70a257/html5/thumbnails/36.jpg)
36
Literature
Kalman Filter and EKF § Thrun et al.: “Probabilistic Robotics”,
Chapter 3 § Schön and Lindsten: “Manipulating the
Multivariate Gaussian Density” § Welch and Bishop: “Kalman Filter
Tutorial”