CSE 571 - Homework 2: EKF and Particle Filter Localization

Due by: Wednesday, Oct 28, 2015 (1:30 PM, start of class)

Summary:

The key goal of this project is to get an understanding of the properties of Kalman filters and Particle filters for state estimation. You will be implementing an Extended Kalman Filter (EKF) and a Particle Filter (PF) for landmark based localization. You will also analyze their performance under various conditions.

Get to know Matlab/Octave:

Similar to the previous homework, you will be programming in MATLAB. You can also use Octave, a free open source Matlab equivalent (roughly). It is currently installed on CSE machines, and is available for all major operating systems. Here's a snarky link to an Octave tuorial: Please show me how to use google :). Contact Arun if you are having trouble running the code on Octave or Matlab.

Get the code:

The code for this homework is accessible here: HW2-code.zip. Extract, then from within the folder, execute "octave" (or Matlab). You can then type "run(10)" to run the simulation for 10 steps and see some plotted output.

Get to know the files:

Here are the descriptions of the files that you'll find in the zip. You may end up not using every single file. Some are utilities for other files, and you don't really need to bother with them. Some have useful utilities, so you won't have to reinvent the wheel. Some have fuller descriptions in the files themselves.

Things to implement:

run.m        		-- Main update loop, should call ekfUpdate and pfUpdate
ekfUpdate.m 		-- EKF update
pfUpdate.m 		-- Particle filter update
resample.m 		-- Particle filter resampling, called by pfUpdate
runExperiments.m 	-- Useful later for running multiple experiments

Tools:

You should not need to use these files, but look at them if you like:
generateScript.m 	-- Generates data according to initial mean and noise parameters
generateMotion.m 	-- Simulates simple motion commands
You may find these files useful:
prediction.m 		-- Move robot according to specified motion
observation.m 		-- Returns the observation of the specified marker given the current state
sampleOdometry.m 	-- Implements Table 5.6
sample.m 		-- Samples from a covariance matrix 
meanAndVariance.m  	-- Returns mean and variance for a set of unweighted samples (illustrates handling of angles)
getfieldinfo.m 		-- gets field information
minimizedAngle.m 	-- Normalizes an angle to [-pi, pi]
endPoint.m 		-- Returns the location of an observation
noiseFromMotion.m 	-- Get variance based on alphas
matlab.el 		-- Customization file for emacs
Display functions:
plotcircle.m 		-- Draws a circle
plotcov2d.m 		-- Draws a 2-D covariance matrix
plotfield.m 		-- Draws the field with landmarks
plotmarker.m  		-- Draws an 'x' at a specified point (useful for drawing samples)
plotrobot.m 		-- Draws the robot
plotSamples.m 		-- Plots particles from the pf
plotLine.m 		-- Plot a ray (origin, angle, length)

Data format (see run.m and generateScript.m):

State 			: [x,y,theta];
Observation 		: [bearing to landmark, landmark ID];
Control			: [drot1,trans,drot2];

The script generates motion information according to the odometry-based motion model (rotation first, then translation followed by another rotation). Observations are landmark detections. Each landmark has a unique ID. At each timestep, the robot starts from the current state and moves according to the control input. The robot then receives a landmark observation from the world. This information needs to be used by you to localize the robot over the whole time-sequence, by using an EKF or a PF.

Requirements:

In addition to implementing the required EKF and PF functionality, we will ask you to generate a writeup including several plots.

As a way to check that you have implemented the filters correctly, you can set the variable fixSeed to true. This ensures that all random steps will happen the same way on multiple runs.

On Octave, you should get the following values:

EKF:

run(200, false, 0.001, true) -->
meanPositionError =  5.2527
meanMahalanobisError =  1.9864
ANEES =  0.66212
meanPOfZ =  2.1756

PF:

run(200, true, 0.001, true) -->
meanPositionError =  7.1791
meanMahalanobisError =  7.5702
ANEES =  2.5234
meanPOfZ =  2.1626

On MATLAB, because the random number generator works differently, you should get the following values:

EKF:

run(200, false, 0.001, true) -->
meanPositionError = 7.2436
meanMahalanobisError = 3.0394
ANEES = 1.0131
meanPOfZ = 2.1214

PF:

run(200, true, 0.001, true) -->
meanPositionError = 8.2722
meanMahalanobisError = 8.2194
ANEES = 2.7398
meanPOfZ = 2.1447

Please provide a typeset writeup analyzing the performance of your filters under various conditions. The file runExperiments.m can be quite useful for generating these tables and plots. Your writeup must include the following:

  • A plot showing the noise free path, real robot path, and filter path for each filter under the default (provided) parameters. (2 plots total).
  • A table of values and corresponding plots of the mean position error as the alpha and beta factors range over range = [1/64,1/16,1/4,1,4,16,64] (note that this is between 1/8 and 8 times the default noise values) for both filters. This means that one run should be, for example, run(200, false, 0.001, true, false, [1/64,1/64,1/64,1/64]). (2 tables, 2 plots).
  • Tables and plots for both filters of mean position error, ANEES, and pOfZ in which the actual data has the default noise, but the filter noise estimates range over range. For example, one run should be run(200, false, 0.001, true, false, [1,1/64,1,1/64]). (6 tables, 6 plots).
  • Tables and plots of mean position error and ANEES as noise for data + filter vary over range and the number of particles varies over [20,100,300]. For example, one run should be run(200, true, 0.001, true, false, [1/64,1/64,1/64,1/64], 20). Note that you have already produced values for numParticles=100 earlier. (2 additional tables, 2 additional plots).
  • For each set of tables and plots, you should comment on what you see! What trends do you see? Which filters perform better under which conditions?

Hints:

  • Make sure to call minimizedAngle() any time an angle or angle difference could exceed [-pi,pi].
  • Try visualizing the extra covariance matrices returned by the EKF.
  • Turn off plotting for a significant speedup. Enclose all plotting commands within blocks so they can be turned off with the run parameter.
  • It's easy to visualize multiple plots. You can also zoom in on a plot (when it's static, for example when the pause time is negative).
  • Make sure to use the low variance sampler from the textbook / slides. It gives you smoother particle distribution, and also requires only a single random number per resampling step. This will make your runs consistent with the reference implementation.

Get to work:

Don't wait till the last minute for this one, folks. Make sure you have a working MATLAB/Octave environment as soon as possible.