This is machine translation

Translated by Microsoft
Mouseover text to see original. Click the button below to return to the English version of the page.

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.

Particle Filter Workflow

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.

Estimation Workflow

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 Particle Filter

Create a ParticleFilter object by calling robotics.ParticleFilter.

Set Parameters of Nonlinear System

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.

Initialize Particles

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

Sample Particles from a Distribution

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.

Predict

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.

Get Measurement

The measurements collected from sensors are used in the next step to correct the current predicted state.

Correct

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.

Extract Best State Estimation

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.

Resample Particles

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.

Continuously Predict and Correct

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.

Estimate Robot Position in a Loop Using Particle Filter

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.

See Also

| | |

Related Examples

More About