correct

Adjust state estimate based on sensor measurement

Syntax

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

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

Description

[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

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: [1×1 resamplingPolicyPF]
            ResamplingMethod: 'multinomial'
       StateEstimationMethod: 'mean'
            StateOrientation: 'row'
                   Particles: [1000×3 double]
                     Weights: [1000×1 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.

measurement — Sensor measurements

array

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.

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 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

stateCorr — Corrected system state

vector with length NumStateVariables

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

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™.

Version History

Introduced in R2016a