?? test.asv
字號:
clear
clc
%第一部分，產生航跡
load data.dat
%第二部分，兩點起始
Start_Gate=8; %起始門限
Predict_Gate=1.5; %預測門限
MaxFlame=9; %總幀數
MaxTrack=5; %最大航跡數
MaxLeavings=3; %最大雜波數
dataassociation=5; %關聯起始時間
deletetracknum=0; %刪除的航跡，用于作圖
T=1; %采樣間隔
time=9;
ZZ=0;
%讀第一幀數據
spt=1;
[framedata1,num1]=ReadFrameData(spt);
%讀第二幀數據
spt=2;
[framedata2,num2]=ReadFrameData(spt);
track(MaxTrack+1,7,MaxFlame)=0;
SaveTrack=track;
frameleavings(MaxLeavings,5,time)=0;
%兩點航跡起始 函數為Track_Begin
[track,tracknum]=Track_Begin(track,framedata1,framedata2,num1,num2,Start_Gate);
% track
% framedata
% tracknum
% track1
% track2
% framedata1
% framedata2
spt=3;
DeleteorNot(MaxFlame,MaxTrack)=0;
C0kcv=[1,0,0];
P_jpda_cvk_1(:,:,1)=[1,0,0;
0,1,0;
0,0,1]*10000;
Rmeasure=[5^2,0,0;
0,5^2,0;
0,0,5^2];
for i=1:tracknum
X_jpda_cvk_1r(:,1,i)=[track(i,2,1);(track(i,2,2)-track(i,2,1))/T;eps];
X_jpda_cvk_1theta(:,1,i)=[track(i,3,1);(track(i,3,2)-track(i,3,1))/T;eps];
X_jpda_cvk_1q(:,1,i)=[track(i,4,1);(track(i,4,2)-track(i,4,1))/T;eps];
P_jpda_cvk_1r(:,:,1,i)=P_jpda_cvk_1(:,:,1);
P_jpda_cvk_1theta(:,:,1,i)=P_jpda_cvk_1(:,:,1);
P_jpda_cvk_1q(:,:,1,i)=P_jpda_cvk_1(:,:,1);
MUr1(:,1,i)=[1/3;1/3;1/3];
MUtheta1(:,1,i)=[1/3;1/3;1/3];
MUq1(:,1,i)=[1/3;1/3;1/3];
end
for spt=spt-1:dataassociation-1;
[framedata,num]=ReadFrameData(spt);
for k=1:num
tracktemp(k,:,spt)=[k,framedata(k,2),framedata(k,3),framedata(k,4)];
end
for j=1:tracknum %濾波
Zcv_1r(:,spt,j)=[tracktemp(j,2,spt)]';
Zcv_1theta(:,spt,j)=[tracktemp(j,3,spt)]';
Zcv_1q(:,spt,j)=[tracktemp(j,4,spt)]';
[X_jpda_cvk_1r(:,spt,j),P_jpda_cvk_1r(:,:,spt,j),P_jpda_cvkk1_1r(:,:,spt,j),S1r(:,spt,j),K1r(:,spt,j),Xkk11r(:,spt,j),MUr1(:,spt,j)]=fimm(spt,Rmeasure(1,1),X_jpda_cvk_1r(:,spt-1,j),P_jpda_cvk_1r(:,:,spt-1,j),MUr1(:,spt-1,j),T,Zcv_1r(:,spt,j),1);
[X_jpda_cvk_1theta(:,spt,j),P_jpda_cvk_1theta(:,:,spt,j),P_jpda_cvkk1_1theta(:,:,spt,j),S1theta(:,spt,j),Ktheta1(:,spt,j),Xkk11theta(:,spt,j),MUtheta1(:,spt,j)]=fimm(spt,Rmeasure(2,2),X_jpda_cvk_1theta(:,spt-1,j),P_jpda_cvk_1theta(:,:,spt-1,j),MUtheta1(:,spt-1,j),T,Zcv_1theta(:,spt,j),2);
[X_jpda_cvk_1q(:,spt,j),P_jpda_cvk_1q(:,:,spt,j),P_jpda_cvkk1_1q(:,:,spt,j),S1q(:,spt,j),K1q(:,spt,j),Xkk11q(:,spt,j),MUq1(:,spt,j)]=fimm(spt,Rmeasure(3,3),X_jpda_cvk_1q(:,spt-1,j),P_jpda_cvk_1q(:,:,spt-1,j),MUq1(:,spt-1,j),T,Zcv_1q(:,spt,j),2);
X_jpda_cvk_1(:,spt,j)=[X_jpda_cvk_1r(1,spt,j);X_jpda_cvk_1theta(1,spt,j);X_jpda_cvk_1q(1,spt,j)];
X_jpda_cvkk1_1(:,spt,j)=[Xkk11r(1,spt,j);Xkk11theta(1,spt,j);Xkk11q(1,spt,j)];
S_jpda_1(:,:,spt,j)=diag([S1r(:,spt,j),S1theta(:,spt,j),S1q(:,spt,j)]);
K_jpda_1(:,:,spt,j)=[K1r(:,spt,j),Ktheta1(:,spt,j),K1q(:,spt,j)];
track(j,2,spt)=X_jpda_cvk_1r(1,spt,j);
track(j,3,spt)=X_jpda_cvk_1theta(1,spt,j);
track(j,4,spt)=X_jpda_cvk_1q(1,spt,j);
track(j,5,spt)=Xkk11r(1,spt,j);
track(j,6,spt)=Xkk11theta(1,spt,j);
track(j,7,spt)=Xkk11q(1,spt,j);
end
end
tracknum
track
sumtime=0;
frameR(:,:) = zeros(3,4); % r 暫存的數據
frameq(:,:) = zeros(3,4); %
frametheta(:,:)= zeros(3,4);
for spt=dataassociation:time
[framedata,num]=ReadFrameData(spt);
[track,SaveTrack,tracknum,frameleavings,DeleteorNot,deletetracknum,ZZ_jpda,P_jpda_cvk_1r,MUr1,P_jpda_cvk_1theta,MUtheta1,P_jpda_cvk_1q,MUq1]=Judge(track,SaveTrack,framedata,frameleavings,tracknum,num,spt,DeleteorNot,Predict_Gate,deletetracknum,P_jpda_cvk_1r,MUr1,P_jpda_cvk_1theta,MUtheta1,P_jpda_cvk_1q,MUq1);
% ZZ_jpda
% ZZ_jpda(:,:,1)
frameR(:,:,dataassociation-1) = eye(3,4); % r 暫存的數據
frameq(:,:,dataassociation-1) = eye(3,4); %
frametheta(:,:,dataassociation-1)=eye(3,4);
for j=1:tracknum %濾波
Zcv_1r(:,spt,j)=[track(j,2,spt)]';
Zcv_1theta(:,spt,j)=[track(j,3,spt)]';
Zcv_1q(:,spt,j)=[track(j,4,spt)]';
[X_jpda_cvk_1r(:,spt,j),P_jpda_cvk_1r(:,:,spt,j),P_jpda_cvkk1_1r(:,:,spt,j),S1r(:,spt,j),K1r(:,spt,j),Xkk11r(:,spt,j),MUr1(:,spt,j)]=fimm(spt,Rmeasure(1,1),X_jpda_cvk_1r(:,spt-1,j),P_jpda_cvk_1r(:,:,spt-1,j),MUr1(:,spt-1,j),T,Zcv_1r(:,spt,j),1);
[X_jpda_cvk_1theta(:,spt,j),P_jpda_cvk_1theta(:,:,spt,j),P_jpda_cvkk1_1theta(:,:,spt,j),S1theta(:,spt,j),Ktheta1(:,spt,j),Xkk11theta(:,spt,j),MUtheta1(:,spt,j)]=fimm(spt,Rmeasure(2,2),X_jpda_cvk_1theta(:,spt-1,j),P_jpda_cvk_1theta(:,:,spt-1,j),MUtheta1(:,spt-1,j),T,Zcv_1theta(:,spt,j),2);
[X_jpda_cvk_1q(:,spt,j),P_jpda_cvk_1q(:,:,spt,j),P_jpda_cvkk1_1q(:,:,spt,j),S1q(:,spt,j),K1q(:,spt,j),Xkk11q(:,spt,j),MUq1(:,spt,j)]=fimm(spt,Rmeasure(3,3),X_jpda_cvk_1q(:,spt-1,j),P_jpda_cvk_1q(:,:,spt-1,j),MUq1(:,spt-1,j),T,Zcv_1q(:,spt,j),2);
X_jpda_cvk_1(:,spt,j)=[X_jpda_cvk_1r(1,spt,j);X_jpda_cvk_1theta(1,spt,j);X_jpda_cvk_1q(1,spt,j)];
X_jpda_cvkk1_1(:,spt,j)=[Xkk11r(1,spt,j);Xkk11theta(1,spt,j);Xkk11q(1,spt,j)]; % 預測直
S_jpda_1(:,:,spt,j)=diag([S1r(:,spt,j),S1theta(:,spt,j),S1q(:,spt,j)]);
K_jpda_1(:,:,spt,j)=[K1r(:,spt,j),Ktheta1(:,spt,j),K1q(:,spt,j)];
track(j,2,spt)=X_jpda_cvk_1r(1,spt,j);
track(j,3,spt)=X_jpda_cvk_1theta(1,spt,j);
track(j,4,spt)=X_jpda_cvk_1q(1,spt,j);
track(j,5,spt)=Xkk11r(1,spt,j);
track(j,6,spt)=Xkk11theta(1,spt,j);
track(j,7,spt)=Xkk11q(1,spt,j);
end
PG=1-eps;
gama=1.1;
falsealarmdensities=0.000000000000000001;
%suppose the probability of dectation PD=0.99
PD=0.99;
ZZ_jpda_1=ZZ_jpda(:,:,1);
ZZ_jpda_2=ZZ_jpda(:,:,2);
% X_jpda_cvk_1(:,i)=X_jpda_cvk_1(:,spt,1);
% X_jpda_cvk_2(:,i)=X_jpda_cvk_1(:,spt,2);
% S_jpda_1(:,:,i)=
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 計算單個目標jpda的計算量
t=cputime;
clear omega ZZ;
% 生成確認陣
[omega,ZZ]=fform_validation_matrix(ZZ_jpda_1,ZZ_jpda_2);
ZZ
clear em ee;
% 生成可行矩陣
[em,ee]=fget_data_association_hypotheses(omega);
clear tao sigma phi;
% 計算 關聯指示器\目標檢測指示器 和 聯合事件中假量測數量
[tao,sigma,phi]=fcalculate_indicators(ee);
% sigma
%計算可行矩陣行、列和數量
[n1,n2,n3]=size(ee);
% n1
% n2
% n3
clear sigma0
sigma0=sigma;
clear e V_jpda P_product_PD numerator PDF_measurements;
for ii=1:n3 %%%%( n3=7 )
product_Gause=1;
for j=1:n1 %% 2
e(j)=1;
% em =
% 1 2 2 3 3 1 1
% 1 1 3 1 2 2 3
if em(j,ii)==1
e(j)=1;
elseif em(j,ii)==2
%V_jpda(:,j) is the innovation of the jth measurement
V_jpda(:,j)=ZZ(:,j)-X_jpda_cvk_1(:,spt,1);
e(j)=exp(-0.5*V_jpda(:,j)'*inv(S_jpda_1(:,:,spt,1))*V_jpda(:,j));
elseif em(j,ii)==3
V_jpda(:,j)=ZZ(:,j)-X_jpda_cvk_1(:,spt,2);
e(j)=exp(-0.5*V_jpda(:,j)'*inv(S_jpda_1(:,:,spt,2))*V_jpda(:,j));
end
product_Gause=product_Gause*e(j);
end
PDF_measurements(ii)=product_Gause;
product_PD=1;
for k=2:n2
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -