M-File Help: ParticleFilter View code for ParticleFilter

ParticleFilter

Particle filter class

Monte-carlo based localisation for estimating vehicle position based on odometry and observations of known landmarks.

Methods

run run the particle filter
plot_xy display estimated vehicle path
plot_pdf display particle distribution

Properties

robot reference to the robot object
sensor reference to the sensor object
history vector of structs that hold the detailed information from each time step
nparticles number of particles used
x particle states; nparticles x 3
weight particle weights; nparticles x 1
x_est mean of the particle population
std standard deviation of the particle population
Q covariance of noise added to state at each step
L covariance of likelihood model
dim maximum xy dimension

Example

Create a landmark map
map = Map(20);
and a vehicle with odometry covariance and a driver
W = diag([0.1, 1*pi/180].^2);
veh = Vehicle(W);
veh.add_driver( RandomPath(10) );
and create a range bearing sensor
R = diag([0.005, 0.5*pi/180].^2);
sensor = RangeBearingSensor(veh, map, R);
For the particle filter we need to define two covariance matrices. The first is is the covariance of the random noise added to the particle states at each iteration to represent uncertainty in configuration.
Q = diag([0.1, 0.1, 1*pi/180]).^2;
and the covariance of the likelihood function applied to innovation
L = diag([0.1 0.1]);
Now construct the particle filter
pf = ParticleFilter(veh, sensor, Q, L, 1000);
which is configured with 1000 particles. The particles are initially uniformly distributed over the 3-dimensional configuration space. We run the simulation for 1000 time steps
pf.run(1000);
then plot the map and the true vehicle path
map.plot();
veh.plot_xy('b');
and overlay the mean of the particle cloud
pf.plot_xy('r');
We can plot the standard deviation against time
plot(pf.std(1:100,:))
The particles are a sampled approximation to the PDF and we can display this as
pf.plot_pdf()

Acknowledgement

Based on code by Paul Newman, Oxford University, http://www.robots.ox.ac.uk/~pnewman

Reference

Robotics, Vision & Control, Peter Corke, Springer 2011

See also

Vehicle, RandomPath, RangeBearingSensor, Map, EKF


ParticleFilter.ParticleFilter

Particle filter constructor

pf = Vehicle, Sensor, RangeBearingSensor, Map


ParticleFilter.plot_pdf

Plot particles as a PDF

PF.

ParticleFilter.plot_xy

Plot vehicle position

PF.plot_xy(ls) as above but the optional line style arguments ls are passed to plot.

ParticleFilter.run

Run the particle filter

PF.run the filter for n time steps.

Notes


 

© 1990-2011 Peter Corke.