logicinit.m

基于纯角度信息的采用逻辑规则和简单方法的航迹起始仿真程序

%逻辑规则的航迹起始
%代入全局参数：
% CandidateNumber侯选航迹数量
% Candidate(:,:,:)侯选航迹库,页：侯选航迹，行：点迹，列：方位、俯仰角
% CandidateStatusIndex( )侯选航迹库航迹状态索引(侯选航迹中的点迹数目)
% pointNum(k)孤立点迹数量
% k扫描序号

stagate1 = 85;        %方位角初始关联门大小
faigate1 = 85;        %俯仰角初始关联门大小
gate = 20;
%NewCandidateNumber = 0;     %新侯选航迹库中的侯选航迹数量
%NewCandidate = zeros(0);    %新侯选航迹库
%NewCandidate(5,2,1) = 0;
%NewCandidateStatusIndex=zeros(0);   %新侯选航迹库航迹状态索引
%pointasssign=zeros(1,pointNum(k));   %点迹关联标志，未关联为0，反之为1


%for i = 1:1:CandidateNumber     %扫描所有的侯选航迹
    for j = 1:1:pointNum  %扫描所有的孤立点迹
        if CandidateStatusIndex(i) == 1     %------侯选航迹只有一个点-------------------
            if abs( Candidate(1,1,i) - isoPoint(j,1) ) < stagate1
            %第i条侯选航迹第一个点迹方位sta角Candidate(1,1,i)与第k次扫描的第j个量测的方位角isoPoint(j,1,k)
            if abs( Candidate(1,2,i) - isoPoint(j,2) ) < faigate1
            %第i条侯选航迹第一个点迹俯仰fai角Candidate(1,2,i)与第k次扫描的第j个量测的俯仰角isoPoint(j,2,k)
                %---------符合关联条件---------
                NewCandidateNumber = NewCandidateNumber + 1;    %新侯选航迹库中航迹数量加1
                NewCandidate_symbol(NewCandidateNumber)=1
                for m = 1:1:4
                    NewCandidate(6-m,:,NewCandidateNumber) = Candidate(5-m,:,i);
                end
                NewCandidate(1,:,NewCandidateNumber) = isoPoint(j,:);
                NewCandidateStatusIndex(NewCandidateNumber) = CandidateStatusIndex(i)+1;
                pointasssign(j) = 1;    %标志该点迹已被关联
                %-------------------------------
            end
            end
        elseif CandidateStatusIndex(i) == 2     %------侯选航迹只有两个点-------------------
            if abs( 2*Candidate(1,1,i) - Candidate(2,1,i) - isoPoint(j,1) ) < abs( Candidate(1,1,i) - Candidate(2,1,i))/2
            %第i条侯选航迹第一个点迹方位sta角Candidate(1,1,i)与第k次扫描的第j个量测的方位角isoPoint(j,1,k)
            if abs( 2*Candidate(1,2,i) - Candidate(2,2,i) - isoPoint(j,2) ) < abs( Candidate(1,2,i) - Candidate(2,2,i))/2
            %第i条侯选航迹第一个点迹俯仰fai角Candidate(1,2,i)与第k次扫描的第j个量测的俯仰角isoPoint(j,2,k)
                %---------符合关联条件---------
                NewCandidateNumber = NewCandidateNumber + 1;    %新侯选航迹库中航迹数量加1
                NewCandidate_symbol(NewCandidateNumber)=1
                for m = 1:1:4
                    NewCandidate(6-m,:,NewCandidateNumber) = Candidate(5-m,:,i);
                end
                NewCandidate(1,:,NewCandidateNumber) = isoPoint(j,:);
                NewCandidateStatusIndex(NewCandidateNumber) = CandidateStatusIndex(i)+1;
                pointasssign(j) = 1;    %标志该点迹已被关联
                %-------------------------------
            end
            end
        elseif CandidateStatusIndex(i) == 3     %------侯选航迹有三个点-------------------
            if Candidate_symbol(i)==1  %三点外推
            PrePointSta = 3*Candidate(1,1,i) - 3*Candidate(2,1,i) + Candidate(3,1,i);
            PrePointFai = 3*Candidate(1,2,i) - 3*Candidate(2,2,i) + Candidate(3,2,i);
           else
            PrePointSta = 6*Candidate(1,1,i) - 8*Candidate(2,1,i) + 3*Candidate(3,1,i);
            PrePointFai = 6*Candidate(1,2,i) - 8*Candidate(2,2,i) + 3*Candidate(3,2,i);
           end
            %关联
            if abs( PrePointSta - isoPoint(j,1) ) < gate
            %第i条侯选航迹方位sta角外推值与第k次扫描的第j个量测的方位角isoPoint(j,1,k)
            if abs( PrePointFai - isoPoint(j,2) ) < gate
           
                %第i条侯选航迹俯仰fai角外推值与第k次扫描的第j个量测的俯仰角isoPoint(j,2,k)
                %---------符合关联条件---------
                NewCandidateNumber = NewCandidateNumber + 1;    %新侯选航迹库中航迹数量加1
                for m = 1:1:4
                    NewCandidate(6-m,:,NewCandidateNumber) = Candidate(5-m,:,i);
                end
                NewCandidate(1,:,NewCandidateNumber) = isoPoint(j,:);
                NewCandidateStatusIndex(NewCandidateNumber) = CandidateStatusIndex(i)+1;
                pointasssign(j) = 1;    %标志该点迹已被关联
                %-------------------------------
            end
            end
  %判断是否需要五点外推
          if  NewCandidateNumber == v
              NewCandidate_symbol(i)=2   
          end
        end     %------侯选航迹分别有一，二，三个点-------------------
        v=NewCandidateNumber
    end
    %end

%未被关联点迹作为航迹头加入新侯选航迹库
%for i = 1:1:pointNum
%    if pointasssign(i) == 0
%        NewCandidateNumber = NewCandidateNumber + 1;    %新侯选航迹库中航迹数量加1
%        NewCandidate(1,:,NewCandidateNumber) = isoPoint(i,:,k);     %添加航迹头
%        NewCandidateStatusIndex(NewCandidateNumber) = 1;                %新航迹状态索引，1-一个点迹
%    end
%end

%Candidate = NewCandidate;
%CandidateStatusIndex = NewCandidateStatusIndex
%CandidateNumber = NewCandidateNumber