Due: End of Semester

The goal of this assignment is to implement a simple Kalman
filter-based localization algorithm.

Your algorithm should work for a holonomic robot operating in the
plane.  The state of the robot is its location in the plane.  You do
not need to consider orientation.  The robot is controlled by
supplying the motion velocity in the plane.

The robot localizes itself by observing the distance (but not
direction) to a list of beacons at a known set of locations.

Implement the following:

  1. Implement a function for performing the prediction step of the
     Kalman filter.  The function should accept the current state mean
     and state covariance, a velocity, a time step, and a covariance
     that is the driving noise.  It should return the mean and
     covariance of the updated state.

  2. Implement a function that, given a state, simulates the
     observation of the beacons.  It should accept a state and a list
     of beacons, and it should return a distance to each beacon.

  3. Implement the update set of the Kalman filter.  The function
     should accept the mean and covariance of the state, the observed
     distances, and the list of points.  Note the observation system
     is nonlinear, so you will have to compute a Jacobian and do an
     EKF update here.

  4. Implement a demonstration of this system that shows the mean and
     covariance of the estimates as the robot drives around a circle.