get_approx_cov1.m

来自「一个目标跟踪系统的MATLAB 源程序包」· M 代码 · 共 31 行

% function crossXY = get_approx_cov1(X, Y, rho)
%
% get approximate cross-covariance of the state estimate from different local tracker (YBS95, pp. 455)
%
% Input parameters: 
%     X   --------- the covariance of the state estimate from tracker i
%     Y   --------- the covariance of the state estimate from tracker j
%     rho --------- a matrix where rho_ij are the approximation terms chosen within (-1, 1)
% Output parameters:
%     crossXY ----- the approxiamte cross-covariance between X and Y

function crossXY = get_approx_cov1(X, Y, rho)
[n1, n2] = size(X);
[n3, n4] = size(Y);

if n1~=n2 | n3~=n4 | n1~=n3
     errmsg = [ 'Incorrect dimension of the state estimates!' ];
     errordlg(errmsg, 'DTA Simulation: Warning', 'modal');
     return;
end    
crossXY = zeros(size(X));
for i=1:n1
    for j=1:n1
        rho1 = X(i,j)/sqrt(X(i,i)*X(j,j));
        rho2 = Y(i,j)/sqrt(Y(i,i)*Y(j,j));
        crossXY(i,j) = rho(i,j) * sqrt(X(i,i)*Y(j,j)*abs(rho1*rho2)) * sign(rho1) * sign(rho2);
    end
end