A particle filter is a recursive, Bayesian state estimator that uses discrete particles to approximate the posterior distribution of the estimated state.
The particle filter algorithm computes the state estimate recursively and involves two steps:
Prediction – The algorithm uses the previous state to predict the current state based on a given system model.
Correction – The algorithm uses the current sensor measurement to correct the state estimate.
The algorithm also periodically redistributes, or resamples, the particles in the state space to match the posterior distribution of the estimated state.
The estimated state consists of all the state variables. Each particle represents a discrete state hypothesis. The set of all particles is used to help determine the final state estimate.
You can apply the particle filter to arbitrary nonlinear system models. Process and measurement noise can follow arbitrary non-Gaussian distributions.
To use the particle filter properly, you must specify parameters such as the number of particles, the initial particle location, and the state estimation method. Also, if you have a specific motion and sensor model, you specify these parameters in the state transition function and measurement likelihood function, respectively. For more information, see Particle Filter Parameters.
Follow this basic workflow to create and use a particle filter. This page details the estimation workflow and shows an example of how to run a particle filter in a loop to continuously estimate state.
When using a particle filter, there is a required set of steps to create the particle filter and estimate state. The prediction and correction steps are the main iteration steps for continuously estimating state.
ParticleFilter object by calling
ParticleFilter parameters to
fit for your specific system or application:
Default values for these parameters are given for basic operation.
define the system behavior and measurement integration. They are vital
for the particle filter to track accurately. For more information,
see Particle Filter Parameters.
initialize method to set the number
of particles and the initial state. See
You can sample the initial particle locations in two ways:
Initial pose and covariance — If you have an idea of your initial state, it is recommended you specify the initial pose and covariance. This specification helps to cluster particles closer to your estimate so tracking is more effective from the start.
State bounds — If you do not know your initial state, you can specify the possible limits of each state variable. Particles are uniformly distributed across the state bounds for each variable. Widely distributed particles are not as effective at tracking, because fewer particles are near the actual state. Using state bounds usually requires more particles, computation time, and iterations to converge to the actual state estimate.
Based on a specified state transition function, particles evolve
to estimate the next state. Use
predict to execute
the state transition function specified in the
The measurements collected from sensors are used in the next step to correct the current predicted state.
Measurements are then used to adjust the predicted state and
correct the estimate. Specify your measurements using the
robotics.ParticleFilter.correct uses the
calculate the likelihood of sensor measurements for each particle.
Resampling of particles is required to update your estimation as the
state changes in subsequent iterations. This step triggers resampling
based on the
correct, the best state estimate
is automatically extracted based on the
each particle and the
robotics.ParticleFilter. The best estimated state and covariance
is output by the
This step is not separately called, but is executed when you
correct. Once your state has changed enough,
resample your particles based on the newest estimate. The
ResamplingPolicy for the triggering
of particle resampling according to the current distribution of particles
and their weights. If resampling is not triggered, the same particles
are used for the next estimation. If your state does not vary by much
or if your time step is low, you can call the predict and correct
methods without resampling.
Repeat the previous prediction and correction steps as needed
for estimating state. The correction step determines if resampling
of the particles is required. Multiple calls for
be required when:
No measurement is available but control inputs and
time updates are occur at a high frequency. Use the
to evolve the particles to get the updated predicted state more often.
Multiple measurement reading are available. Use
integrate multiple readings from the same or multiple sensors. The
function corrects the state based on each set of information collected.
ParticleFilter object to track a robot as it moves in a 2-D space. The measured position has random noise added. Using
correct, track the robot based on the measurement and on an assumed motion model.
Initialize the particle filter and specify the default state transition function, the measurement likelihood function, and the resampling policy.
pf = robotics.ParticleFilter; pf.StateEstimationMethod = 'mean'; pf.ResamplingMethod = 'systematic';
Sample 1000 particles with an initial position of [0 0] and unit covariance.
Prior to estimation, define a sine wave path for the dot to follow. Create an array to store the predicted and estimated position. Define the amplitude of noise.
t = 0:0.1:4*pi; dot = [t; sin(t)]'; robotPred = zeros(length(t),2); robotCorrected = zeros(length(t),2); noise = 0.1;
Begin the loop for predicting and correcting the estimated position based on measurements. The resampling of particles occurs based on the
ResamplingPolicy property. The robot moves based on a sine wave function with random noise added to the measurement.
for i = 1:length(t) % Predict next position. Resample particles if necessary. [robotPred(i,:),robotCov] = predict(pf); % Generate dot measurement with random noise. This is % equivalent to the observation step. measurement(i,:) = dot(i,:) + noise*(rand([1 2])-noise/2); % Correct position based on the given measurement to get best estimation. % Actual dot position is not used. Store corrected position in data array. [robotCorrected(i,:),robotCov] = correct(pf,measurement(i,:)); end
Plot the actual path versus the estimated position. Actual results may vary due to the randomness of particle distributions.
plot(dot(:,1),dot(:,2),robotCorrected(:,1),robotCorrected(:,2),'or') xlim([0 t(end)]) ylim([-1 1]) legend('Actual position','Estimated position') grid on
The figure shows how close the estimate state matches the actual position of the robot. Try tuning the number of particles or specifying a different initial position and covariance to see how it affects tracking over time.