www.gusucode.com > robotalgs 工具箱 matlab源码程序 > robotalgs/+robotics/en/ParticleFilter.m
classdef ParticleFilter< handle
%ParticleFilter Create a particle filter state estimator
% The 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 this state estimate recursively
% and involves two steps, prediction and correction (also known as
% the update step).
%
% The prediction step uses the previous state to predict the current
% state based on a given system model. The correction step uses the
% current sensor measurement to correct the state estimate. The
% algorithm periodically redistributes, or resamples, the particles
% in the state space to match the posterior distribution of the
% estimated state.
% The particle filter can be applied to arbitrary non-linear system
% models and process and measurement noise can follow arbitrary
% non-Gaussian distributions.
%
% The estimated state consists of a number of state variables. Each particle
% represents a discrete state hypothesis. The set of all particles
% approximates the posterior distribution of the estimated state.
%
% PF = robotics.ParticleFilter creates a ParticleFilter
% object PF that enables the state estimation for a simple system
% with 3 state variables. Use the initialize method to initialize the
% particles with a known mean and covariance or uniformly distributed
% within defined state bounds. To customize the particle filter's
% system and measurement models, modify the StateTransitionFcn and
% MeasurementLikelihoodFcn.
%
%
% ParticleFilter properties:
% NumStateVariables - (Read-only) Number of state variables for the particle filter
% NumParticles - (Read-only) Number of particles used in the filter
% StateTransitionFcn - Callback function calculating the state transition
% MeasurementLikelihoodFcn - Callback function calculating the likelihood of sensor measurements
% IsStateVariableCircular - (Read-only) Indicator if state variables have a circular distribution
% ResamplingPolicy - Policy settings that determine when to trigger resampling
% ResamplingMethod - Method used for particle resampling
% StateEstimationMethod - Method used for state estimation
% Particles - Array of particle values
% Weights - Vector of particle weights
%
% ParticleFilter methods:
% initialize - Initialize the state of the particle filter
% predict - Calculate the predicted state in the next time step
% correct - Adjust state estimate based on sensor measurement
% getStateEstimate - Extract the best state estimate and covariance from the particles
% copy - Create a copy of the particle filter
%
%
% Example:
%
% % Create a particle filter with 3 state variables
% pf = robotics.ParticleFilter
%
% % Pick the mean state estimation method and systematic resampling
% pf.StateEstimationMethod = 'mean';
% pf.ResamplingMethod = 'systematic';
%
% % Initialize the particle filter at state [4 1 9] with unit covariance
% % Use 5000 particles
% initialize(pf, 5000, [4 1 9], eye(3));
%
% % Assume we have 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])
%
%
% References:
%
% [1] M.S. Arulampalam, S. Maskell, N. Gordon, T. Clapp, "A tutorial on
% particle filters for online nonlinear/non-Gaussian Bayesian tracking,"
% IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174-188,
% Feb 2002
% [2] Z. Chen, "Bayesian filtering: From Kalman filters to particle filters,
% and beyond," Statistics, vol. 182, no. 1, pp. 1-69, 2003
% Copyright 2015 The MathWorks, Inc.
methods
function out=ParticleFilter
%ParticleFilter Constructor for object
% Please see the class documentation (help robotics.ParticleFilter)
% for more details on different call syntaxes.
end
function copy(in) %#ok<MANU>
%COPY Create a copy of the particle filter
% COBJ = COPY(OBJ) creates a deep copy of the ParticleFilter
% object OBJ and returns it in COBJ. OBJ has to be a scalar
% handle object.
%
% COBJ is an independent handle object that has the same
% property values as OBJ.
end
function correct(in) %#ok<MANU>
%CORRECT Adjust state estimate based on sensor measurement
% [STATECORR, STATECOV] = CORRECT(OBJ, MEASUREMENT) calculates
% the corrected system state STATECORR and its associated
% uncertainty covariance STATECOV based on a sensor
% MEASUREMENT at the current time step.
% CORRECT uses the measurement likelihood model specified in
% MeasurementLikelihoodFcn to calculate the likelihood for
% the sensor measurement for each particle. It then extracts
% the best state estimate and covariance based on the
% setting in StateEstimationMethod.
%
% [STATECORR, STATECOV] = CORRECT(OBJ, MEASUREMENT, VARARGIN)
% passes all additional arguments supplied in VARARGIN to the
% underlying MeasurementLikelihoodFcn. The first two inputs to
% MeasurementLikelihoodFcn are the set of particles from the
% current time step and the MEASUREMENT, followed by all arguments
% in VARARGIN
%
%
% Example:
%
% % Create a particle filter with 5000 particles and initialize it
% pf = robotics.ParticleFilter
% initialize(pf, 5000, [0 0 pi], eye(3), 'CircularVariables', [0 0 1]);
%
% % Run one prediction step
% predict(pf)
%
% % Assume we have a measurement [-1 0 pi]. Run the correction step.
% [stateCorrected, stateCov] = CORRECT(pf, [-1 0 pi])
%
% See also MeasurementLikelihoodFcn, ResamplingMethod.
end
function getStateEstimate(in) %#ok<MANU>
%getStateEstimate Extract the best state estimate and covariance from the particles
% STATEEST = getStateEstimate(OBJ) returns the best state
% estimate STATEEST based on the current set of particles.
% How this state estimate is extracted is determined by the
% StateEstimationMethod algorithm.
%
% [STATEEST, STATECOV] = getStateEstimate(OBJ) also returns
% the covariance STATECOV around the state estimate. This is
% a measure of the uncertainty of the state estimate STATEEST.
% Note that not all state estimation methods support the STATECOV
% output. If a method does not support this output, STATECOV
% is set to [].
%
% See also StateEstimationMethod.
end
function initialize(in) %#ok<MANU>
%INITIALIZE Initialize the state of the particle filter
% INITIALIZE(OBJ, NUMPARTICLES, MEAN, COVARIANCE)
% initializes the particle filter with NUMPARTICLES
% particles. Their initial location in the state space is
% determined by sampling from the multivariate normal
% distribution with the given MEAN and COVARIANCE.
% The number of state variables (NumStateVariables) is
% retrieved automatically based on the length of the MEAN vector.
% The COVARIANCE matrix has a size of
% NumStateVariables-by-NumStateVariables.
%
% INITIALIZE(OBJ, NUMPARTICLES, STATEBOUNDS) determines the
% initial location of NUMPARTICLES particles by sampling from
% the multivariate uniform distribution with the given STATEBOUNDS.
% STATEBOUNDS is an NumStateVariables-by-2 array, with each
% row specifying the sampling limits for one state variable.
% The number of state variables (NumStateVariables) is
% retrieved automatically based on the number of rows of the
% STATEBOUNDS array.
%
% INITIALIZE(___, Name, Value) provides additional options
% specified by one or more Name, Value pair arguments:
%
% 'CircularVariables' -
% Specifies which state variables are described by a
% circular distribution, like angles. This vector needs
% to have a length of NumStateVariables.
%
%
% Example:
% % Create particle filter object
% pf = robotics.ParticleFilter;
%
% % Use 5,000 particles and initialize 2 state variables
% % by sampling from Gaussian with zero mean and covariance of 1.
% INITIALIZE(pf, 5000, [0 0], eye(2))
% pf.Particles
%
% % Use 20,000 particles and initialize 3 state variables
% % by sampling from uniform distribution
% INITIALIZE(pf, 20000, [0 1; -4 1; 10 12])
%
% % Initialize 3 state variables by sampling from uniform
% % distribution. Designate third variable circular.
% INITIALIZE(pf, 20000, [0 1; -4 1; -pi pi], 'CircularVariables', ...
% [0 0 1])
% pf.Particles
end
function initializeResampler(in) %#ok<MANU>
%initializeResampler Initialize the resampler object
% The type of the object is determined by the current setting
% of the ResamplingMethod
end
function initializeStateEstimator(in) %#ok<MANU>
%initializeStateEstimator Initialize the state estimator object
% The type of the object is determined by the current setting
% of the StateEstimationMethod
end
function predict(in) %#ok<MANU>
%PREDICT Calculate the predicted state in the next time step
% [STATEPRED, STATECOV] = PREDICT(OBJ) calculates the
% predicted system state STATEPRED and its associated
% uncertainty covariance STATECOV.
% PREDICT uses the system model specified in
% StateTransitionFcn to evolve the state of all particles and
% then extract the best state estimate and covariance based on the
% setting in StateEstimationMethod.
%
% [STATEPRED, STATECOV] = PREDICT(OBJ, VARARGIN) passes
% all additional arguments supplied in VARARGIN to the
% underlying StateTransitionFcn. The first input to
% StateTransitionFcn is the set of particles from the
% previous time step, followed by all arguments in VARARGIN.
%
%
% Example:
%
% % Create a particle filter with 5000 particles and initialize it
% pf = robotics.ParticleFilter
% initialize(pf, 5000, [4 1 9], eye(3));
%
% % Run one prediction step
% [statePredicted, stateCov] = PREDICT(pf)
%
% See also StateTransitionFcn.
end
function resample(in) %#ok<MANU>
%RESAMPLE Resample the current set of particles
% Resampling is only executed if the ResamplingPolicy
% verifies that a resampling trigger has been reached.
end
function sampleGaussian(in) %#ok<MANU>
%sampleGaussian Sample the multivariate Gaussian distribution with given mean and covariance
% This function assigns initial values to the particles.
end
function sampleUniform(in) %#ok<MANU>
%sampleUniform Sample uniformly within the given state bounds
% This function assigns initial values to the particles.
end
end
methods (Abstract)
end
properties
%InternalIsStateVariableCircular - Internal storage for circular variable setting
InternalIsStateVariableCircular;
%InternalNumParticles - Internal storage for number of particles
% This is user-exposed through the NumParticles property.
InternalNumParticles;
%InternalNumStateVariables - Internal storage for number of state variables
% This is user-exposed through the NumStateVariables property.
InternalNumStateVariables;
%InternalParticles - Internal storage for particle values
InternalParticles;
%InternalResamplingMethod - Internal storage for resampling method string
InternalResamplingMethod;
%InternalStateEstimationMethod - Internal storage for state estimation string
InternalStateEstimationMethod;
%InternalWeights - Internal storage for particle weights
InternalWeights;
%IsStateVariableCircular - Indicator if state variables have a circular distribution
% The probability density function of a circular state variable
% takes on angular values in the range [-pi,pi].
IsStateVariableCircular;
%MeasurementLikelihoodFcn - Callback function calculating the likelihood of sensor measurements
% Once a sensor measurement is available, this callback function
% calculates the likelihood that the measurement is consistent
% with the state hypothesis of each particle.
%
% The callback function should accept at least three input arguments.
% The first argument PF is the associated ParticleFilter object.
% The second argument is the set of particles PREDICTPARTICLES that
% represent the predicted system state at the current time step
% (NumParticles-by-NumStateVariables array),
% MEASUREMENT is the state measurement at the current time step.
% Additional input arguments can be provided with VARARGIN (these
% are passed through to the correct function).
% The callback needs to return exactly one output,
% LIKELIHOOD, a vector with NumParticles length, which is the
% likelihood of the given MEASUREMENT for each particle state
% hypothesis.
%
% The function signature is as follows:
%
% function LIKELIHOOD = measurementLikelihoodFcn(PF, PREDICTPARTICLES, MEASUREMENT, VARARGIN)
%
% See also correct.
MeasurementLikelihoodFcn;
%NumParticles - Number of particles used in the filter
% Each particle represents a state hypothesis.
NumParticles;
%NumStateVariables - Number of state variables for the particle filter
% The state is comprised of this number of state
% variables.
NumStateVariables;
%Particles - Array of particle values
% This is a NumParticles-by-NumStateVariables array. Each row
% corresponds to the state hypothesis of a single particle.
Particles;
%ResamplingMethod - Method used for particle resampling
% Possible choices are 'multinomial', 'systematic', 'stratified',
% and 'residual'.
%
% Default: 'multinomial'
ResamplingMethod;
%ResamplingPolicy - Policy settings that determine when to trigger resampling
% The resampling can be triggered either at fixed intervals or
% dynamically based on the number of effective particles.
ResamplingPolicy;
%StateEstimationMethod - Method used for state estimation
% Possible choices are 'mean', 'maxweight'.
%
% Default: 'mean'
StateEstimationMethod;
%StateTransitionFcn - Callback function calculating the state transition
% The state transition function evolves the system state for each
% particle.
%
% The callback function should accept at least two input arguments.
% The first argument PF is the associated ParticleFilter object.
% The second argument is the set of particles PREVPARTICLES that
% represent the system state at the previous time step
% (NumParticles-by-NumStateVariables array).
% Additional input arguments can be provided with VARARGIN (these
% are passed to the predict function).
% The callback needs to return exactly one output,
% PREDICTPARTICLES, which is the set of predicted particle locations
% for the current time step (NumParticles-by-NumStateVariables array).
%
% The function signature is as follows:
%
% function PREDICTPARTICLES = stateTransitionFcn(PF, PREVPARTICLES, VARARGIN)
%
% See also predict.
StateTransitionFcn;
%Weights - Vector of particle weights
% This is a NumParticles-by-1 vector of particle weights. Each
% weight is associated with the particle in the same row in
% Particles.
Weights;
end
end