www.gusucode.com > robotics 案例源码程序 matlab代码 > robotics/ParticleFilterPredictionAndCorrectionExample.m

    %% Particle Filter Prediction and Correction
% Create a |ParticleFilter| object, and execute a
% prediction and correction step for state estimation. The particle filter 
% gives a predicted state estimate based on the return value of |StateTransitionFcn|. It 
% then corrects the state based on a given measurement and the return value of 
% |MeasurementLikelihoodFcn|.

%%
% Create a particle filter with the default three states.
pf = robotics.ParticleFilter
%%
% Specify the mean state estimation method and systematic resampling method.
pf.StateEstimationMethod = 'mean';
pf.ResamplingMethod = 'systematic';
%%
% Initialize the particle filter at state [4 1 9] with unit covariance (|eye(3)|). 
% Use 5000 particles.
initialize(pf,5000,[4 1 9],eye(3));
%%
% Assuming a measurement [4.2 0.9 9], run one predict and one correct step.
[statePredicted,stateCov] = predict(pf);
[stateCorrected,stateCov] = correct(pf,[4.2 0.9 9]);
%%
% Get the best state estimate based on the |StateEstimationMethod|
% algorithm.
stateEst = getStateEstimate(pf)