📄 fnnassociation.m
字号:
function Z=fnnassociation(Xkk1,Z1,Z2)
function [Xkk,Pkk,Pkk1,S,K,Xkk2]=fimm(i,Rmeasure,X0,T,Z)
% this function is JPDA
Xkk=[X_jpda_cvk_1r(1,i);X_jpda_cvk_1theta(1,i);X_jpda_cvk_1q(1,i)];
Xkk2=[Xkk11r(1,i);Xkk11theta(1,i);Xkk11q(1,i)];
S=diag([S1r(:,i),S1theta(:,i),S1q(:,i)]);
X_jpda_cvk_2(:,i)=[X_jpda_cvk_2r(1,i);X_jpda_cvk_2theta(1,i);X_jpda_cvk_2q(1,i)];
X_jpda_cvkk1_2(:,i)=[Xkk12r(1,i);Xkk12theta(1,i);Xkk12q(1,i)];
S=diag([S2r(:,i),S2theta(:,i),S2q(:,i)]);
PG=1-eps;
gama=11;
%suppose the probability of dectation PD=0.99
PD=0.99;
clear ZZ_jpda_1;
j=1;
for ii=1:(clutternum+2) %%%%%%%%%%%%%%%%%%%%%%%% 1:4
% d_jpda_1(:,ii)=C0kcv*F0kcv*X_jpda_cvk_1(:,i-1)-[clutterx(ii,i),cluttery(ii,i),clutterz(ii,i)]';
d1(:,ii)=Xkk2(:,i)-clutterx(ii,i);
a11= d1(:,ii)'*inv( S(i))* d1(:,ii);
if d1(:,ii)'*inv( S(i))* d1(:,ii)<=a*
ZZ1(:,j)=clutterx(ii,i); % 落入门限的量测
j=j+1;
end
end
% 如果没有量测落入门限，则用量测预测作为量测
% ZZ_jpda_1 可以判断有没有杂波
if j==1
% ZZ_jpda_1(:,j)=C0kcv*F0kcv*X_jpda_cvk_1(:,i-1);
ZZ1(:,j)=Xkk(:,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 确定落入门限的量测
clear ZZ2;
j=1;
for ii=1:(clutternum+2)
% d_jpda_2(:,ii)=C0kcv*F0kcv*X_jpda_cvk_2(:,i-1)-[clutterx(ii,i),cluttery(ii,i),clutterz(ii,i)]';
d2(:,ii)=Xkk2(:,i)-clutterx(ii,i);
%%%%%%%%%%%%%%%%%%%%%%%%%%
a12=d2(:,ii)'*inv(S(i))*d2(:,ii);
% a12
if d2(:,ii)'*inv(S(i))*d2(:,ii)<=gama
ZZ2(:,j)=clutterx(ii,i);
j=j+1;
end
end
% 如果没有量测落入门限，则用量测预测作为量测
if j==1
% ZZ_jpda_2(:,j)=C0kcv*F0kcv*X_jpda_cvk_2(:,i-1);
ZZ2(:,j)=Xkk(:,i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 计算单个目标jpda的计算量
t=cputime;
clear omega ZZ;
% 生成确认阵
[omega,ZZ]=fform_validation_matrix(ZZ1,ZZ2);
% omega
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);
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)-Xkk 1(:,i);
e(j)=exp(-0.5*V_jpda(:,j)'*inv( S)*V_jpda(:,j));
elseif em(j,ii)==3
V_jpda(:,j)=ZZ(:,j)-Xkk 2(:,i);
e(j)=exp(-0.5*V_jpda(:,j)'*inv(S)*V_jpda(:,j));
end
product_Gause=product_Gause*e(j);
end
PDF_measurements(ii)=product_Gause;
product_PD=1;
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);
% sigma =
% 0 0 0 0 0 0 0
% 0 1 1 0 1 1 0
% 0 0 1 1 1 0 1
end
% numerator
denominator=sum(numerator);
% denominator
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
Xkk=Xkk2;
else
clear V_jpda_1;
for ii=1:n1
V_jpda_1(:,ii)=ZZ(:,ii)-Xkk(:,i); % 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
Xkk=Xkk2+K_jpda_1(:,:,i)*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_1(:,:,i)=K1(:,i)*(SumbV_jpda_1(1,1)-SumV_jpda_1(1)*SumV_jpda_1(1)')*K1(:,i)';
P_jpda_c_1(:,:,i)=(eye(3)-K1(:,i)*C0kcv)*P_jpda_cvkk1_1(:,:,i);
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);
% P_jpda_p_1theta(:,:,i)=Ktheta1(:,i)*(SumbV_jpda_1(2,2)-SumV_jpda_1(2)*SumV_jpda_1(2)')*Ktheta1(:,i)';
% P_jpda_c_1theta(:,:,i)=(eye(3)-Ktheta1(:,i)*C0kcv)*P_jpda_cvkk1_1theta(:,:,i);
% P_jpda_cvk_1theta(:,:,i)=beta0_1*P_jpda_cvkk1_1theta(:,:,i)+(1-beta0_1)*P_jpda_c_1theta(:,:,i)+P_jpda_p_1theta(:,:,i);
%
% P_jpda_p_1q(:,:,i)=K1q(:,i)*(SumbV_jpda_1(3,3)-SumV_jpda_1(3)*SumV_jpda_1(3)')*K1q(:,i)';
% P_jpda_c_1q(:,:,i)=(eye(3)-K1q(:,i)*C0kcv)*P_jpda_cvkk1_1q(:,:,i);
% P_jpda_cvk_1q(:,:,i)=beta0_1*P_jpda_cvkk1_1q(:,:,i)+(1-beta0_1)*P_jpda_c_1q(:,:,i)+P_jpda_p_1q(:,:,i);
%
% 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);
else
clear V_jpda_2;
for ii=1:n1
V_jpda_2(:,ii)=ZZ(:,ii)-X_jpda_cvk_2(:,i);
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_2(:,i)=X_jpda_cvkk1_2(:,i)+K_jpda_2(:,:,i)*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_2(:,:,i)=K2(:,i)*(SumbV_jpda_2(1,1)-SumV_jpda_2(1)*SumV_jpda_2(1)')*K2(:,i)';
P_jpda_c_2(:,:,i)=(eye(3)-K2(:,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_2theta(:,:,i)=Ktheta2(:,i)*(SumbV_jpda_2(2,2)-SumV_jpda_2(2)*SumV_jpda_2(2)')*Ktheta2(:,i)';
% P_jpda_c_2theta(:,:,i)=(eye(3)-Ktheta2(:,i)*C0kcv)*P_jpda_cvkk1_2theta(:,:,i);
% P_jpda_cvk_2theta(:,:,i)=beta0_2*P_jpda_cvkk1_2theta(:,:,i)+(1-beta0_2)*P_jpda_c_2theta(:,:,i)+P_jpda_p_2theta(:,:,i);
%
% P_jpda_p_2q(:,:,i)=K2q(:,i)*(SumbV_jpda_2(3,3)-SumV_jpda_2(3)*SumV_jpda_2(3)')*K2q(:,i)';
% P_jpda_c_2q(:,:,i)=(eye(3)-K2q(:,i)*C0kcv)*P_jpda_cvkk1_2q(:,:,i);
% P_jpda_cvk_2q(:,:,i)=beta0_2*P_jpda_cvkk1_2q(:,:,i)+(1-beta0_2)*P_jpda_c_2q(:,:,i)+P_jpda_p_2q(:,:,i);
end
% 判断各算法对2两目标的估计是否落入3sigma门限内
for i=dataassociation_begintime:time
Vcv_jpda_1(:,i)=[X_jpda_cvk_1(1,i);X_jpda_cvk_1(2,i);X_jpda_cvk_1(3,i)]-[xm_1(i);ym_1(i);zm_1(i)]; % ??
Vcv_jpda_2(:,i)=[X_jpda_cvk_2(1,i);X_jpda_cvk_2(2,i);X_jpda_cvk_2(3,i)]-[xm_2(i);ym_2(i);zm_2(i)];
%判断目标1的估计是否落入3sigma门限内
if Vcv_jpda_1(:,i)'*inv(Rmeasure_1(:,:,i))*Vcv_jpda_1(:,i)<gama
estimationfallintogatecv_jpda_1(i)=1;
else
estimationfallintogatecv_jpda_1(i)=0;
end
%判断目标2的估计是否落入3sigma门限内
if Vcv_jpda_2(:,i)'*inv(Rmeasure_2(:,:,i))*Vcv_jpda_2(:,i)<gama
estimationfallintogatecv_jpda_2(i)=1;
else
estimationfallintogatecv_jpda_2(i)=0;
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
D1=sqrt((Z1(1)-Xkk1(1))^2);
D2=sqrt((Z2(1)-Xkk1(1))^2);
V=[D1,D2];
[D,I]=min(V);
switch I
case 1
Z=Z1;
otherwise
Z=Z2;
end⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -