predict

Predict state of robot in next time step

collapse all in page

Syntax

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

Description

example

[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

collapse all

Open Live Script

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

collapse all

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

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

collapse all

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

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 [].

See Also

| | | | |

Topics

Introduced in R2016a

Robotics System Toolbox Documentation

Support