逻辑航迹起始算法代码

clear all; clc; close all;
warning（‘off‘）

% 扫描次数与扫描周期
N = 4;
M = 3;
T = 5; %秒

% 所考虑的正方形仿真区域
Xscope = 10^5;
Yscope = 10^5;

% 目标运动参数
v = 350;     % 500m/s
theta = 0;   % 水平正x轴运动

sigmax=50;
sigmay=5;

% 距离观测标准差与方位角观测标准差
sigma_r = 40;
sigma_theta = 0.3;

% 所考虑的正方形仿真区域内的杂波平均数
renbuda = 100;

% 指定4次扫描的杂波个数，每个周期的数目服从泊松分布，分布的均值由面积大小
% 以及单位面积内杂波数的乘积确定
K = poissrnd（renbuda 1 N）;

% 限制关联规则中的最大与最小速度、最大加速度和连续三次扫描的夹角
vmin = 2*v/3;
vmax = 3*v/2;
% amax = 50;
thetamax = 90;
% thetamax = pi/2;

%量测方程
H = [1 0 0 0;0 0 1 0];
F = [1 T 0 0; 0 1 0 0;  0 0 1 T;0 0 0 1];
R = [80 0;0 80];


%% 仿真产生5个目标的航迹（量测数据） %%
radar1 = simutrack（55000 55000 v theta 0 0 sigma_r sigma_theta T N）; %4行2列
radar2 = simutrack（45000 45000 v theta 0 0 sigma_r sigma_theta T N）;
radar3 = simutrack（35000 35000 v theta 0 0 sigma_r sigma_theta T N）;
radar4 = simutrack（45000 25000 v theta 0 0 sigma_r sigma_theta T N）;
radar5 = simutrack（55000 15000 v theta 0 0 sigma_r sigma_theta T N）;

%% 每次扫描所得点迹集合sample中的前5个点被设定为目标点 %%
i = 0;
for k = K
    i = i + 1;
    cycle（i）.sample = [rand（k1）*Xscope rand（k1）*Yscope];       %cycle为结构体   存储杂波点
    cycle（i）.sample = [radar1（i:）; radar2（i:）; radar3（i:）;
        radar4（i:）; radar5（i:）; cycle（i）.sample];
end

%% 用第一次扫描的点迹建立暂时航迹 %%
for i = 1:size（cycle（1）.sample 1）
    track（i）.seq = cycle（1）.sample（i:）;
%     track（i）.shouldadd = [];
    track（k）.assoi_point = [];      %存储与航迹关联的点迹的关联值
end

%% 用第二次扫描的点建立可能航迹 %%
for i = 2
    tracknum = size（track2）;      %求得暂态航迹数
    tracknum_temp = tracknum;
    samplenum = size（cycle（i）.sample1）;     %求得第二帧的量测点迹数
    
    D = zeros（tracknumsamplenum）;      %存储暂态航迹与量测的关联值
    %% 计算本次扫描的所有点迹与暂态航迹的关联值 %%
    for j = 1:samplenum
        data = cycle（i）.sample（j:）;
        for k = 1:tracknum
            if size（track（k）.seq1） > 0
                data1 = track（k）.seq;
                D（kj） = （data（1）-data1（1））^2 + （data（2）-data1（2））^2;
            end                 
        end
    end
    
    for j = 1:samplenum
        flag = 0;
        for k = 1:tracknum
            if D（kj） >= （vmin*T）^2 && D（kj） <= （vmax*T）^2
                track（k）.assoi_point = [track（k）.assoi_point;D（kj） j];
                flag = 1;
            end
        end
        
        %% 与暂态航迹未关联的点迹作为新的暂态航迹头
        if flag == 0
            tracknum_temp =tracknum_temp + 1;
            
            track（tracknum_temp）.seq = cycle（i）.sample（j:）;
            track（tracknum_temp）.assoi_point = [];
        end
    end
    
    %% 由关联点迹判别，对暂态航迹进行处理 %%
    for k = 1:tracknum
        L = size（track（k）.assoi_point1）;
        if L == 1
            j = track（k）.assoi_point（end2）;
            track（k）.seq = [track（k）.seq;cycle（i）.sample（j:）];
        end
        if L > 1
            min = track（k）.assoi_point（1:）;
            for j = 2:L
                if （track（k）.assoi_point（j1） - （v*T