predict

Predict state of robot in next time step

Syntax

[statePred,stateCov] = predict(pf)

[statePred,stateCov] = predict(pf,varargin)

Description

[statePred,stateCov] = predict(pf) calculates the predicted system state and its associated uncertainty covariance. predict uses the StateTransitionFcn property of stateEstimatorPF object, pf, to evolve the state of all particles. It then extracts the best state estimate and covariance based on the setting in the StateEstimationMethod property.

[statePred,stateCov] = predict(pf,varargin) passes all additional arguments specified in varargin to the underlying StateTransitionFcn property of pf. The first input to StateTransitionFcn is the set of particles from the previous time step, followed by all arguments in varargin.

Examples

Particle Filter Prediction and Correction

Create a stateEstimatorPF 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 = stateEstimatorPF

pf = 
  stateEstimatorPF with properties:

           NumStateVariables: 3
                NumParticles: 1000
          StateTransitionFcn: @nav.algs.gaussianMotion
    MeasurementLikelihoodFcn: @nav.algs.fullStateMeasurement
     IsStateVariableCircular: [0 0 0]
            ResamplingPolicy: [1x1 resamplingPolicyPF]
            ResamplingMethod: 'multinomial'
       StateEstimationMethod: 'mean'
            StateOrientation: 'row'
                   Particles: [1000x3 double]
                     Weights: [1000x1 double]
                       State: 'Use the getStateEstimate function to see the value.'
             StateCovariance: 'Use the getStateEstimate function to see the value.'

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)

stateEst = 1×3

    4.1562    0.9185    9.0202

Input Arguments

pf — stateEstimatorPF object

handle

stateEstimatorPF object, specified as a handle. See stateEstimatorPF for more information.

varargin — Variable-length input argument list

comma-separated list

Variable-length input argument list, specified as a comma-separated list. This input is passed directly into the StateTransitionFcn property of pf to evolve the system state for each particle. When you call:

predict(pf,arg1,arg2)

MATLAB^® essentially calls the stateTranstionFcn as:

stateTransitionFcn(pf,prevParticles,arg1,arg2)

Output Arguments

statePred — Predicted system state

vector

Predicted system state, returned as a vector with length NumStateVariables. The predicted state is calculated based on the StateEstimationMethod algorithm.

stateCov — Corrected system covariance

N-by-N matrix | []

Corrected system variance, returned as an N-by-N matrix, where N is the value of NumStateVariables property from pf. The corrected state is calculated based on the StateEstimationMethod algorithm and the MeasurementLikelihoodFcn. If you specify a state estimate method that does not support covariance, then the function returns stateCov as [].

Extended Capabilities

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