Description
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
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
2 P4: Problems [Due May 6, 2021]
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
Exercise 2. For this exercise, you will implement EKF SLAM in the ekf slam.py file. You
can use the same controller from your previous parts or write a new one. Before integrating
this module with Webots, you can run the script by $ python ekf slam.py to test your
implementation. Feel free to write your own unit-testing scripts. You should not use any
existing Python package that implements EKF. [Hints: remember to wrap heading angles
to [−π, π]]
Check the performance of your controller by running the Webots simulation. You can press
the play button in the top menu to start the simulation in real-time, the fast-forward button
to run the simulation as quickly as possible, and the triple fast-forward to run the simulation
without rendering (any of these options is acceptable, and the faster options may be better
for quick tests). If you complete the track, the scripts will generate a performance plot via
matplotlib. This plot contains a visualization of the car’s trajectory, and also shows the
variation of states with respect to time.
Submit your controller ekf slam.py, ekf slam.py, and the final completion plot as described on the title page. You do not need to submit the plot generated by running the
test script (by running $ python ekf slam.py). Your controller is required to achieve the
following performance criteria to receive full points:
1. Time to complete the loop = 180 s
2. Maximum deviation from the reference trajectory = 8.0 m
3. Average deviation from the reference trajectory = 4.0 m
Debugging tips:
• Do not hardcode the number of map features. Instead, use n in your code.
• Check all the signs carefully.
• Check all the numpy array indexing.
• Use wrap to pi function smartly. Only wrap the angle terms but not the distance
terms.
• When you run $ python ekf slam.py, it is normal if the estimation diverge gradually,
but you should have some reasonable tracking performance.
• Remember the sequence of calling self. compute F() and self. compute H().
5
3 Appendix
(Already covered in P1)
Figure 1: Bicycle model[2]
Figure 2: Tire slip-angle[2]
We will make use of a bicycle model for the vehicle, which is a popular model in the
study of vehicle dynamics. Shown in Figure 1, the car is modeled as a two-wheel vehicle
with two degrees of freedom, described separately in longitudinal and lateral dynamics. The
model parameters are defined in Table 2.
3.1 Lateral dynamics
Ignoring road bank angle and applying Newton’s second law of motion along the y-axis:
may = Fyf cos δf + Fyr
where ay =
d
2
y
dt2
inertial
is the inertial acceleration of the vehicle at the center of geometry
in the direction of the y axis, Fyf and Fyr are the lateral tire forces of the front and rear
6
wheels, respectively, and δf is the front wheel angle, which will be denoted as δ later. Two
terms contribute to ay: the acceleration ¨y, which is due to motion along the y-axis, and the
centripetal acceleration. Hence:
ay = ¨y + ψ˙x˙
Combining the two equations, the equation for the lateral translational motion of the vehicle
is obtained as:
y¨ = −ψ˙x˙ +
1
m
(Fyf cos δ + Fyr)
Moment balance about the axis yields the equation for the yaw dynamics as
ψI¨
z = lfFyf − lrFyr
The next step is to model the lateral tire forces Fyf and Fyr. Experimental results show that
the lateral tire force of a tire is proportional to the “slip-angle” for small slip-angles when
vehicle’s speed is large enough – i.e. when ˙x ≥ 0.5 m/s. The slip angle of a tire is defined
as the angle between the orientation of the tire and the orientation of the velocity vector of
the vehicle. The slip angle of the front and rear wheel is
αf = δ − θV f
αr = −θV r
where θV p is the angle between the velocity vector and the longitudinal axis of the vehicle,
for p ∈ {f, r}. A linear approximation of the tire forces are given by
Fyf = 2Cα
δ −
y˙ + lfψ˙
x˙
!
Fyr = 2Cα
−
y˙ − lrψ˙
x˙
!
where Cα is called the cornering stiffness of the tires. If ˙x < 0.5 m/s, we just set Fyf and
Fyr both to zeros.
3.2 Longitudinal dynamics
Similarly, a force balance along the vehicle longitudinal axis yields:
x¨ = ψ˙y˙ + ax
max = F − Ff
Ff = fmg
where F is the total tire force along the x-axis, and Ff is the force due to rolling resistance
at the tires, and f is the friction coefficient.
7
3.3 Global coordinates
In the global frame we have:
X˙ = ˙x cos ψ − y˙ sin ψ
Y˙ = ˙x sin ψ + ˙y cos ψ
3.4 System equation
Gathering all of the equations, if ˙x ≥ 0.5 m/s, we have:
y¨ = −ψ˙x˙ +
2Cα
m
(cos δ
δ −
y˙ + lfψ˙
x˙
!
−
y˙ − lrψ˙
x˙
)
x¨ = ψ˙y˙ +
1
m
(F − fmg)
ψ¨ =
2lfCα
Iz
δ −
y˙ + lfψ˙
x˙
!
−
2lrCα
Iz
−
y˙ − lrψ˙
x˙
!
X˙ = ˙x cos ψ − y˙ sin ψ
Y˙ = ˙x sin ψ + ˙y cos ψ
otherwise, since the lateral tire forces are zeros, we only consider the longitudinal model.
3.5 Measurements
The observable states are:
y =
x˙
y˙
ψ˙
X
Y
ψ
3.6 Physical constraints
The system satisfies the constraints that:
|δ| 6
π
6
rad
F > 0 and F 6 16000 N
x˙ > 10−5 m/s
8
Table 1: Model parameters.
Name Description Unit Value
( ˙x, y˙) Vehicle’s velocity along the direction of
vehicle frame
m/s State
(X, Y ) Vehicle’s coordinates in the world
frame
m State
ψ, ψ˙ Body yaw angle, angular speed rad,
rad/s
State
δ or δf Front wheel angle rad State
F Total input force N Input
m Vehicle mass kg 4500
lr Length from rear tire to the center of
mass
m 3.32
lf Length from front tire to the center of
mass
m 1.01
Cα Cornering stiffness of each tire N 20000
Iz Yaw intertia kg mˆ2 29526.2
Fpq Tire force, p ∈ {x, y},q ∈ {f, r} N Depends on input force
f Rolling resistance coefficient N/A 0.028
delT Simulation timestep sec 0.032
3.7 Simulation
Figure 3: Simulation code flow
Several files are provided to you within the controllers/main folder. The main.py script
initializes and instantiates necessary objects, and also contains the controller loop. This loop
runs once each simulation timestep. main.py calls your controller.py’s update method
9
on each loop to get new control commands (the desired steering angle, δ, and longitudinal
force, F). The longitudinal force is converted to a throttle input, and then both control
commands are set by Webots internal functions. The additional script util.py contains
functions to help you design and execute the controller. The full codeflow is pictured in
Figure 3.
Please design your controller in the your controller.py file provided for the project part
you’re working on. Specifically, you should be writing code in the update method. Please
do not attempt to change code in other functions or files, as we will only grade the
relevant your controller.py for the programming portion. However, you are free to
add to the CustomController class’s init method (which is executed once when the
CustomController object is instantiated).
3.8 BaseController Background
The CustomController class within each your controller.py file derives from the BaseController class in the base controller.py file. The vehicle itself is equipped with a
Webots-generated GPS, gyroscope, and compass that have no noise or error. These sensors
are started in the BaseController class, and are used to derive the various states of the
vehicle. An explanation on the derivation of each can be found in the table below.
Table 2: State Derivation.
Name Explanation
(X, Y ) From GPS readings
( ˙x, y˙) From the derivative of GPS readings
ψ From the compass readings
ψ˙ From the gyroscope readings
3.9 Trajectory Data
The trajectory is given in buggyTrace.csv. It contains the coordinates of the trajectory as
(x, y). The satellite map of the track is shown in Figure 4.
10
Figure 4: Buggy track[3]
4 Reference
1. Rajamani Rajesh. Vehicle Dynamics and Control. Springer Science & Business Media,
2011.
2. Kong Jason, et al. “Kinematic and dynamic vehicle models for autonomous driving
control design.” Intelligent Vehicles Symposium, 2015.
3. cmubuggy.org, https://cmubuggy.org/reference/File:Course_hill1.png
4. “PID Controller – Manual Tuning.” Wikipedia, Wikimedia Foundation, August 30th,
2020. https://en.wikipedia.org/wiki/PID_controller#Manual_tuning
