plot_3d_kf1.m

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

% plot_3d_kf1.m
%
% Kalman filter with 3-D hard assignment 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 fig = plot_3d_kf1(target, measurement)

fig = figure;
hold on;
plot(target(1).state(1,:), target(1).state(3,:), '.');
plot(target(2).state(1,:), target(2).state(3,:), '.');
axis([100e3,130e3,147e3,151e3]);

% 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.* [9, 0, -6, 0;
                            0, .2, 0, 0;
                           -6 0,  5, 0;
                           0, 0, 0, .2];
track(2).P(:,:,1) = 1e4.* [9, 0, -6, 0;
                            0, .2, 0, 0;
                           -6 0,  5, 0;
                           0, 0, 0, .2];
% sufficient stat. at the rear end of the frozen window 
track(1).X = track(1).est(:,1);
track(1).Cov = track(1).P(:,:,1);
track(2).X = track(2).est(:,1);
track(2).Cov = track(2).P(:,:,1);
frozen_time = track(1).time(1);

q = .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-7;
numScan = length(target(1).time);

    for i=1:numScan-1
        T = measurement(i).time - frozen_time;
        T2 = measurement(i+1).time - measurement(i).time;
        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];
        F2 = [1, T2, 0, 0;
            0, 1, 0, 0;
            0, 0, 1, T2;
            0, 0, 0, 1];
        G2 = [T2*T2/2, 0;
            T2, 0;
            0, T2*T2/2;
            0, T2];
        z = []; z1 = []; z2 = [];
        R = []; R1 = []; R2 = [];
        cost = [];
        assign = [];
        ind00 = length(measurement(i).flag);
        ind01 = length(measurement(i+1).flag);
        if ind00 == 0
            [track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
            [track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
            if ind01 == 0
                [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
            else
                k1 = 1;
                for j=1:ind01
                    if measurement(i+1).flag(j) == 0
                        [z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
                        measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
                        plot(z(1,k1), z(2,k1), 'k*');                    
                        k1 = k1 + 1;
                        [z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
                        measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
                        k1 = k1 + 1;
                    else
                        [z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
                        measurement(i+1).pos(1), measurement(i+1).pos(2), C_r/sqrt(12), C_b/sqrt(12));
                        plot(z(1,k1), z(2,k1), 'c*');                    
                        k1 = k1 + 1;
                    end                        
                end
                for j=1:k1-1
                    % form the cost function for 2-D assignment
                    cost(1,j) = lr_kalman(track(1).X, track(1).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F2, G2, H, Ik, Pd, lambda);
                    cost(2,j) = lr_kalman(track(2).X, track(2).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F2, G2, H, Ik, Pd, lambda);            
                end
                if max(max(cost)) < 0
                    [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                    [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                else
                    [q,omiga,assign] = auction_2D(cost);
                    % update tracks
                    ind1 = find(~(assign-1));
                    if isempty(ind1) | cost(1, ind1) < 0
                        [track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
                    else
                        [track(1).est(:,i+1), track(1).P(:,:,i+1)] = stdkalman(track(1).X, track(1).Cov,...
                            z(:,ind1), Qk, R(:,:,ind1), vm, wm, F2, G2, H, Ik);
                    end
                    ind2 = find(~(assign-2));
                    if isempty(ind2) | cost(2, ind2) < 0
                        [track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
                    else
                        [track(2).est(:,i+1), track(2).P(:,:,i+1)] = stdkalman(track(2).X, track(2).Cov,...
                            z(:,ind2), Qk, R(:,:,ind2), vm, wm, F2, G2, H, Ik);
                    end
                end
            end
        else         % ind00~=0
            if ind01 == 0
                k1 = 1;
                for j=1:ind00
                    if measurement(i).flag(j) == 0
                        [z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
                        measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
                        plot(z(1,k1), z(2,k1), 'k*');
                        k1 = k1 + 1;
                        [z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
                        measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
                        k1 = k1 + 1;
                    else
                        [z(1,k1), z(2,k1), R(:,:,k1)] = 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));
                        plot(z(1,k1), z(2,k1), 'c*');
                        k1 = k1 + 1;
                    end                        
                end
                for j=1:k1-1
                    % form the cost function for 2-D assignment
                    cost(1,j) = lr_kalman(track(1).X, track(1).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);
                    cost(2,j) = lr_kalman(track(2).X, track(2).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);            
                end