correctjpda

Correct state and state estimation error covariance using tracking filter and JPDA

Syntax

[xcorr,Pcorr] = correctjpda(filter,zmeas,jpdacoeffs)

[xcorr,Pcorr] = correctjpda(filter,zmeas,jpdacoeffs,measparams)

[xcorr,Pcorr] = correctjpda(filter,zmeas,jpdacoeffs,zcov)

[xcorr,Pcorr,zcorr] = correctjpda(filter,zmeas,jpdacoeffs)

[xcorr,Pcorr,zcorr] = correctjpda(filter,zmeas,jpdacoeffs,zcov)

Description

[xcorr,Pcorr] = correctjpda(filter,zmeas,jpdacoeffs) returns the corrected state, xcorr, and the corrected state estimation error covariance, Pcorr, for the next time step of the input tracking filter. The corrected values are based on a set of measurements, zmeas, and their joint probabilistic data association coefficients, jpdacoeffs. These values overwrite the internal state and state estimation error covariance of filter.

[xcorr,Pcorr] = correctjpda(filter,zmeas,jpdacoeffs,measparams) specifies additional parameters used by the measurement function that is defined in the MeasurementFcn property of the tracking filter object.

If filter is a trackingKF or trackingABF object, then you cannot use this syntax.

[xcorr,Pcorr] = correctjpda(filter,zmeas,jpdacoeffs,zcov) specifies additional measurement covariance, zcov, used in the MeasurementNoise property of filter.

You can use this syntax only when filter is a trackingKF object.

[xcorr,Pcorr,zcorr] = correctjpda(filter,zmeas,jpdacoeffs) also returns the correction of measurements, zcorr.

You can use this syntax only when filter is a trackingABF object.

[xcorr,Pcorr,zcorr] = correctjpda(filter,zmeas,jpdacoeffs,zcov) returns the correction of measurements, zcorr, and also specifies additional measurement covariance, zcov, used in the MeasurementNoise property of filter.

You can use this syntax only when filter is a trackingABF object.

Input Arguments

filter — Filter for object tracking

trackingKF object | trackingEKF object | trackingUKF object | trackingABF object | trackingCKF object | trackingIMM object | trackingGSF object | trackingPF object | trackingMSCEKF object

Filter for object tracking, specified as one of these objects:

• trackingKF — Linear Kalman filter

• trackingEKF — Extended Kalman filter

• trackingUKF — Unscented Kalman filter

• trackingABF — Alpha-beta filter

• trackingCKF — Cubature Kalman filter

• trackingIMM — Interacting multiple model (IMM) filter

• trackingGSF — Gaussian-sum filter

• trackingPF — Particle filter

• trackingMSCEKF — Extended Kalman filter using modified spherical coordinates (MSC)

zmeas — Measurements

M-by-N matrix

Measurements, specified as an M-by-N matrix, where M is the dimension of a single measurement, and N is the number of measurements.

Data Types: single | double

jpdacoeffs — Joint probabilistic data association coefficients

(N+1)-element vector

Joint probabilistic data association coefficients, specified as an (N+1)-element vector. The ith (i = 1, …, N) element of jpdacoeffs is the joint probability that the ith measurement in zmeas is associated with the filter. The last element of jpdacoeffs corresponds to the probability that no measurement is associated with the filter. The sum of all elements of jpdacoeffs must equal 1.

Data Types: single | double

zcov — Measurement covariance

M-by-M matrix

Measurement covariance, specified as an M-by-M matrix, where M is the dimension of the measurement. The same measurement covariance matrix is assumed for all measurements in zmeas.

Data Types: single | double

measparams — Measurement parameters

comma-separated list of arguments

Measurement function arguments, specified as a comma-separated list of arguments. These arguments are the same ones that are passed into the measurement function specified by the MeasurementFcn property of the tracking filter. If filter is a trackingKF or trackingABF object, then you cannot specify measparams.

Suppose you set MeasurementFcn to @cameas, and then call correctjpda:

[xcorr,Pcorr] = correctjpda(filter,frame,sensorpos,sensorvel)

The correctjpda function internally calls the following:

meas = cameas(state,frame,sensorpos,sensorvel)

Output Arguments

xcorr — Corrected state

P-element vector

Corrected state, returned as a P-element vector, where P is the dimension of the estimated state. The corrected state represents the a posteriori estimate of the state vector, taking into account the current measurements and their associated probabilities.

Pcorr — Corrected state error covariance

positive-definite P-by-P matrix

Corrected state error covariance, returned as a positive-definite P-by-P matrix, where P is the dimension of the state estimate. The corrected state covariance matrix represents the a posteriori estimate of the state covariance matrix, taking into account the current measurements and their associated probabilities.

zcorr — Corrected measurements

M-by-N matrix

Corrected measurements, returned as an M-by-N matrix, where M is the dimension of a single measurement, and N is the number of measurements. You can return zcorr only when filter is a trackingABF object.

More About

JPDA Correction Algorithm for Discrete Extended Kalman Filter

In the measurement update of a regular Kalman filter, the filter usually only needs to update the state and covariance based on one measurement. For instance, the equations for measurement update of a discrete extended Kalman filter can be given as

$$\begin{array}{l}{x}_k{}^{+}=x_k{}^{-}+K_k(y-h(x_k{}^{-}))\\ P_k{}^{+}=P_k{}^{-}-K_k{S}_k{K}_k{}^T\end{array}$$

where x_k⁻ and x_k⁺ are the a priori and a posteriori state estimates, respectively, K_k is the Kalman gain, y is the actual measurement, and h(x_k⁻) is the predicted measurement. P_k⁻ and P_k⁺ are the a priori and a posteriori state error covariance matrices, respectively. The innovation matrix S_k is defined as

$$S_k=H_k{P}_k{}^{-}{H}_k{}^T$$

where H_k is the Jacobian matrix for the measurement function h.

In the workflow of a JPDA tracker, the filter needs to process multiple probable measurements y_i (i = 1, …, N) with varied probabilities of association β_i (i = 0, 1, …, N). Note that β₀ is the probability that no measurements is associated with the filter. The measurement update equations for a discrete extended Kalman filter used for a JPDA tracker are

$$\begin{array}{l}{x}_k{}^{+}=x_k{}^{-}+K_k{\displaystyle \sum _{i=1}^N{\beta }_i\left(y_i-h(x_k{}^{-})\right)}\\ P_k{}^{+}=P_k{}^{-}-(1-{\beta }_0)K_k{S}_k{K}_k{}^T+P_k\end{array}$$

where

$$P_k=K_k{\displaystyle \sum _{i=1}^N\left[{\beta }_i\left(y_i-h(x_k{}^{-})\right){\left(y_i-h(x_k{}^{-})\right)}^T-\left(\delta y\right){\left(\delta y\right)}^T\right]}{K}_k{}^T$$

and

$$\delta y={\displaystyle \sum _{j=1}^N{\beta }_j\left(y_j-h(x_k{}^{-})\right)}$$

Note that these equations only apply to trackingEKF and are not the exact equations used in other tracking filters.

References

[1] Fortmann, T., Y. Bar-Shalom, and M. Scheffe. "Sonar Tracking of Multiple Targets Using Joint Probabilistic Data Association." IEEE Journal of Ocean Engineering. Vol. 8, Number 3, 1983, pp. 173–184.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

correctjpda supports only double-precision code generation, not single-precision.

Version History

Introduced in R2019a