M-File Help: ParticleFilter View code for ParticleFilter

ParticleFilter

Particle filter class

Monte-carlo based localisation for estimating vehicle pose 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
w0 offset in 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 = ParticleFilter(vehicle, sensor, q, L, np, options) is a particle filter that estimates the state of the vehicle with a sensor sensor. q is covariance of the noise added to the particles at each step (diffusion), L is the covariance used in the sensor likelihood model, and np is the number of particles.

Options

'verbose' Be verbose.
'private' Use private random number stream.
'reset' Reset random number stream.
'seed', S Set the initial state of the random number stream. S must be a proper random number generator state such as saved in the seed0 property of an earlier run.
'nohistory' Don't save history.

Notes

See also

Vehicle, Sensor, RangeBearingSensor, Map


ParticleFilter.char

Convert to string

PF.char() is a string representing the state of the ParticleFilter object in human-readable form.

See also

ParticleFilter.display


ParticleFilter.display

Display status of particle filter object

PF.display() displays the state of the ParticleFilter object in human-readable form.

Notes

See also

ParticleFilter.char


ParticleFilter.init

Initialize the particle filter

PF.init() initializes the particle distribution and clears the history.

Notes


ParticleFilter.plot_pdf

Plot particles as a PDF

PF.plot_pdf() plots a sparse PDF as a series of vertical line segments of height equal to particle weight.


ParticleFilter.plot_xy

Plot vehicle position

PF.plot_xy() plots the estimated vehicle path in the xy-plane.

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(n, options) runs the filter for n time steps.

Options

'noplot' Do not show animation.

Notes


 

© 1990-2012 Peter Corke.