M-File Help: ParticleFilter

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 landmark sensor sensor. q is the 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.
'x0' | Initial particle states (Nx3)

## Notes

- ParticleFilter subclasses Handle, so it is a reference object.
- If initial particle states not given they are set to a uniform distribution over the map, essentially the kidnapped robot problem which is quite unrealistic.
- Initial particle weights are always set to unity.
- The 'private' option creates a private random number stream for the methods rand, randn and randi. If not given the global stream is used.

## 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

- This method is invoked implicitly at the command line when the result of an expression is a ParticleFilter object and the command has no trailing semicolon.

## See also

ParticleFilter.char

# ParticleFilter.init

Initialize the particle filter

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

## Notes

- If initial particle states were given to the constructor the states are set to this value, else a random distribution over the map is used.
- Invoked by the run() method.

# 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

- All previously estimated states and estimation history is cleared.

© 1990-2014 Peter Corke.