test_2d_pda2.m

一个目标跟踪系统的MATLAB 源程序包

% test_2d_PDA2.m
%
% Kalman filter with 2-D PDA using LP for tracking multiple targets with possibly unresolved measurements,
% assuming the resolvability indicator is known.

% See also: gene_2d_scn.m, gene_2d_meas.m
function track = test_2d_pda2(target, MCruns)

% define SNR
SNRdb = 10;
SNR = 10^(SNRdb/10);
% detection threshold
threshold = 2.55;
% calc. the probability of detection and false alarm per cell 
% according to certain model (currently Swerling-I)
Pfa = exp(-threshold*threshold/2);
Pd = exp(-threshold*threshold/2/(1+SNR));
% measurement std, cell size
C_r = 50;
C_b = 2e-3;

% track initiation
track(1).time(1) = 1;
track(2).time(1) = 1;
track(1).est(:,1) = target(1).state(:,1); 
track(2).est(:,1) = target(2).state(:,1);
track(1).P(:,:,1) = 1e4.* [2, 0, 2, 0;
                            0, .2, 0, 0;
                           2 0,  2.7, 0;
                           0, 0, 0, .2];
track(2).P(:,:,1) = 1e4.* [2, 0, 2, 0;
                            0, .2, 0, 0;
                           2 0, 2.7, 0;
                           0, 0, 0, .2];
q = 0.01; % process noise
H = [1, 0, 0, 0; 0, 0, 1, 0];
vm = zeros(2,1);
wm = zeros(2,1);
Ik = eye(2);
Qk = q.*Ik;
lambda = 1e-8;
numScan = length(target(1).time);
h = waitbar(0,'Please wait...');

for MC=1:MCruns
    measurement = gene_2d_sen2(target);
    
    for i=1:numScan
        T = measurement(i).time - track(1).time(i);
        F = [1, T, 0, 0;
            0, 1, 0, 0;
            0, 0, 1, T;
            0, 0, 0, 1];
        G = [T*T/2, 0;
            T, 0;
            0, T*T/2;
            0, T];
        z = [];
        R = [];
        cost = [];
        assign = [];
        ind = length(measurement(i).flag);
        if ind == 0
            [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).est(:,i), track(1).P(:,:,i),vm,Qk,F,G);
            [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).est(:,i), track(2).P(:,:,i),vm,Qk,F,G);
        else
            k = 1;
            cost(1,1) = 0;
            cost(2,1) = 0;
            for j=1:ind
                if measurement(i).flag(j) == 0
                    [z(1,k), z(2,k), R(:,:,k)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
                    k = k + 1;
                    [z(1,k), z(2,k), R(:,:,k)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
                    k = k + 1;
                else
                    [z(1,k), z(2,k), R(:,:,k)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), measurement(i).pos(1), measurement(i).pos(2), C_r/sqrt(12), C_b/sqrt(12));
                    k = k + 1;
                end
            end
            for j=1:k-1
                cost(1,j+1) = lr_kalman(track(1).est(:,i), track(1).P(:,:,i), z(:,j), Qk, R(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);
                cost(2,j+1) = lr_kalman(track(2).est(:,i), track(2).P(:,:,i), z(:,j), Qk, R(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);            
            end
            % LP solution of the 2-D assignment problem
            associate_prob = myLinProg(-cost);
            if isempty(associate_prob)
                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).est(:,i), track(1).P(:,:,i),vm,Qk,F,G);
                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).est(:,i), track(2).P(:,:,i),vm,Qk,F,G);
            else
                % update tracks
                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = pdakalman(track(1).est(:,i), track(1).P(:,:,i),...
                z, R, associate_prob(1,:), Qk, vm, wm, F, G, H, Ik);
                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = pdakalman(track(2).est(:,i), track(2).P(:,:,i),...
                z, R, associate_prob(2,:), Qk, vm, wm, F, G, H, Ik);
            end
        end
        track(1).time(i+1) = measurement(i).time;
        track(2).time(i+1) = measurement(i).time;
        track(1).error(MC, :, i) = (track(1).est(:,i+1) - target(1).state(:,i)).^2;
        track(2).error(MC, :, i) = (track(2).est(:,i+1) - target(2).state(:,i)).^2;
        % chi2 test
        track(1).NEES(MC, i) = (track(1).est(:,i+1) - target(1).state(:,i))'*inv(track(1).P(:,:,i+1))*(track(1).est(:,i+1) - target(1).state(:,i));
        track(2).NEES(MC, i) = (track(2).est(:,i+1) - target(2).state(:,i))'*inv(track(2).P(:,:,i+1))*(track(2).est(:,i+1) - target(2).state(:,i));
    end
    waitbar(MC/MCruns, h);
end
close(h);

% performance analysis
track1_ind = [];
track2_ind = [];
track1_loss = 0;
track2_loss = 0;
for i=1:MCruns
    if max(track(1).NEES(i,:))<13.3   % 99% upper limit of chi2 distribution
        track1_ind = [track1_ind, i];
    else
        track1_loss = track1_loss + 1;
    end
    if max(track(2).NEES(i,:))<13.3
        track2_ind = [track2_ind, i];
    else
        track2_loss = track2_loss + 1;
    end
end
track(1).RMS_pos = sqrt(reshape(mean(track(1).error(track1_ind,1,:)+track(1).error(track1_ind,3,:)), 1, numScan));
track(2).RMS_pos = sqrt(reshape(mean(track(2).error(track2_ind,1,:)+track(2).error(track2_ind,3,:)), 1, numScan));
track(1).RMS_vel = sqrt(reshape(mean(track(1).error(track1_ind,2,:)+track(1).error(track1_ind,4,:)), 1, numScan));
track(2).RMS_vel = sqrt(reshape(mean(track(2).error(track2_ind,2,:)+track(2).error(track2_ind,4,:)), 1, numScan));
track(1).loss = track1_loss;
track(2).loss = track2_loss;