Homework 2 (Programming part): EKF and Particle Filter Localization

Assigned: Thursday, Jan 12, 2012
Due: Tuesday, Jan 31, 2012 (noon, start of class)

Note that this is only part of homework 2. See the course website and hw2.pdf.

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 EKF and a particle filter. You will also analyze their performance under various conditions.

Get to know Matlab / Octave

Octave is a free, open source Matlab equivalent (roughly). It is currently installed on CSE machines, and is available for all major operating systems. I have tested the Linux version. Please contact Peter if you are having trouble running octave.

Octave is so similar to Matlab that Matlab tutorials can help you to learn it. Here's a top hit from Google: http://www.math.ufl.edu/help/matlab-tutorial/. And here's a snarky link for Octave: Please show me how to use google :).

Matlab has a lot of useful functions, so don't reinvent the wheel! Need to sum a whole list of numbers (hint, hint)? Use sum! Need to know more about function <function-name>? Type help <function-name>. You will find help to be very helpful. Additionally, randn is a good one to be aware of.

Get the files

The files are accessible here: hw2-revised.tgz. 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. Note that the provided files have changed notably since the assignment was originally given. You should be able to transfer work based on the original files over to these files.

Get to know the files

Here are the descriptions of the files that you'll find in the tarball. 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
sampleOdometry.m -- implements Table 5.6
sample.m -- samples from a covariance matrix
meanAndVariance.m -- returns the mean and variance for a set of non-weighted 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. Observations are landmark detections. Each landmark has a unique ID.

Requirements

In addition to implementing the required EKF and particle filter 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 fixSeed to true. This means 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).
Table and plot of the average normalized estimation error squared (ANEES) over the same range for both filters. See Practical Measures and Test for Credibility of an Estimator . This measures how optimistic or pessimistic your filter is with respect to its estimated uncertainty. (2 tables, 2 plots).
Table and plot of the measurement likelihood (pOfZ) over range for both filters. (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 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 octave environment as soon as possible.