📄 test.m
字号:
for k=2:n2
product_PD=product_PD*(PD^(sigma(k,ii))*((1-PD)^(1-sigma(k,ii))));
% sigma
% product_PD
end
P_product_PD(ii)=product_PD;
% 参数版
numerator(ii)=(falsealarmdensities)^(phi(ii)-n1) *PDF_measurements(ii)*P_product_PD(ii);
% numerator =
% 1.0000e-004
% sigma =
% 0 0 0 0 0 0 0
% 0 1 1 0 1 1 0
% 0 0 1 1 1 0 1
% if phi(ii)==0
% phi(ii)=1;
% Volume=1;
% end
% numerator(ii)=(factorial(phi(ii))/Volume^phi(ii))*PDF_measurements(ii)*P_product_PD(ii);
end
% numerator
denominator=sum(numerator);
clear Con_Pro_joint_association_event;
for ii=1:n3
Con_Pro_joint_association_event(ii)=numerator(ii)/denominator; % 式子(7-2-80)
end
%%%%%%%%%%%
clear beta;
%%%%%%%%%%%
for ii=1:n1
for j=1:n2
bbb=0;
for k=1:n3
bbb=bbb+Con_Pro_joint_association_event(k)*ee(ii,j,k); % 式子(7-2-81)
end
beta(ii,j)=bbb;
end
end
clear betai_1;
clear betai_2;
for ii=1:n1
betai_1(ii)=beta(ii,2); % (1,0)
betai_2(ii)=beta(ii,3);
end
beta0_1=1-sum(betai_1);
beta0_2=1-sum(betai_2);
% 计算目标1的状态更新
SumV_jpda_1=[0;0;0]; % ???????
if n1==0
X_jpda_cvk_1(:,spt,1)=X_jpda_cvkk1_1(:,spt,1);
else
clear V_jpda_1;
for ii=1:n1
V_jpda_1(:,ii)=ZZ(:,ii)-X_jpda_cvk_1(:,spt,1); % 2维变3维
% V_jpda_1 % 3*1维
SumV_jpda_1=V_jpda_1(:,ii)*betai_1(ii)+SumV_jpda_1; % 3*1维
% SumV_jpda_1 % 3*1维
end
% the following SumV_jpda_1 is combined innovation
X_jpda_cvk_1(:,spt,1)=X_jpda_cvkk1_1(:,spt,1)+K_jpda_1(:,:,spt,1)*SumV_jpda_1; % 3*1维
end
% update P_jpda_cvk_1(:,:,i)
SumbV_jpda_1=[0,0,0;0,0,0;0,0,0]; % ???????
for ii=1:n1
SumbV_jpda_1=betai_1(ii)*V_jpda_1(:,ii)*V_jpda_1(:,ii)'+SumbV_jpda_1;
% SumbV_jpda_1
end
% the follow formula is from Tracking and Data Association in p165
% P_jpda_p_1(:,:,i)=K_jpda_1(:,:,i)*(SumbV_jpda_1-SumV_jpda_1*SumV_jpda_1')*K_jpda_1(:,:,i)';% 3*3维
% P_jpda_c_1(:,:,i)=(eye(3)-K_jpda_1(:,:,i))*P_jpda_cvkk1_1(:,:,i); % 3*3维
% P_jpda_cvk_1(:,:,i)=beta0_1*P_jpda_cvkk1_1(:,:,i)+(1-beta0_1)*P_jpda_c_1(:,:,i)+P_jpda_p_1(:,:,i);% 7-2-90
P_jpda_p_1r(:,:,spt,1)=K1r(:,spt,1)*(SumbV_jpda_1(1,1)-SumV_jpda_1(1)*SumV_jpda_1(1)')*K1r(:,spt,1)';
P_jpda_c_1r(:,:,spt,1)=(eye(3)-K1r(:,spt,1)*C0kcv)*P_jpda_cvkk1_1r(:,:,spt,1);
P_jpda_cvk_1r(:,:,spt,1)=beta0_1*P_jpda_cvkk1_1r(:,:,spt,1)+(1-beta0_1)*P_jpda_c_1r(:,:,spt,1)+P_jpda_p_1r(:,:,spt,1);
P_jpda_p_1theta(:,:,spt,1)=Ktheta1(:,spt,1)*(SumbV_jpda_1(2,2)-SumV_jpda_1(2)*SumV_jpda_1(2)')*Ktheta1(:,spt,1)';
P_jpda_c_1theta(:,:,spt,1)=(eye(3)-Ktheta1(:,spt,1)*C0kcv)*P_jpda_cvkk1_1theta(:,:,spt,1);
P_jpda_cvk_1theta(:,:,spt,1)=beta0_1*P_jpda_cvkk1_1theta(:,:,spt,1)+(1-beta0_1)*P_jpda_c_1theta(:,:,spt,1)+P_jpda_p_1theta(:,:,spt,1);
P_jpda_p_1q(:,:,spt,1)=K1q(:,spt,1)*(SumbV_jpda_1(3,3)-SumV_jpda_1(3)*SumV_jpda_1(3)')*K1q(:,spt,1)';
P_jpda_c_1q(:,:,spt,1)=(eye(3)-K1q(:,spt,1)*C0kcv)*P_jpda_cvkk1_1q(:,:,spt,1);
P_jpda_cvk_1q(:,:,spt,1)=beta0_1*P_jpda_cvkk1_1q(:,:,spt,1)+(1-beta0_1)*P_jpda_c_1q(:,:,spt,1)+P_jpda_p_1q(:,:,spt,1);
tt=cputime-t;
sumtime=sumtime+tt;
% 计算目标2的状态更新
SumV_jpda_2=[0;0;0];
if n1==0
% X_jpda_cvk_2(:,i)=X_jpda_cvkk1_2(:,i);
X_jpda_cvk_1(:,spt,2)=X_jpda_cvkk1_1(:,spt,2);
else
clear V_jpda_2;
for ii=1:n1
V_jpda_2(:,ii)=ZZ(:,ii)-X_jpda_cvk_1(:,spt,2);
SumV_jpda_2=betai_2(ii)*V_jpda_2(:,ii)+SumV_jpda_2;
% SumV_jpda_2
end
% the following SumV_jpda_2 is combined innovation
X_jpda_cvk_1(:,spt,2)=X_jpda_cvkk1_1(:,spt,2)+K_jpda_1(:,:,spt,2)*SumV_jpda_2;
end
% update P_jpda_cvk_2(:,:,i)
SumbV_jpda_2=[0,0,0;0,0,0;0,0,0];
for ii=1:n1
SumbV_jpda_2=betai_2(ii)*V_jpda_2(:,ii)*V_jpda_2(:,ii)'+SumbV_jpda_2;
end
% SumbV_jpda_2
% the follow formula is from Tracking and Data Association in p165
% P_jpda_p_2(:,:,i)= K_jpda_2(:,:,i)*(SumbV_jpda_2-SumV_jpda_2*SumV_jpda_2') *K_jpda_2(:,:,i)';
% P_jpda_c_2(:,:,i)=(eye(9)- K_jpda_2(:,:,i)*C0kcv)*P_jpda_cvkk1_2(:,:,i);
% P_jpda_cvk_2(:,:,i)=beta0_2*P_jpda_cvkk1_2(:,:,i)+(1-beta0_2)*P_jpda_c_2(:,:,i)+P_jpda_p_2(:,:,i);
P_jpda_p_1r(:,:,spt,2)=K1r(:,spt,2)*(SumbV_jpda_2(1,1)-SumV_jpda_2(1)*SumV_jpda_2(1)')*K1r(:,spt,2)';
P_jpda_c_1r(:,:,spt,2)=(eye(3)-K1r(:,spt,2)*C0kcv)*P_jpda_cvkk1_1r(:,:,spt,2);
P_jpda_cvk_1r(:,:,spt,2)=beta0_2*P_jpda_cvkk1_1r(:,:,spt,2)+(1-beta0_2)*P_jpda_c_1r(:,:,spt,2)+P_jpda_p_1r(:,:,spt,2);
P_jpda_p_1theta(:,:,spt,2)=Ktheta1(:,spt,2)*(SumbV_jpda_2(2,2)-SumV_jpda_2(2)*SumV_jpda_2(2)')*Ktheta1(:,spt,2)';
P_jpda_c_1theta(:,:,spt,2)=(eye(3)-Ktheta1(:,spt,2)*C0kcv)*P_jpda_cvkk1_1theta(:,:,spt,2);
P_jpda_cvk_1theta(:,:,spt,2)=beta0_2*P_jpda_cvkk1_1theta(:,:,spt,2)+(1-beta0_2)*P_jpda_c_1theta(:,:,spt,2)+P_jpda_p_1theta(:,:,spt,2);
P_jpda_p_1q(:,:,spt,2)=K1q(:,spt,2)*(SumbV_jpda_2(3,3)-SumV_jpda_2(3)*SumV_jpda_2(3)')*K1q(:,spt,2)';
P_jpda_c_1q(:,:,spt,2)=(eye(3)-K1q(:,spt,2)*C0kcv)*P_jpda_cvkk1_1q(:,:,spt,2);
P_jpda_cvk_1q(:,:,spt,2)=beta0_2*P_jpda_cvkk1_1q(:,:,spt,1)+(1-beta0_2)*P_jpda_c_1q(:,:,spt,2)+P_jpda_p_1q(:,:,spt,2);
%
for j=1:2
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
%[track,frameR(:,:,spt),frametheta(:,:,spt), frameq(:,:,spt)]=begin1[frameleavings(:,:,spt),tracknum,track, frameR(:,:,spt-1),frametheta(:,:,spt-1), frameq(:,:,spt-1)];
%% [track,frameR,frametheta, frameq]=begin1(spt,frameleavings,tracknum,track, frameR,frametheta,frameq)
Vcv_jpda_1(:,spt)=[X_jpda_cvk_1(1,spt,1);X_jpda_cvk_1(2,spt,1);X_jpda_cvk_1(3,spt,1)]-[Zcv_1r(:,spt,1); Zcv_1theta(:,spt,1);Zcv_1q(:,spt,1)]; % ??
Vcv_jpda_2(:,spt)=[X_jpda_cvk_1(1,spt,2);X_jpda_cvk_1(2,spt,2);X_jpda_cvk_1(3,spt,2)]-[Zcv_1r(:,spt,2); Zcv_1theta(:,spt,2);Zcv_1q(:,spt,2)];
% 判断目标1的估计是否落入3sigma门限内
if Vcv_jpda_1(:,spt)'*inv(Rmeasure)*Vcv_jpda_1(:,spt)<gama
estimationfallintogatecv_jpda_1(spt)=1;
else
estimationfallintogatecv_jpda_1(spt)=0;
end
estimationfallintogatecv_jpda_1
%判断目标2的估计是否落入3sigma门限内
if Vcv_jpda_2(:,spt)'*inv(Rmeasure)*Vcv_jpda_2(:,spt)<gama
estimationfallintogatecv_jpda_2(spt)=1;
else
estimationfallintogatecv_jpda_2(spt)=0;
end
end
end
tracklost_cv_jpda_1=0;
tracklost_cv_jpda_2=0;
% for i=dataassociation_begintime+19:time
% %使用jpda算法判断目标1的跟踪是否丢失
% if estimationfallintogatecv_jpda_1(i-19)==0&estimationfallintogatecv_jpda_1(i-18)==0&estimationfallintogatecv_jpda_1(i-17)==0&estimationfallintogatecv_jpda_1(i-16)==0&estimationfallintogatecv_jpda_1(i-15)==0&estimationfallintogatecv_jpda_1(i-14)==0&estimationfallintogatecv_jpda_1(i-13)==0&estimationfallintogatecv_jpda_1(i-12)==0&estimationfallintogatecv_jpda_1(i-11)==0&estimationfallintogatecv_jpda_1(i-10)==0&estimationfallintogatecv_jpda_1(i-9)==0&estimationfallintogatecv_jpda_1(i-8)==0&estimationfallintogatecv_jpda_1(i-7)==0&estimationfallintogatecv_jpda_1(i-6)==0&estimationfallintogatecv_jpda_1(i-5)==0&estimationfallintogatecv_jpda_1(i-4)==0&estimationfallintogatecv_jpda_1(i-3)==0&estimationfallintogatecv_jpda_1(i-2)==0&estimationfallintogatecv_jpda_1(i-1)==0&estimationfallintogatecv_jpda_1(i)==0
% tracklost_cv_jpda_1=1;
% end
% %使用jpda算法判断目标2的跟踪是否丢失
% if estimationfallintogatecv_jpda_2(i-19)==0&estimationfallintogatecv_jpda_2(i-18)==0&estimationfallintogatecv_jpda_2(i-17)==0&estimationfallintogatecv_jpda_2(i-16)==0&estimationfallintogatecv_jpda_2(i-15)==0&estimationfallintogatecv_jpda_2(i-14)==0&estimationfallintogatecv_jpda_2(i-13)==0&estimationfallintogatecv_jpda_2(i-12)==0&estimationfallintogatecv_jpda_2(i-11)==0&estimationfallintogatecv_jpda_2(i-10)==0&estimationfallintogatecv_jpda_2(i-9)==0&estimationfallintogatecv_jpda_2(i-8)==0&estimationfallintogatecv_jpda_2(i-7)==0&estimationfallintogatecv_jpda_2(i-6)==0&estimationfallintogatecv_jpda_2(i-5)==0&estimationfallintogatecv_jpda_2(i-4)==0&estimationfallintogatecv_jpda_2(i-3)==0&estimationfallintogatecv_jpda_2(i-2)==0&estimationfallintogatecv_jpda_2(i-1)==0&estimationfallintogatecv_jpda_2(i)==0
% tracklost_cv_jpda_2=1;
% end
% end
DeleteorNot
frameleavings
track
tracknum
SaveTrack⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -