Extract best state estimate and covariance from particles
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
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
stateEst = getStateEstimate(pf)
stateEst = 1×3 4.1562 0.9185 9.0202
stateEst— Best state estimate
Best state estimate, returned as a row vector with
estimate is extracted based on the
stateCov— Corrected system covariance
Corrected system variance, returned as an
matrix, where N is the value of
NumStateVariables property from
pf. The corrected state is
calculated based on the
If you specify a state estimate method that does not
support covariance, then the function returns