Robot Mapping

Extended Kalman Filter

Gian Diego Tipaldi, Wolfram Burgard

1
SLAM is a State Estimation
Problem
 Estimate the map and robot’s pose
 Bayes filter is one tool for state
estimation
 Prediction

 Correction

2
Kalman Filter
 It is a Bayes filter
 Estimator for the linear Gaussian case
 Optimal solution for linear models and
Gaussian distributions

3
Kalman Filter Distribution
 Everything is Gaussian

3D
1D

Courtesy: K. Arras 4
Properties: Marginalization and
Conditioning
 Given

 The marginals are Gaussians

 as well as the conditionals

5
Marginalization
 Given

with

 The marginal distribution is

with
6
Conditioning
 Given

with

 The conditional distribution is

with

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

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
Linear Motion Model
 Motion under Gaussian noise leads to

10
Linear Motion Model
 Motion under Gaussian noise leads to

 describes the noise of the motion

11
Linear Observation Model
 Measuring under Gaussian noise leads
to

12
Linear Observation Model
 Measuring under Gaussian noise leads
to

 describes the measurement noise

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
Kalman Filter Algorithm

15
1D Kalman Filter Example (1)
prediction measurement

correction

It's a weighted mean!

16
1D Kalman Filter Example (2)

prediction

correction
measurement

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

What if this is not the case?

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

19
Linearity Assumption Revisited

Courtesy: Thrun, Burgard, Fox 20

 Non-Linear Function

Non-Gaussian!

Courtesy: Thrun, Burgard, Fox 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
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
EKF Linearization: First Order
Taylor Expansion
 Prediction:

 Correction:
Jacobian matrices

24
Reminder: Jacobian Matrix
 It is a non-square matrix in general

 Given a vector-valued function

 The Jacobian matrix is defined as

25
Reminder: Jacobian Matrix
 It is the orientation of the tangent plane to
the vector-valued function at a given point

Courtesy: K. Arras

 Generalizes the gradient of a scalar valued

function
26
EKF Linearization: First Order
Taylor Expansion
 Prediction:

 Correction:
Linear functions!

27
Linearity Assumption Revisited

Courtesy: Thrun, Burgard, Fox 28

Non-Linear Function

Courtesy: Thrun, Burgard, Fox 29

EKF Linearization (1)

Courtesy: Thrun, Burgard, Fox 30

EKF Linearization (2)

Courtesy: Thrun, Burgard, Fox 31

EKF Linearization (3)

Courtesy: Thrun, Burgard, Fox 32

Linearized Motion Model
 The linearized model leads to

 describes the noise of the motion

33
Linearized Observation Model
 The linearized model leads to

 describes the measurement noise

34
Extended Kalman Filter
Algorithm

KF vs. EKF
35
Extended Kalman Filter
Summary
 Extension of the Kalman filter
 One way to handle the non-linearities
 Performs local linearizations
 Works well in practice for moderate
non-linearities
 Large uncertainty leads to increased
approximation error error

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”
 Tipaldi: “Notes on Univariate
Gaussians and 1D Kalman Filters”
37
Slide Information
 These slides have been created by Cyrill Stachniss as part of
the robot mapping course taught in 2012/13 and 2013/14. I
created this set of slides partially extending existing material
from courses of Wolfram Burgard, Dieter Fox, and myself.
 I tried to acknowledge all people that contributed image or
video material. In case I missed something, please let me
know. If you adapt this course material, please make sure
you keep the acknowledgements.
 Feel free to use and change the slides. If you use them, I
would appreciate an acknowledgement as well. To satisfy my
own curiosity, I appreciate a short email notice in case you
use the material in your course.
 My video recordings are available through YouTube:
http://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_&feature=g-list

Cyrill Stachniss, 2014

[email protected]
bonn.de38