getStateEstimate

Extract best state estimate and covariance from particles

collapse all in page

Syntax

stateEst = getStateEstimate(pf)
[stateEst,stateCov] = getStateEstimate(pf)

Description

example

stateEst = getStateEstimate(pf) returns the best state estimate based on the current set of particles. The estimate is extracted based on the StateEstimationMethod property from the stateEstimatorPF object, pf.

[stateEst,stateCov] = getStateEstimate(pf) also returns the covariance around the state estimate. The covariance is a measure of the uncertainty of the state estimate. Not all state estimate methods support covariance output. In this case, getStateEstimate returns stateCov as [].

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.

Output Arguments

collapse all

Best state estimate, returned as a row vector with length NumStateVariables. The estimate is extracted based on the StateEstimationMethod algorithm specified in pf.

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