EEC193A Lab 6: EKF-SLAM
Teja Aluru, Kolin Guo, Jeff Lai and Wenda Xu
Due Date: Friday, November 15, 2019
Introduction
In this week’s lab, you will extend your knowledge of Kalman Filtering to solve
the SLAM problem. You will be doing this in simulation using C++. You
will also have known data associations (i.e. you will have the corresponding
measurements at each time step for each landmark). You will be working in
groups of 3 for this lab.
Docker Setup
You can choose to do this lab locally or on the server, however it is recommended
you run it locally as you will need to visualize a lot of things, and it can be a
hassle to setup X11 forwarding on the server. The only dependency for this lab
that needs to be installed is Python2.7 with matplotlib and numpy.
Using atlas/kronos Server
The docker image is already built on both servers. Use the following command
to create and run your container:
docker run -it --name=_lab6 \
-v :/workspace \
-v /tmp/.X11-unix:/tmp/.X11-unix \
-v ~/.Xauthority:/root/.Xauthority \
-e DISPLAY=$DISPLAY --net=host eec193a-lab6 /bin/bash
Use the following command to restart your stopped container:
docker start -ai _lab6
Using Your Computer
There is a Dockerfile included with the project that installs these dependencies
for you. In the docker directory run
docker build -t eec193a-lab6 .
1
After building the image you will need to setup X11 forwarding on your
local machine. This will be dependent on you computer’s operating system. On
OSX, I had to do the following in terminal after installing Xquartz.
xhost + 127.0.0.1
docker run -it --name=_lab6 \
-v :/workspace \
-e DISPLAY==host.docker.internal:0 eec193a-lab6 /bin/bash
These instructions will change depending on your OS.
Project Structure
Your project consists of three folders, a data folder, an include folder, a src
folder and a Makefile.
• The data folder contains all the sensor data information for the simulation.
• The include folder includes all public headers necessary for the project.
In this case, a public header is any header file that is used in multiple
.cpp files.
• The src folder is where the majority of your programming will happen.
This is the directory that contains the actual implementations for the
interfaces you defined in the headers.
• The Makefile is already completed for you due to courtesy of TAs.
Phase 1: Reading in Sensor and Map Data
In this phase, you must implement the function Mapper::initialize() in
include/mapper.h.
Reading in Sensor Information
The first step for this lab is to read in all the sensor readings from the file
sensor.dat.
The file consists of two types of sensor readings, ODOMETRY and SENSOR.
There is a struct in the file sensor info.h called Record that consists of an
OdoReading and a vector of LaserReading. This represents all the sensor ob-
servations at 1 time step.
In this file there is also a class called MeasurementPackage. This class
consists of a public vector of Record. This represents all the measurements for
each time step. The second is a public function initialize that is used to read
in the information from the file sensor.dat and create the vector of Record.
This will allow you to have access to all the sensor information throughout the
rest of the project.
2
Note: Typically if you have a lightweight implementation for a class you
can throw it directly into the header rather than separating interface and im-
plementation. This is why we implement MeasurementPackage directly in
sensor info.h.
Sensor.dat Structure
There are two possible types of data in sensor.dat. The first is an odometry
reading, and the others are sensor readings. At every time step there will be
one piece of odometry data, and potentially multiple sensor readings. As an
example,
ODOMETRY 0.100692392654 0.100072845247 0.000171392857486
SENSOR 1 1.89645381418 0.374031885671
SENSOR 2 3.85367751107 1.51951017943
For the odometry reading, the first entry represents the first rotation,
the second entry represents the translation and the last entry represents the
second rotation.
For each sensor reading, the first integer is the landmark ID. This is how
you can keep track of each landmark without associating them at every step.
The next entry represents the range of the observed landmark, and the
third entry represents the bearing angle relative to the car.
World.dat Structure
The world.dat file contains the ground truth information for all the simulated
landmarks.
1 2 1
2 0 4
These are the first two entries in the world.dat file. The first entry in each row
is the landmark ID. The last two entries represent the groundtruth (x,y)
coordinate of the landmark.
Reading in Map Information
The map information in world.dat consists of the locations of all simulated
landmarks.
The mapper.h file is where we will implement a class to read in data about
the world. This function is nearly exactly the same as our MeasurementPackage
class that we used to read in sensor information. In this class, there is a struct
called MapPoint which holds all the information needed to read in map data.
The Mapper class is nearly the same as the MeasurementPackage class. You
will need to implement a file reader same as the sensor info.h file.
3
Phase 2: Implementing the EKF
In this phase, you must complete all functions in src/ekfslam.h and src/ekfslam.cpp.
It’s perfectly fine to define your own auxiliary functions besides those given ones.
EKFSLAM Class
You have been provided with an ekfslam.h file that exists within the src
directory. This header file defines all the functions needed in order to implement
EKFSLAM, as well as all the necessary matrices that need to be tracked. At
this point, we will start using the Eigen Library in order to do all the matrix
operations necessary to implement the EKF-SLAM algorithm. Refer to this
tutorial from the official Eigen website to become more familiar with the library.
There are three main functions that need to be implemented for the EKFSLAM
class. The constructor to initialize the mean and covariance matrices/vectors,
the prediction step, and the correction step. We use a class here, as all the
variables for the class will be self-contained within it. A key thing to note, is that
we have the known data associations between time steps for each landmarks.
These are tracked by the landmark IDs. So you will not need to do any data
association for the lab.
Constructor
In order to add noise to the model, we define a noise motion constant in the
constructor. It us up to you as the programmer to both make the proper size
for the noise matrix Q, as well as add the noise due to motion.
Prediction
There is an interface for an odometry model. For the odometry information,
you will use the MeasurementPackage class from before.
Correction
In the correction step, you will use the Mapper class combined with the
MeasurementPackage in order to update the mean and covariances for your
EKF. One thing to note is that observed landmarks is a vector of booleans. This
represents the IDs of the observed landmarks. This is how we can guarantee
that this lab has known data associations for observed landmarks.
Calibrating the Noise Matrices
One very important thing to remember about the Kalman Filter is
that it is a tunable algorithm. One of the main parameters that needs to
be tuned is the modeling of the noise matrices Q and R. This is something that
you will have to play with in order to find a good answer. As part of this lab,
4
you should try and find the optimal values to model the sensor noise and motion
noise. It is very typical for these two values to be something you can’t just find.
You have to go through the work of calibrating your filter to find the optimal
noise for each of your noise and observation models.
Phase 3: Create a Main File
In this phase, you must complete src/main.cpp.
This is the last step when coding the SLAM algorithm, but you need to
combine everything and run it in one singular main file. You must loop through
all available sensor data and you must plot the estimates of each state at every
time step.
You also need to check the number of command-line arguments. Your
main file must take in two and only two arguments, the filepath to the sensor
data and world data. Additionally, if the user does not input any files or an
incorrect number of arguments the program should print usage instructions and
terminate.
If you make your main file properly, you should see a plot that evolves for
each update step. The final step should look like the figure below
Figure 1: EKFSLAM Final Update Step
5
Visualizing the Simulation
To visualize your simulation, you need to plot the landmarks, the robot’s posi-
tion, and then the estimated states of the robot’s position as well as the esti-
mations of the position of each landmark.
You are given the Draw class inside include/plotter.h to help you with
visualization. Specifically, you may find functions Plot State(), Clear(),
Pause() and Show() helpful.
If you can not get X11 forwarding working, I recommend saving images of
the plots by calling Draw::Save().
6
Submission Details
Your Lab6 submission should be a zip file that includes the following.
• The include directory holding all necessary headers
• A src directory with the files ekfslam.cpp, ekfslam.h and main.cpp
• A text file with the names of your partners and their student IDs
Your code needs to be commented and styled according to the Google C++
Style Guide. I recommend downloading a linter in order to ensure your code
follows style. In terms of commenting, please make sure that each function you
have has well-defined inputs and outputs. Each function should have a comment
with the following.
• A description of what the function is doing
• Its Inputs
• Its Outputs (if necessary)
I also recommend commenting on code you feel is too verbose or confusing.
If there is a piece of code that you have written and the TA’s can not understand
what you are doing then it is safe to say it needs a comment. You will be graded
on this.
Grading Breakdown
• EKF-SLAM Implementation: 60 points
• Style and Comments: 10 points
• Interactive Grading: 30 points
7