Description
1 Introduction
In this project, you will design an EKF SLAM to estimate the position and heading of the
vehicle.
In this final part of the project, you will design and implement an Extended Kalman
Filter Simultaneous Localization and Mapping (EKF SLAM). In previous parts,
we assume that all state measurements are available. However, it is not always true in the
real world. Localization information from GPS could be missing or inaccurate in a tunnel,
or closed to tall structures in an urban environment. In this case, we do not have direct
access to the global position X, Y and heading ψ and have to estimate them from ˙x, ˙y, ψ˙
on the vehicle frame and range and bearing measurements of map features. In fact, given
that we will always need CV / LiDAR for obstacle avoidance, we can always use local
measurements to improve the accuracy of our position estimate by augmenting GPS with
landmark information.
Consider the discrete-time dynamics of the system:
Xt+1 = Xt + δtX˙
t + ω
x
t
Yt+1 = Yt + δtY˙
t + ω
y
t
ψt+1 = ψt + δtψ˙
t + ω
ψ
t
(1)
Substitute X˙
t = ˙xt cos ψt − y˙t sin ψt and Y˙
t = ˙xt sin ψt + ˙yt cos ψt
in to (1),
Xt+1 = Xt + δt( ˙xt cos ψt − y˙t sin ψt) + ω
x
t
Yt+1 = Yt + δt( ˙xt sin ψt + ˙yt cos ψt) + ω
y
t
ψt+1 = ψt + δtψ˙
t + ω
ψ
t
,
(2)
with δt the discrete time step. The input ut
is [ ˙xt y˙t ψ˙
t
]
T
. Let pt = [Xt Yt
]
T
. Suppose
you have n map features at global position mj = [mj
x mj
y
]
T
for j = 1, …, n. The ground
truth of these map feature positions are static but unknown, meaning they will not move
but we do not know where they are exactly. However, the vehicle has both range and
bearing measurements relative to these features. The range measurement is defined as the
distance to each feature with the measurement equations y
j
t,distance = kmj − ptk + v
j
t,distance
for j = 1, …, n, with v
j
t,distance representing the measurement noise. The bearing measure is
defined as the angle between the vehicle’s heading (yaw angle) and ray from the vehicle to the
feature with the measurement equations y
j
t,bearing = atan2(mj
y − Yt
, mj
x − Xt) − ψt + v
j
t,bearing
for j = 1, …, n, where v
j
t,bearing is the measurement noise.
2 24-677
Let the state vector be
xt =
Xt
Yt
ψt
m1
x
m1
y
m2
x
m2
y
.
.
.
mn
x
mn
y
(3)
The measurement system be
yt =
km1 − ptk
.
.
.
kmn − ptk
atan2(m1
y − Yt
, m1
x − Xt) − ψt
.
.
.
atan2(mn
y − Yt
, mn
x − Xt) − ψt
+
v
1
t,distance
.
.
.
v
n
t,distance
v
1
t,bearing
.
.
.
v
n
t,bearing
(4)
In class we derived the full equations for the EKF SLAM problem – in this project you
will implement them.
3 24-677
2 P4: Problems
Exercise 1. Before implementing the EKF in the simulator we should practice on some
simple Kalman filter problems. Complete the following using Matlab for all computations.
1. Consider the SISO system
xk+1 =
1 1
0 1
xk +
0
1
wk
yk =
1 0
xk + vk,
with wk a zero mean Gaussian noise with variance 0.1 and vk a zero mean Gaussian
noise with variance 0.01.
Starting from a random initial condition, solve the Kalman filter problem over the first
100 steps. Plot the mean-square current state prediction error vs. k in one graph, and
the estimated states and predicted states on a second graph.
2. A kinematic Kalman filter (KKF) is often used to fuse accelerometer measurements
with encoder readings. The benefit is that the measurements are related purely kinematically, i.e. the plant dynamics do not affect the estimator. Using a zero order hold
approximation to the double integrator kinematics we have
xk+1 =
1 T
0 1
xk +
T
2
2
T
(ak + wk),
where T is the sampling period, ak is the true acceleration at time k, and wk is the
accelerometer noise. We measure the position directly from the encoder which has its
own associated noise.
yk =
1 0
xk + vk,
where vk is the encoder noise which can reasonably approximated by vk =
q
2
12 and q is
the encoder quantization level.
Using a sampling period of 0.002, an accelerometer noise variance of 2.5, and an encoder
noise variance of 2×10−4
, solve the Kalman filter problem over 1 second. Simulate the
system and plot the true position state, position state estimated with the KKF, and
direct encoder position measurement on the same graph.
4 24-677
