correctjpda

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

Description

[xcorr,Pcorr] = correctjpda(filter,zmeas) 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.

correctjpda(filter,___) updates filter with the corrected state and state estimation error covariance without returning the corrected values. Specify the tracking filter and any of the input argument combinations from preceding syntaxes.

xcorr = correctjpda(filter,___) updates filter with the corrected state and state estimation error covariance but returns only the corrected state, xcorr.

Input Arguments

collapse all

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

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

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

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

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

collapse all

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.

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.

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.

collapse all

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}\left(y-h\left({x}_{k}{}^{-}\right)\right)\\ {P}_{k}{}^{+}={P}_{k}{}^{-}-{K}_{k}{S}_{k}{K}_{k}{}^{T}\end{array}$

where xk and xk+ are the a priori and a posteriori state estimates, respectively, Kk is the Kalman gain, y is the actual measurement, and h(xk) is the predicted measurement. Pk and Pk+ are the a priori and a posteriori state error covariance matrices, respectively. The innovation matrix Sk is defined as

${S}_{k}={H}_{k}{P}_{k}{}^{-}{H}_{k}{}^{T}$

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

In the workflow of a JPDA tracker, the filter needs to process multiple probable measurements yi (i = 1, …, N) with varied probabilities of association βi (i = 0, 1, …, N). Note that β0 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}\sum _{i=1}^{N}{\beta }_{i}\left({y}_{i}-h\left({x}_{k}{}^{-}\right)\right)\\ {P}_{k}{}^{+}={P}_{k}{}^{-}-\left(1-{\beta }_{0}\right){K}_{k}{S}_{k}{K}_{k}{}^{T}+{P}_{k}\end{array}$

where

${P}_{k}={K}_{k}\sum _{i=1}^{N}\left[{\beta }_{i}\left({y}_{i}-h\left({x}_{k}{}^{-}\right)\right){\left({y}_{i}-h\left({x}_{k}{}^{-}\right)\right)}^{T}-\left(\delta y\right){\left(\delta y\right)}^{T}\right]{K}_{k}{}^{T}$

and

$\delta y=\sum _{j=1}^{N}{\beta }_{j}\left({y}_{j}-h\left({x}_{k}{}^{-}\right)\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.