Note: This page has been translated by MathWorks. Click here to see

To view all translated materials including this page, select Country from the country navigator on the bottom of this page.

To view all translated materials including this page, select Country from the country navigator on the bottom of this page.

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.

Create a `ParticleFilter`

object by calling `robotics.ParticleFilter`

.

Modify these `ParticleFilter`

parameters to
fit for your specific system or application:

`StateTransitionFcn`

`MeasurementLikelihoodFcn`

`ResamplingPolicy`

`ResamplingMethod`

`StateEstimationMethod`

Default values for these parameters are given for basic operation.

The `StateTransitionFcn`

and `MeasurementLikelihoodFcn`

functions
define the system behavior and measurement integration. They are vital
for the particle filter to track accurately. For more information,
see Particle Filter Parameters.

Use the `initialize`

method to set the number
of particles and the initial state. See `robotics.ParticleFilter.initialize`

.

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 `StateTransitionFcn`

property.
See `robotics.ParticleFilter.predict`

.

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 `correct`

function. `robotics.ParticleFilter.correct`

uses the `MeasurementLikelihoodFcn`

to
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 `ResamplingMethod`

and `ResamplingPolicy`

properties.

After calling `correct`

, the best state estimate
is automatically extracted based on the `Weights`

of
each particle and the `StateEstimationMethod`

property
specified in `robotics.ParticleFilter`

. The best estimated state and covariance
is output by the `correct`

function.

This step is not separately called, but is executed when you
call `correct`

. Once your state has changed enough,
resample your particles based on the newest estimate. The `correct`

method
checks 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 `predict`

or `correct`

might
be required when:

No measurement is available but control inputs and time updates are occur at a high frequency. Use the

`predict`

method to evolve the particles to get the updated predicted state more often.Multiple measurement reading are available. Use

`correct`

to integrate multiple readings from the same or multiple sensors. The function corrects the state based on each set of information collected.

Use the `ParticleFilter`

object to track a robot as it moves in a 2-D space. The measured position has random noise added. Using `predict`

and `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.

initialize(pf,1000,[0 0],eye(2));

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.

`robotics.ParticleFilter`

| `robotics.ParticleFilter.correct`

| `robotics.ParticleFilter.initialize`

| `robotics.ParticleFilter.predict`