correct

Adjust state estimate based on sensor measurement

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: `pf` – The `stateEstimatorPF` object, which contains the particles of the current iteration`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

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

Version History

Introduced in R2016a