?? f_initiate_tracks.asv
字號:
function [new_target_total,x_init,p_init,asso_mes]=f_initiate_tracks(M,dm,measurement,mes_total,vmin,vmax,r,t,yz1,yz2,yz3,yz4)
%一步延遲航跡起始
%M:用于航跡起始的掃描周期數
%measurement:所有量測(按列)
%mes_total:各個時刻的量測個數
%用于起始若干新航跡
x_init=[];
p_init=[];
count=linspace(0,0,M);%各個時刻的假設數
count(1)=mes_total(1);
for i=1:mes_total(1)
hp(1,i).matrix=i;%hp(k,i):直至k時刻第i個假設的量測向量組合
end
for k=1:2:M-1
mes_index1=sum(mes_total(1:k))+1;
mes_index2=sum(mes_total(1:k+1));
for i=1:count(k)%對k時刻的所有假設
if k==1%初始時刻,利用vmax和vmin確定候選量測區域
mes_now=measurement(:,i);
p_now=eye(dm*3);%單位陣
a_now=[];
for l=1:dm
a_now=[a_now;mes_now(l);0;0];
end
[confirm_mes,confirm_mes_index,confirm_mes_total,a1,p1,d]=f_confirm1(k,t,mes_now,a_now,p_now,mes_total(k+1),measurement(:,mes_index1:mes_index2),vmin,vmax,r,yz1);
%[confirm_mes,confirm_mes_index,confirm_mes_total,a1,p1,d]=f_confirm1(k,t,dm,mes_now,mes_total(k+1),measurement(:,mes_index1:mes_index2),vmin,vmax,r);
else
[confirm_mes,confirm_mes_index,confirm_mes_total,a1,p1,d]=f_confirm2(k,t,hp(k,i).a,hp(k,i).p,mes_total(k+1),measurement(:,mes_index1:mes_index2),r,yz2);
end
if k==M-1
[mincost,min_index]=min(d);
b=confirm_mes_index(min_index);
count(k+1)=count(k+1)+1;
[hp(k+1,count(k+1)).distance,hp(k+1,count(k+1)).matrix,hp(k+1,count(k+1)).a,hp(k+1,count(k+1)).p]=f_decide(dm,hp(k,i).matrix,b,min_index,a,p);%決策,僅保留其一
break;%退出循環
end
if confirm_mes_total>0%當一步確認區域有候選量測時
mes_index3=sum(mes_total(1:k+1))+1;
mes_index4=sum(mes_total(1:k+2));
b=[];
a=[];
p=[];
cost=[];
for l=1:confirm_mes_total%對于k+1時刻的所有確認量測,確定k+2時刻相應的候選量測
a0=a1(:,l);%%%%%%%%%%%%%%%%%%%%%%
p0=p1(:,(l-1)*dm*3+1:l*dm*3);
[b1,cost_temp,a_temp,p_temp]=f_extend(r,k,t,a0,p0,confirm_mes_index(l),confirm_mes(:,l),mes_total(k+2),measurement(:,mes_index3:mes_index4),yz4);
%b1:可能關聯矩陣,兩列(分別對應k+1時刻和k+2時刻的候選量測編號)
b=[b;b1];
a=[a a_temp];
p=[p p_temp];
cost=[cost cost_temp];
end
if isempty(b)==0
[mincost,min_index]=min(cost);
count(k+2)=count(k+2)+1;% 計數加1
[hp(k+2,count(k+2)).matrix,hp(k+2,count(k+2)).a,hp(k+2,count(k+2)).p]=f_decide(dm,hp(k,i).matrix,b,min_index,a,p);%決策,僅保留其一
end
end
end
end
%最終決策
decide_track_no=[];
decide_track_total=0;
asso_mes=[];%被關聯上的量測
for j=1:count(M)
[x_rt,p_rt,d_rt]=f_get_xp(M,t,hp(M,j).matrix,hp(M,j).a,hp(M,j).p,mes_total,measurement,r);
if d_rt<yz3
asso_mes=[asso_mes;hp(M,j).matrix];
decide_track_no=[decide_track_no j];
decide_track_total=decide_track_total+1;
x_init=[x_init x_rt(:,M)];
p_init=[p_init p_rt(:,(M-1)*dm*3+1:M*dm*3)];
end
end
new_target_total=decide_track_total;
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -