Exercise 2: Simultaneous Localization and
Mapping using the Extended Kalman Filter
August 11, 2010
1 Introduction
One important aspect of robotic navigation is the ability to fuse multiple sources of
data. In the case of a mobile robot, we might have a number of sensors telling us our
current position. Each of these sensors is subject to noise and errors of various kinds.
Some sources of position data, such as triangulated range and bearing observations to
known targets, are often quite noisy and subject to short term errors. Other position
information, such as odometry, is subject to an accumulation of errors that result from
inaccuracies in our model and noise on the control lines. The fusion of these two
sources of data can, however, give us much better results since one is good over the long
term while the other is fairly reliable for predicting our position over a short distance.
In this exercise, you will implement the Extended Kalman Filter (EKF) for Simultane-
ous Localization and Mapping in Matlab. Large parts of the code are provided, but you
need to implement parts of the transition model (also termed prediction step) and the
sensor model.
The position of the robot and the landmarks in the map are contained in the state vector:
x
k
=
x
v
k
m
1
. . .
m
n
(1)
where x
v
k
is the current vehicle state we are estimating in the prediction stage. m
i
is
the position of landmark i in the map. For clarity x
v
k
can be decomposed as:
x
v
k
= {x
v
k
, y
v
k
, ψ
v
k
}
T
(2)
where x
v
k
and y
v
k
are the x- and y-position of the robot and ψ
v
k
is the orientation of
the robot.
1
