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 fil
e 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
