M-File Help: ParticleFilter | View code for ParticleFilter |
Particle filter class
Monte-carlo based localisation for estimating vehicle position 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 |
dim | maximum xy dimension |
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()
Vehicle, RandomPath, RangeBearingSensor, Map, EKF
Particle filter constructor
pf = Vehicle, Sensor, RangeBearingSensor, MapPlot particles as a PDF
PF.Plot vehicle position
PF.plot_xy(ls) as above but the optional line style arguments ls are passed to plot.Run the particle filter
PF.run the filter for n time steps.© 1990-2011 Peter Corke.