M-File Help: ParticleFilter | View code for ParticleFilter |
Particle filter class
Monte-carlo based localisation for estimating vehicle pose based on odometry and observations of known landmarks.
run | run the particle filter |
plot_xy | display estimated vehicle path |
plot_pdf | display particle distribution |
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 |
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()
Based on code by Paul Newman, Oxford University, http://www.robots.ox.ac.uk/~pnewman
Robotics, Vision & Control, Peter Corke, Springer 2011
Vehicle, RandomPath, RangeBearingSensor, Map, EKF
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.
'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. |
Vehicle, Sensor, RangeBearingSensor, Map
Convert to string
PF.char() is a string representing the state of the ParticleFilter object in human-readable form.
Display status of particle filter object
PF.display() displays the state of the ParticleFilter object in human-readable form.
Initialize the particle filter
PF.init() initializes the particle distribution and clears the history.
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.
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.
Run the particle filter
PF.run(n, options) runs the filter for n time steps.
'noplot' | Do not show animation. |
© 1990-2012 Peter Corke.