correct

Adjust state estimate based on sensor measurement

collapse all in page

Syntax

[stateCorr,stateCov] = correct(pf,measurement)
[stateCorr,stateCov] = correct(pf,measurement,varargin)

Description

example

[stateCorr,stateCov] = correct(pf,measurement) calculates the corrected system state and its associated uncertainty covariance based on a sensor measurement at the current time step. correct uses the MeasurementLikelihoodFcn property from the particle filter object, pf, as a function to calculate the likelihood of the sensor measurement for each particle. The two inputs to the MeasurementLikelihoodFcn function are:

  1. pf – The stateEstimatorPF object, which contains the particles of the current iteration

  2. measurement – The sensor measurements used to correct the state estimate

The MeasurementLikelihoodFcn function then extracts the best state estimate and covariance based on the setting in the StateEstimationMethod property.

[stateCorr,stateCov] = correct(pf,measurement,varargin) passes all additional arguments in varargin to the underlying MeasurementLikelihoodFcn after the first three required inputs.

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.

Sensor measurements, specified as an array. This input is passed directly into the MeasurementLikelihoodFcn property of pf. It is used to calculate the likelihood of the sensor measurement for each particle.

Variable-length input argument list, specified as a comma-separated list. This input is passed directly into the MeasurementLikelihoodFcn property of pf. It is used to calculate the likelihood of the sensor measurement for each particle. When you call: 

correct(pf,measurement,arg1,arg2)
MATLAB® essentially calls measurementLikelihoodFcn as:
measurementLikelihoodFcn(pf,measurement,arg1,arg2)

Output Arguments

collapse all

Corrected system state, returned as a row vector with length NumStateVariables. The corrected state is calculated based on the StateEstimationMethod algorithm and the MeasurementLikelihoodFcn.

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