gene_2d_sen2.m

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

% gene_2d_sen2.m
%
% generate the measurements for a 2-D scenario of two closely spaced targets with 
% sensor 2
function measurement = gene_2d_sen2(target)

% 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;
exp_FAs = 16*Pfa;
sens1_pos = [0, 0];
sens2_pos = [1.8e5, .8e5];
numScan = length(target(1).time);

for i=1:numScan
    measurement(i).time = target(1).time(i);
    measurement(i).flag = [];
    measurement(i).range = [];
    measurement(i).bearing = [];
    
    x = sens2_pos(1);
    y = sens2_pos(2);
    measurement(i).pos = sens2_pos;
    r1 = sqrt((target(1).state(1,i)-x)^2+(target(1).state(3,i)-y)^2);
    r2 = sqrt((target(2).state(1,i)-x)^2+(target(2).state(3,i)-y)^2);
    % This really depends on the target-sensor geometry
    b1 = pi + atan((target(1).state(3,i)-y)/(target(1).state(1,i)-x));
    b2 = pi + atan((target(2).state(3,i)-y)/(target(2).state(1,i)-x));        
    delta_r = abs(r1-r2);
    delta_b = abs(b1-b2);

    ind = 1;
    % a target detection
    if rand < Pd
        if delta_r>2*C_r | delta_b>2*C_b  % fully resolved
            sigma_r = C_r/sqrt(12);
            sigma_b = C_b/sqrt(12);
            if rand < Pd  % fully resolved, two detections
                measurement(i).flag(ind) = 1;
                measurement(i).range(ind) = r1 + randn*sigma_r;
                measurement(i).bearing(ind) = b1 + randn*sigma_b;
                ind = ind + 1;
                
                measurement(i).flag(ind) = 1;
                measurement(i).range(ind) = r2 + randn*sigma_r;
                measurement(i).bearing(ind) = b2 + randn*sigma_b;
                ind = ind + 1;
                
            elseif rand < .5 % fully resolved, one detection, target 1
                measurement(i).flag(ind) = 1;
                measurement(i).range(ind) = r1 + randn*sigma_r;
                measurement(i).bearing(ind) = b1 + randn*sigma_b;
                ind = ind + 1;
                
            else            % fully resolved, one detection, target 2
                measurement(i).flag(ind) = 1;
                measurement(i).range(ind) = r2 + randn*sigma_r;
                measurement(i).bearing(ind) = b2 + randn*sigma_b;
                ind = ind + 1;
            end
        elseif delta_r>C_r | delta_b>C_b   % partially unresolved
            prob = (delta_r/C_r-1)*(delta_b/C_b-1);
            if rand > prob
                if rand < Pd
                    measurement(i).flag(ind) = 0;
                    sigma_r = C_r;
                    sigma_b = C_b;            
                
                    measurement(i).range(ind) = (r1+r2)/2 + randn*sigma_r;
                    measurement(i).bearing(ind) = (b1+b2)/2 + randn*sigma_b;
                    ind = ind + 1;
                end
            else
                if rand < Pd
                    measurement(i).flag(ind) = 1;
                    sigma_r = (2-1/sqrt(12))*C_r - (1-1/sqrt(12))*delta_r;
                    sigma_b = (2-1/sqrt(12))*C_b - (1-1/sqrt(12))*delta_b;

                    measurement(i).range(ind) = r1 + randn*sigma_r;
                    measurement(i).bearing(ind) = b1 + randn*sigma_b;
                    ind = ind + 1;
                end
                if rand < Pd
                    measurement(i).flag(ind) = 1;
                    measurement(i).range(ind) = r2 + randn*sigma_r;
                    measurement(i).bearing(ind) = b2 + randn*sigma_b;
                    ind = ind + 1;
                end
            end
        else          % fully unresolved, one detection
            if rand < Pd
                sigma_r = C_r;
                sigma_b = C_b;            
                measurement(i).flag(ind) = 0;
                measurement(i).range(ind) = (r1+r2)/2 + randn*sigma_r;
                measurement(i).bearing(ind) = (b1+b2)/2 + randn*sigma_b;
                ind = ind + 1;
            end
        end
    end
    numFAs = poissrnd(exp_FAs); % false alarms
    for j=1:numFAs
        measurement(i).flag(ind) = -1;
        if rand>.5
            measurement(i).range(ind) = r1 + C_r*60*(rand-.5);
            measurement(i).bearing(ind) = b1 + C_b*60*(rand-.5);
            ind = ind + 1;
        else
            measurement(i).range(ind) = r2 + C_r*60*(rand-.5);
            measurement(i).bearing(ind) = b2 + C_b*60*(rand-.5);
            ind = ind + 1;
        end
    end
end