Correct state and state estimation error covariance using tracking filter and JPDA
returns the corrected state,
Pcorr] = correctjpda(
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
zmeas — Measurements
Measurements, specified as an M-by-N matrix, where M is the dimension of a single measurement, and N is the number of measurements.
jpdacoeffs — Joint probabilistic data association coefficients
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
jpdacoeffs must equal 1.
zcov — Measurement covariance
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
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.
filter is a
then you cannot specify
Suppose you set
and then call
[xcorr,Pcorr] = correctjpda(filter,frame,sensorpos,sensorvel)
correctjpdafunction internally calls the following:
meas = cameas(state,frame,sensorpos,sensorvel)
xcorr — Corrected state
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.
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
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
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
Note that these equations only apply to
are not the exact equations used in other tracking filters.
 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.
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Usage notes and limitations:
correctjpda supports only double-precision code generation, not
Introduced in R2019a