trackingPF

Particle filter for object tracking

Description

The trackingPF object represents an object tracker that follows a nonlinear motion model or that is measured by a nonlinear measurement model. The filter uses a set of discrete particles to approximate the posterior distribution of the state. The particle filter can be applied to arbitrary nonlinear system models. The process and measurement noise can follow an arbitrary non-Gaussian distribution.

The particles are generated using various resampling methods defined by ResamplingMethod.

Creation

Syntax

pf = trackingPF

pf = trackingPF(transitionFcn,measuremntFcn,state)

pf = trackingPF(___,Name,Value)

Description

pf = trackingPF returns a trackingPF object with state transition function, @constvel, measurement function, @cvmeas, and a distribution of particles around the state, [0;0;0;0], with unit covariance in each dimension. The filter assumes an additive Gaussian process noise model and Gaussian likelihood calculations.

pf = trackingPF(transitionFcn,measuremntFcn,state) specifies the StateTransitionFcn, MeasurementFcn, and State properties directly. The filter assumes a unit covariance around the state.

pf = trackingPF(___,Name,Value) specifies the properties of the particle filter using one or more Name,Value pair arguments. Any unspecified properties take default values.

Properties

State — Current filter state

real-valued M-element vector

This property is read-only.

Current filter state, specified as a real-valued M-element vector. The current state is calculated from Particles and Weight using the specified StateEstimationMethod. M is the NumStateVariables. StateOrientation determines if the state is given as a row or column vector.

Example: [0.1;0.05;0.04;-0.01]

Data Types: double

StateCovariance — State estimation error covariance

M-by-M matrix

This property is read-only.

State error covariance, specified as an M-by-M matrix, where M is the size of the filter state. The current state covariance is calculated from Particles and Weight using the specified StateEstimationMethod. M is the NumStateVariables. The covariance matrix represents the uncertainty in the filter state.

IsStateVariableCircular — Indicates if state variables have circular distribution

[0 0 0 0] (default) | M-element vector of zeros and ones

This property is read-only.

Indicates if state variables have circular distribution, specified as an M-element vector of zeros and ones. Values of 1 indicate it does have a circular distribution. The probability density function of a circular variable takes on angular values in the range [-pi,pi].

StateOrientation — Orientation of state vector

'column' (default) | 'row'

Orientation of state vector, specified as 'column' or 'row'.

Note

If you set the orientation to 'row', the default StateTransitionFcn and MeasurementFcn are not supported. All state transition functions and measurement functions provided (constvel and cvmeas, for example) assume a 'column' orientation.

StateTransitionFcn — State transition function

@constvel (default) | function handle

State transition function, specified as a function handle. The state transition function evolves the system state from each particle. The callback function accepts at least one input argument, prevParticles, that represents the system at the previous time step. If StateOrientation is 'row', the particles are input as a NumParticles-by-NumStateVariables array. If StateOrientation is 'column', the particles are input as a NumStateVariables-by-NumParticles array.

Additional input arguments can be provided with varargin, which are passed to the predict function. The function signature is:

function predictParticles = stateTransitionFcn(prevParticles,varargin)

When the HasAdditiveProcessNoise property of the filter is false, the state transition function can accept an additional input argument, dt. For example:

function predictParticles = stateTransitionFcn(prevParticles,dt,varargin)

dt is the time step of the trackingPF filter, filter, that was specified in the most recent call to the predict function. The dt argument applies when you use the filter within a tracker and call the predict function with the filter to predict the state of the tracker at the next time step. For the nonadditive process noise case, the tracker assumes that you explicitly specify the time step by using this syntax: predict(filter,dt)

Dependencies

This parameter depends on the StateOrientation property.

Data Types: function_handle

ProcessNoiseSamplingFcn — Function to generate noise sample for each particle

@gaussianSampler (default) | function handle

Function to generate noise sample for each particle, specified as a function handle. The function signature is:

function noiseSample = processNoiseSamplingFcn(pf)

• When HasAdditiveProcessNoise is false, this function outputs a noise sample as a W-by-N matrix, where W is the number of process noise terms, and N is the number of particles.

• When HasAdditiveProcessNoise is true, this function outputs a noise sample as an M-by-N matrix, where M is the number of state variables, and N is the number of particles.

To generate a sample from a non-Gaussian distribution, use this property with a custom function handle.

Dependencies

This parameter depends on the HasAdditiveProcessNoise property.

Data Types: function_handle

ProcessNoise — Process noise covariance

1 (default) | positive real-valued scalar | positive-definite real-valued matrix

Process noise covariance:

• When HasAdditiveProcessNoise is true, specify the process noise covariance as a scalar or a positive definite real-valued M-by-M matrix. M is the dimension of the state vector. When specified as a scalar, the matrix is a multiple of the M-by-M identity matrix.

• When HasAdditiveProcessNoise is false, specify the process noise covariance as a Q-by-Q matrix. Q is the size of the process noise vector.

  Specify ProcessNoise before any call to the predict function. In later calls to predict, you can optionally specify the process noise as a scalar. In this case, the process noise matrix is a multiple of the Q-by-Q identity matrix.

If ProcessNoiseSamplingFcn is specified as @gaussianSample, this property defines the Gaussian noise covariance of the process.

Example: [1.0 0.05; 0.05 2]

Dependencies

This parameter depends on the HasAdditiveProcessNoise property.

HasAdditiveProcessNoise — Model additive process noise

true (default) | false

Option to model processes noise as additive, specified as true or false. When this property is true, process noise is added to the state vector. Otherwise, noise is incorporated into the state transition function.

MeasurementFcn — Measurement model function

@cvmeas (default) | function handle

Measurement model function, specified as a function handle. This function calculates the measurements given the current particles' state. Additional input arguments can be provided with varargin. The function signature is:

function predictedParticles = measurementFcn(particles,varargin)

Data Types: function_handle

MeasurementLikelihoodFcn — Callback function calculating the likelihood of sensor measurements

@gaussianLikelihood (default) | function handle

Callback function calculating the likelihood of sensor measurements, specified as a function handle. 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 accepts at least three input arguments, pf, predictedParticles, and measurement. There are two function signatures:

function likelihood = measurementLikelihoodFcn(pf,predictedParticles,measurement,varargin)

function [likelihood,distance] = measurementLikelihoodFcn(pf,predictedParticles,measurement,varargin)

pf is the particle filter object.

predictedParticles represents the set of particles returned from MeasurementFcn. If StateOrientation is 'row', the particles are input as a NumParticles-by-NumStateVariables array. If StateOrientation is 'column', the particles are input as a NumStateVariables-by-NumParticles array.

measurement is the state measurement at the current time step.

varargin allows you to specify additional inputs to the correct function.

The callback output, likelihood, is a vector of length NumParticles, which is the likelihood of the given measurement for each particle state hypothesis.

The optional output, distance, allows you to specify the distance calculations returned by the distance function.

Data Types: function_handle

MeasurementNoise — Measurement noise covariance

1 (default) | positive scalar | positive-definite real-valued matrix

Measurement noise covariance, specified as a positive scalar or positive-definite real-valued matrix. When specified as a scalar, the matrix is a multiple of the N-by-N identity matrix. N is the size of the measurement vector.

If MeasurementLikelihoodFcn is specified as @gaussianLikelihood, this property is used to specify the Gaussian noise covariance of the measurement.

Example: 0.2

Particles — State hypothesis of each particle

matrix

State hypothesis of each particle, specified as a matrix. If StateOrientation is 'row' the particles are a NumParticles-by-NumStateVariables array. If StateOrientation is 'column', the particles are a NumStateVariables-by-NumParticles array.

Each row or column corresponds to the state hypothesis of a single particle.

Data Types: double

Weights — Particle weights

ones(1,NumParticles) (default) | vector

Particle weights, specified as a vector. The vector is either a row or column vector based on StateOrientation. Each row or column is the weight associated with the same row or column in Particles.

Data Types: double

NumStateVariables — Number of state variables

4 (default) | integer

Number of state variables, specified as an integer. The State is comprised of this number of state variables.

NumParticles — Number of particles used

1000 (default) | integer

Number of particles used by the filter, specified as an integer. Each particle represents a state hypothesis.

ResamplingPolicy — Policy settings for triggering resampling

trackingResamplingPolicy object

Policy settings for triggering resampling, specified as a trackingResamplingPolicy object. The resampling can be triggered either at fixed intervals or dynamically based on the number of effective particles.

ResamplingMethod — Method used for particle resampling

'multinomial' (default) | 'systematic' | 'stratified' | 'residual'

Method used for particle resampling, specified as 'multinomial', 'systematic', 'stratified', or 'residual'.

StateEstimationMethod — Method used for state estimation

'mean' (default) | 'maxweight'

Method used for state estimation, specified as 'mean' or 'maxweight'.

Object Functions

predict	Predict state and state estimation error covariance of tracking filter
correct	Correct state and state estimation error covariance using tracking filter
correctjpda	Correct state and state estimation error covariance using tracking filter and JPDA
distance	Distances between current and predicted measurements of tracking filter
likelihood	Likelihood of measurement from tracking filter
clone	Create duplicate tracking filter
initialize	Initialize state and covariance of tracking filter

Examples

Run trackingPF Filter

This example shows how to create and run a trackingPF filter. Call the predict and correct functions to track an object and correct the state estimate based on measurements.

Create the filter. Specify the initial state and state covariance. Specify the number of particles and that there is additive process noise.

state = [0;0;0;0];
stateCov = 10*eye(4);
pf = trackingPF(@constvel,@cvmeas,state,'StateCovariance',stateCov,...
    'NumParticles',2500,'HasAdditiveProcessNoise',true);

Call predict to get the predicted state and covariance of the filter. Use a 0.5 sec time step.

[xPred,pPred] = predict(pf,0.5);

You can also modify the particles in the filter to carry a multi-model state hypothesis. Modify the Particle property with particles around multiple states after initialization.

state1 = [0;0;0;0];
stateCov1 = 10*eye(4);
state2 = [100;0;100;0];
stateCov2 = 10*eye(4);

pf.Particles(:,1:1000) = (state1 + chol(stateCov1)*randn(4,1000));
pf.Particles(:,1001:2000) = (state2 + chol(stateCov2)*randn(4,1000));

Call correct with a given measurement.

meas = [1;1;0];
[xCorr,pCorr] = correct(pf,meas);

Continue to predict the filter state. Specify the desired time step in seconds if necessary.

[xPred,pPred] = predict(pf);         % Predict over 1 second
[xPred,pPred] = predict(pf,2);       % Predict over 2 seconds

References

[1] Arulampalam, M.S., S. Maskell, N. Gordon, and T. Clapp. "A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking." IEEE Transactions on Signal Processing. Vol. 50, No. 2, Feb 2002, pp. 174-188.

[2] Chen, Z. "Bayesian Filtering: From Kalman Filters to Particle Filters, and Beyond." Statistics. Vol. 182, No. 1, 2003, pp. 1-69.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2018b

R2022a: trackingPF assumes row-alignment measurements when measurement matrix is square

As of R2022a, when you use the correct object function of the trackingPF filter object to correct the filter state using a matrix of measurements, the object assumes each row of the matrix is a measurement if the matrix is a square. Previously, the object assumes each column of the square matrix is a measurement.