E28: Mobile Robotics - Fall 2015 - HOMEWORK 12

One-dimensional particle filter

Your goal is to implement a particle filter for a simulated boat driving along a circular channel. Let x denote the boat's position in the interval [0, 1]. Since the channel is circular, going past x = 1 results in wrapping back around to zero, and vice-versa.

Motion model. The motion model for the boat is obtained by applying a desired displacement to the current state, and adding Gaussian noise with a fixed standard deviation σ_x = 0.01, before wrapping around:

x_t+1 = wrap(x_t + u_t + ε_t), ε_t ∼ N (0, σ_x)

Measurement model. The depth d(x) of the channel varies according to the function

d(x) = 0.3 + 0.1 cos(2π(x - 0.03)) - 0.06 cos(4π(x - 0.52)) + 0.04 cos(8π(x + 0.88q))

as illustrated here:

so that the shallowest point in the channel lies at approximately x = 0.5 with a depth of roughly d = 0.1. The boat is able to sense the depth of the channel noisily, with additive Gaussian noise that has a known standard deviation σ_z = 0.04:

z_t = d(x_t) + ν_t,      ν_t ∼ N (0, σ_z)

Task Implement a particle filter that begins with a uniformly random sampling of N = 200 particles over the unit interval [0, 1], and which reads control and measurement values u and v from a text file. Your program should output a scatter plot of particle locations after each motion and measurement update, with particle locations on the x axis, and timesteps on the y axis.

There are two datasets of actions and controls in the starter code distribution at http://www.swarthmore.edu/NatSci/mzucker1/e28_f2015/pf_1d.zip named demo_data.txt and assignment_data.txt. Each text file consists of 25 u, v pairs, one per line. The distribution also contains starter code in both Python and MATLAB that you will find useful. When you are implementing the sensor update step, please consider using the np.random.choice function in Python, or the randsample function in MATLAB for re-sampling the particles after weighting them with the PDF of the measurement model.

Start by verifying that your program produces a plot similar to the one below for the demo_data.txt dataset:

Note that in this example, the particle filter very quickly converges on an estimated location of the boat around x = 0.5, moving slowly to the right.

Attachment:- Assignment.zip