Main Content

fusecovint

Covariance fusion using covariance intersection

collapse all in page

Syntax

[fusedState,fusedCov] = fusecovint(trackState,trackCov)
[fusedState,fusedCov] = fusecovint(trackState,trackCov,minProp)

Description

[fusedState,fusedCov] = fusecovint(trackState,trackCov) fuses the track states in trackState and their corresponding covariance matrices trackCov. The function computes the fused state and covariance as an intersection of the individual covariances. It creates a convex combination of the covariances and finds weights that minimize the determinant of the fused covariance matrix.

example

[fusedState,fusedCov] = fusecovint(trackState,trackCov,minProp) estimates the fused covariance by minimizing minProp, which can be either the determinant or the trace of the fused covariance matrix.

example

Examples

collapse all

Open Live Script

Define a state vector of tracks.

x(:,1) = [1;2;0];
x(:,2) = [2;2;0];
x(:,3) = [2;3;0];

Define the covariance matrices of the tracks.

p(:,:,1) = [10 5 0; 5 10 0; 0 0 1];
p(:,:,2) = [10 -5 0; -5 10 0; 0 0 1]; 
p(:,:,3) = [12 9 0; 9 12 0; 0 0 1];

Estimate the fused state vector and its covariance.

[fusedState,fusedCov] = fusecovint(x,p);

Use trackPlotter to plot the results.

tPlotter = theaterPlot('XLim',[-10 10],'YLim',[-10 10],'ZLim',[-10 10]);
tPlotter1 = trackPlotter(tPlotter, ...
    'DisplayName','Input Tracks','MarkerEdgeColor',[0.000 0.447 0.741]);
tPlotter2 = trackPlotter(tPlotter,'DisplayName', ...
    'Fused Track','MarkerEdgeColor',[0.850 0.325 0.098]);
plotTrack(tPlotter1,x',p)
plotTrack(tPlotter2,fusedState',fusedCov)
title('Covariance Intersection Fusion')

Figure contains an axes object. The axes object with title Covariance Intersection Fusion, xlabel X (m), ylabel Y (m) contains 2 objects of type line. One or more of the lines displays its values using only markers These objects represent Input Tracks, Fused Track.

Open Live Script

Define a state vector of tracks. 

x(:,1) = [1;2;0];
x(:,2) = [2;2;0];
x(:,3) = [2;3;0];

Define the covariance matrices of the tracks.

p(:,:,1) = [10 5 0; 5 10 0; 0 0 1];
p(:,:,2) = [10 -5 0; -5 10 0; 0 0 1]; 
p(:,:,3) = [12 9 0; 9 12 0; 0 0 1];

Estimate the fused state vector and its covariance. Combine the original covariances so that the trace of the fused covariance matrix is minimized.

[fusedState,fusedCov] = fusecovint(x,p,'trace');

Use trackPlotter to plot the results.

tPlotter = theaterPlot('XLim',[-10 10],'YLim',[-10 10],'ZLim',[-10 10]);
tPlotter1 = trackPlotter(tPlotter, ...
    'DisplayName','Input Tracks','MarkerEdgeColor',[0.000 0.447 0.741]);
tPlotter2 = trackPlotter(tPlotter, ...
    'DisplayName','Fused Track','MarkerEdgeColor',[0.850 0.325 0.098]);
plotTrack(tPlotter1,x',p)
plotTrack(tPlotter2,fusedState',fusedCov)
title('Covariance Intersection Fusion')

Figure contains an axes object. The axes object with title Covariance Intersection Fusion, xlabel X (m), ylabel Y (m) contains 2 objects of type line. One or more of the lines displays its values using only markers These objects represent Input Tracks, Fused Track.

Input Arguments

collapse all

Track states, specified as an N-by-M matrix, where N is the dimension of the state and M is the number of tracks.

Data Types: single | double

Track covariance matrices, specified as an N-by-N-by-M array, where N is the dimension of the state and M is the number of tracks.

Data Types: single | double

Property to minimize when estimating the fused covariance, specified as 'det' or 'trace'.

.

Data Types: char | string

Output Arguments

collapse all

Fused state, returned as an N-by-1 vector, where N is the dimension of the state.

Fused covariance matrix, returned as an N-by-N matrix, where N is the dimension of the state.

References

[1] Matzka, Stephan, and Richard Altendorfer. "A comparison of track-to-track fusion algorithms for automotive sensor fusion." In Multisensor Fusion and Integration for Intelligent Systems, pp. 69-81. Springer, Berlin, Heidelberg, 2009.

[2] Julier, Simon, and Jeffrey K. Uhlmann. "General decentralized data fusion with covariance intersection." In Handbook of multisensor data fusion, pp. 339-364. CRC Press, 2017.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2018b

See Also

| |