SLAM using Extended Kalman Filter on a Robot for Localization and Mappings

This is a method used for robot localization and mappings

1. 1
Robot Mapping
Extended Kalman Filter
Cyrill Stachniss

2. 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

3. 3
SLAM is a State Estimation
Problem
! Estimate the map and robot’s pose
! Bayes filter is one tool for state
estimation
! Prediction
! Correction

4. 4
Kalman Filter
! It is a Bayes filter
! Estimator for the linear Gaussian case
! Optimal solution for linear models and
Gaussian distributions

5. 5
Gaussians
! Everything is Gaussian

1D
3D

6. 6
Properties: Marginalization and
Conditioning
! Given

! The marginals are Gaussians
! as well as the conditionals

7. 7
Linear Model
! The Kalman filter assumes a linear
transition and observation model
! Zero mean Gaussian noise

8. 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.

9. 9
Linear Motion Model
! Motion under Gaussian noise leads to

10. 10
Linear Motion Model
! Motion under Gaussian noise leads to
!  describes the noise of the motion

11. 11
Linear Observation Model
! Measuring under Gaussian noise leads
to

12. 12
Linear Observation Model
! Measuring under Gaussian noise leads
to
!  describes the measurement noise

13. 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)

14. 14
Kalman Filter Algorithm

15. 15
1D Kalman Filter Example (1)
15
prediction measurement
correction
It's a weighted mean!

16. 16
1D Kalman Filter Example (2)
prediction
correction
measurement

17. 17
Kalman Filter Assumptions
! Gaussian distributions and noise
! Linear motion and observation model

What if this is not the case?

18. 18
Non-linear Dynamic Systems
! Most realistic problems (in robotics)
involve nonlinear functions

19. 19
Linearity Assumption Revisited

20. 20
Non-Linear Function
Non-Gaussian!

21. 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?

22. 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!

23. 23
EKF Linearization: First Order
Taylor Expansion
! Prediction:
! Correction:
Jacobian matrices

24. 24
Reminder: Jacobian Matrix
! It is a non-square matrix in general
! Given a vector-valued function

! The Jacobian matrix is defined as

25. 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

26. 26
EKF Linearization: First Order
Taylor Expansion
! Prediction:
! Correction:
Linear functions!

27. 27
Linearity Assumption Revisited

28. 28
Non-Linear Function

29. 29
EKF Linearization (1)

30. 30
EKF Linearization (2)

31. 31
EKF Linearization (3)

32. 32
Linearized Motion Model
! The linearized model leads to
!  describes the noise of the motion

33. 33
Linearized Observation Model
! The linearized model leads to
!  describes the measurement noise

34. 34
Extended Kalman Filter
Algorithm
KF vs. EKF

35. 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:

36. 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”