波门跟踪学习使用

已于 2024-04-25 15:20:58 修改
阅读量1k 收藏 25
点赞数 13
文章标签： 学习
于 2024-04-23 11:01:48 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/weixin_54062948/article/details/138116385
版权

在这里插入图片描述

%% Track Start 五个目标航迹起始 3/4逻辑
% clear all;
% close all;
% clc;

T=5;%扫描周期 单位s
npts=4;%扫描周期数
TargetNum=5;
sigmaTheta=0.3;
sigmaR=40;
R=diag([sigmaR^2 sigmaR^2]);%观测误差协方差矩阵
X=zeros(4,npts,TargetNum);%真实航迹 状态参数数目 扫描周期数 目标数目
z=zeros(2,npts,TargetNum);%真实量测点
Phi=[1 T 0 0;…
0 1 0 0;…
0 0 1 T;…
0 0 0 1];%状态转移矩阵
V=wgn(1,2,0);%噪声
Q=[T^4/4*(V’V) T^3/2(V’V) ;T^3/2(V’V) T^2(V’V)];%过程噪声
P=[R(1,1) R(1,1)/T R(1,2) R(1,2)/T;…%初始协方差矩阵
R(1,1)/T 2R(1,1)/T^2 R(1,2)/T 2R(1,2)/T^2;…
R(2,1) R(2,1)/T R(2,2) R(2,2)/T;…
R(2,1)/T 2R(2,1)/T^2 R(2,2)/T 2*R(2,2)/T^2];
H=[1 0 0 0; 0 0 1 0];%观测矩阵
t=0:T:(npts-1)T;
X(:,1,1)=[55000 500 55000 0];%目标一初始状态
X(:,1,2)=[45000 500 45000 0];%目标二初始状态
X(:,1,3)=[35000 500 35000 0];%目标三初始状态
X(:,1,4)=[25000 500 45000 0];%目标四初始状态
X(:,1,5)=[15000 500 55000 0];%目标五初始状态
for i=2:npts
for j=1:TargetNum
X(:,i,j)=PhiX(:,i-1,j);
end
end
for i=1:npts
for j=1:TargetNum
z(:,i,j)=X(1:2:4,i,j)+wgn(2,1,0)*sigmaR;
end
end

figure;
for i=1:TargetNum
p1=plot(X(1,:,i),X(3,:,i),‘-’,‘MarkerEdgeColor’,‘b’);
hold on;
p2=plot(z(1,:,i),z(2,:,i),‘o’,‘MarkerEdgeColor’,‘k’);
hold on;
end
grid on;
axis([0 10e4 0 10e4]);

TARTRACK = struct(…
‘tra_pro’,[],…%航迹属性 1：确认航迹 2：中止航迹
‘X’,[],…%位置信息 x y
‘tra_index’,[],…%航迹号
‘track_quality’,[]…%航迹质量
);
TRACK_TEMP= struct(… %当前时刻航迹集合
‘tra_pro’,[],…%航迹属性 0:航迹头 1：暂时航迹 2：确认航迹 3：中止航迹
‘X’,[],…%位置信息 x y
‘tra_index’,[],…%航迹号
‘track_quality’,[]…%航迹质量
);
TRACK_TEMP_LASTMOMENT= struct(… %上一时刻航迹集合
‘tra_pro’,[],…%航迹属性 1：暂时航迹 2：确认航迹 3：中止航迹
‘X’,[],…%位置信息 x y
‘tra_index’,[],…%航迹号
‘track_quality’,[]…%航迹质量
);
Ob_Set=struct(… %量测集合
‘X’,[],…%位置信息 x y
‘inDoor’,[]…% 0 :未落入相关波门 1：落入相关波门
);
Vmin=[0;0];%目标最小速度 X方向 Y方向
Vmax=[1000;0];%目标最大速度 X方向 Y方向
lambda=50;
noisePoint=[];%噪声
Track=[];%航迹
flag=[‘s’,‘d’,‘v’,‘p’,‘h’,‘h’,‘h’,‘h’,‘h’,‘h’];
PG=1;%门概率质量
Chi=25;%对应2维 门概率质量PG=1

TarTrackIndex=zeros(1,1000);%目标航迹号
for i=1:1000
TarTrackIndex(i)=i;
end
TarTrackNum=0;%目标航迹数
for i=1:npts
TRACK_TEMP= struct(…
‘tra_pro’,[],…%航迹属性 0:航迹头 1：暂时航迹 2：确认航迹 3：中止航迹
‘X’,[],…%位置信息 x y
‘tra_index’,[],…%航迹号
‘track_quality’,[]…%航迹质量
);
%%产生噪声点
J=1;%产生噪声点的数量
r=rand();
while 1
temp1=0;
temp2=0;
for j=1:J
temp1=temp1+exp(-lambda)*lambda^(j-1)/factorial(j-1);
end
for j=1:J+1
temp2=temp2+exp(-lambda)*lambda^(j-1)/factorial(j-1);
end
if r<=temp2 && r>temp1
break;
else
J=J+1;
end
end
noisePoint=[];
noisePoint=rand(2,J)*1e5; %噪声点
p3(i)=plot(noisePoint(1,:),noisePoint(2,:),flag(i),‘MarkerEdgeColor’,‘r’);
hold on;
for j=1:TargetNum
temp3(j)=z(1,i,j);
temp4(j)=z(2,i,j);
end
Ob_Set=[]; %该时刻量测集合
for j=1:TargetNum
Ob_Set(j).X=[temp3(j);temp4(j)];
Ob_Set(j).inDoor=0;
end
for j=1:J
Ob_Set(j+TargetNum).X=noisePoint(:,j);
Ob_Set(j+TargetNum).inDoor=0;
end
p3(i)=plot(noisePoint(1,:),noisePoint(2,:),flag(i),‘MarkerEdgeColor’,‘r’);
hold on;

if length(TARTRACK)>=1 && size(TARTRACK(1).X,2)>0 %若有 已起始的航迹
    for j =1 :length(TARTRACK)
        %一阶多项式外推该时刻航迹位置 X=X0+VT                    
        X0=[TARTRACK(j).X(1,1)+(TARTRACK(j).X(1,1)-TARTRACK(j).X(1,2))/T*T ;TARTRACK(j).X(2,1)+(TARTRACK(j).X(2,1)-TARTRACK(j).X(2,2))/T*T];
        S=H*P*H'+R;%新息协方差矩阵      
        temp=1e8;
        k_temp=-1;
        for k=1:length(Ob_Set)
            d=X0-Ob_Set(k).X(:,1);           %X0外推位置，可以看作是预测位置？  
            D=d'*inv(S)*d;%统计距离
            if D<Chi 
                %计算最近三个时刻量测点的夹角 夹角大于阈值 就是虚假量测
                A=Ob_Set(k).X-TARTRACK(j).X(:,1);
                B=TARTRACK(j).X(:,2)-TARTRACK(j).X(:,1);
                alpha=acosd(dot(A,B)/norm(A)/norm(B));
                if D<temp && alpha >90
                    xy_temp=Ob_Set(k).X;
                    temp=D;
                    k_temp=k;
                end
            end
        end
        if temp~=1e8 %波门内有量测 才保留航迹                       
            TARTRACK(j).X=[Ob_Set(k_temp).X  TARTRACK(j).X];
            TARTRACK(j).track_quality=TARTRACK(j).track_quality+1;
            Ob_Set(k_temp)=[];%将匹配上的量测从量测集合中删除
        else %否则外推一个时刻
            TARTRACK(j).X=[X0 TARTRACK(j).X];
            TARTRACK(j).track_quality=TARTRACK(j).track_quality-1;                    
        end                   
    end
end
if length(TRACK_TEMP_LASTMOMENT)==1 && size(TRACK_TEMP_LASTMOMENT(1).X,2)==0 %若临时航迹为空 将全部量测点列为航迹头
    for j=1:length(Ob_Set)
        TRACK_TEMP(j).X=Ob_Set(j).X;
        TRACK_TEMP(j).tra_pro=0;%全部为航迹头
        TRACK_TEMP(j).tra_index=j;
        TRACK_TEMP(j).track_quality=1;
    end
else  
    index=1;
    for j=1:length(TRACK_TEMP_LASTMOMENT)             %周期大于1  用来保存过程数据？
        if TRACK_TEMP_LASTMOMENT(j).tra_pro==0 %如果只有航迹头 用速度波门法形成波门
            for k=1:length(Ob_Set)
                d=max(zeros(2,1),Ob_Set(k).X-TRACK_TEMP_LASTMOMENT(j).X-T*Vmax)+max(zeros(2,1),-Ob_Set(k).X+TRACK_TEMP_LASTMOMENT(j).X+T*Vmin);
                D=d'*inv(R+R)*d;
                if D<=Chi %量测互联 若落入波门 建立可能航迹 否则不建立
                    TRACK_TEMP(index).X=[Ob_Set(k).X TRACK_TEMP_LASTMOMENT(j).X ];
                    TRACK_TEMP(index).tra_index=index;
                    TRACK_TEMP(index).tra_pro=1;
                    TRACK_TEMP(index).track_quality=TRACK_TEMP_LASTMOMENT(j).track_quality+1;
                    index=index+1;        
                    Ob_Set(k).inDoor=1;%落入相关波门
                end
            end
        elseif TRACK_TEMP_LASTMOMENT(j).tra_pro==1 
            %一阶多项式外推该时刻航迹位置 X=X0+VT
            X0=[TRACK_TEMP_LASTMOMENT(j).X(1,1)+(TRACK_TEMP_LASTMOMENT(j).X(1,1)-TRACK_TEMP_LASTMOMENT(j).X(1,2))/T*T ;TRACK_TEMP_LASTMOMENT(j).X(2,1)+(TRACK_TEMP_LASTMOMENT(j).X(2,1)-TRACK_TEMP_LASTMOMENT(j).X(2,2))/T*T];
            S=H*P*H'+R;%新息协方差矩阵      
            temp=1e8;
            k_temp=-1;
            for k=1:length(Ob_Set)
                d=X0-Ob_Set(k).X(:,1);
                D=d'*inv(S)*d;%统计距离
                if D<Chi 
                    %计算三个时刻量测点的夹角 夹角大于阈值 就是虚假量测
                    A=Ob_Set(k).X-TRACK_TEMP_LASTMOMENT(j).X(:,1);
                    B=TRACK_TEMP_LASTMOMENT(j).X(:,2)-TRACK_TEMP_LASTMOMENT(j).X(:,1);
                    alpha=acosd(dot(A,B)/norm(A)/norm(B));
                    if D<temp  && alpha >90  %判断夹角
                        xy_temp=Ob_Set(k).X;
                        temp=D;
                        k_temp=k;
                    end
                    Ob_Set(k).inDoor=1;%落入相关波门
                end
            end
            if temp~=1e8 %波门内有量测 保留航迹                       
                Ob_Set(k_temp).match=1;
                TRACK_TEMP(index).X=[Ob_Set(k_temp).X TRACK_TEMP_LASTMOMENT(j).X];
                TRACK_TEMP(index).tra_index=index;
                TRACK_TEMP(index).track_quality=TRACK_TEMP_LASTMOMENT(j).track_quality+1;
                index=index+1;
            else
                TRACK_TEMP(index).X=[X0 TRACK_TEMP_LASTMOMENT(j).X];
                TRACK_TEMP(index).tra_index=index;
                TRACK_TEMP(index).track_quality=TRACK_TEMP_LASTMOMENT(j).track_quality;
                index=index+1;                        
            end   
        end
    end
    for k=1:length(Ob_Set) 
        if Ob_Set(k).inDoor==0%未落入相关波门 设为新航迹头
            TRACK_TEMP(index).X=[Ob_Set(k).X];
            TRACK_TEMP(index).tra_index=index;
            TRACK_TEMP(index).tra_pro=0;
            TRACK_TEMP(index).track_quality=1;
            index=index+1;                  
        end
    end
    deleteIndex =[];
    for j=1:length(TARTRACK)
        if TARTRACK(j).track_quality<0
            deleteIndex = [deleteIndex ,j];
        end
    end
    if(size(deleteIndex,2)~=0)
        TARTRACK(deleteIndex) =[];
        TarTrackNum = TarTrackNum - length(deleteIndex);
    end
    deleteIndex =[];
    TarTrackNum = TarTrackNum - length(deleteIndex);
    for j=1:length(TRACK_TEMP)
        if TRACK_TEMP(j).track_quality==3 % 将符合条件的航迹转为目标航迹存储
            TARTRACK(TarTrackNum+1).X=TRACK_TEMP(j).X;
            TARTRACK(TarTrackNum+1).track_quality=1;
            TARTRACK(TarTrackNum+1).tra_index = TarTrackIndex(1);
            TARTRACK(TarTrackNum+1).tra_pro = 1;
            TarTrackIndex(1) =[];%删除已经使用的航迹号
            deleteIndex =[deleteIndex j];%储存待会儿要删除的航迹编号
            TarTrackNum =TarTrackNum +1;
        end
        if size(TRACK_TEMP(j).X,2)==4 %该航迹等于四个周期
            if TRACK_TEMP(j).track_quality<=2  %只关联上两次
               deleteIndex =[deleteIndex j];%储存待会儿要删除的航迹编号
            end
        end
    end
    if(size(deleteIndex,2)~=0)
        TRACK_TEMP(deleteIndex) =[];
    end
end
TRACK_TEMP_LASTMOMENT=TRACK_TEMP;

end
legend([p1,p2,p3(1),p3(2),p3(3),p3(4)],{“真实航迹”,“真实量测”,“第一时刻虚假航迹”,“第二时刻虚假航迹”,“第三时刻虚假航迹”,“第四时刻虚假航迹”});

figure;
for j=1:length(TARTRACK)
plot(TARTRACK(j).X(1,:),TARTRACK(j).X(2,:),‘- .’,‘MarkerEdgeColor’,‘b’);
hold on;
end
grid on;
axis([0 10e4 0 10e4]);
legend(“航迹1”,“航迹2”,“航迹3”,“航迹4”,“航迹5”);

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值