最近邻航迹关联算法 最近邻航迹关联算法

% close all; % clear all; T=1; N=50; Pd=0.9; ClutterD=0.6; Gama=9; deta = 100; load TrueTrack common X1p=[];X1v=[];Y1p=[];Y1v=[]; X2p=[];X2v=[];Y2p=[];Y2v=[]; X3p=[];X3v=[];Y3p=[];Y3v=[]; for MonteN=1:1 %generate array for storing data x1p=[];x1v=[];y1p=[];y1v=[]; x2p=[];x2v=[];y2p=[];y2v=[]; x3p=[];x3v=[];y3p=[];y3v=[]; X1=[];X2=[];X3=[]; %Initialize Variance P0 state X0 estimates x10 = TrueX1(:,1)-[TrueX1(2,1)*T 0 TrueX1(4,1)*T 0]'; x20 = TrueX2(:,1)-[TrueX2(2,1)*T 0 TrueX2(4,1)*T 0]'; x30 = TrueX3(:,1)-[TrueX3(2,1)*T 0 TrueX3(4,1)*T 0]'; P10=diag([10000,100,10000,100]); P20=diag([10000,100,10000,100]); P30=diag([10000,100,10000,100]); %generate messurements [Z,Vol] = Messurement(Pd,ClutterD,TrueX1,TrueX2,TrueX3,deta,N); for t=1:N %function Z = messurement1(x1,x2,x3,zP1,zP2,zP3,H,Pd,deta,ClutterD) % z=messurement1(TrueX1(:,t),TrueX2(:,t),TrueX3(:,t),H*F*x10,H*F*x20,H*F*x30,H,Pd,deta,ClutterD); z = Z{t}; %function [XE,PE,SS] = KalamFilter(F,Q,H,R,X,P,z) %[X1E,P1E,ss] = KalamFilter(F,Q,H,R,x10,P10,z); %function [XE,PE]=NNF(F,Q,P,H,R,X,Z,Gama) [X1E,P1E,ValidateEchoNum] = NNF(F,Q,P10,H,R,x10,z,Gama); ValidateEchoNum [X2E,P2E,ValidateEchoNum] = NNF(F,Q,P20,H,R,x20,z,Gama); ValidateEchoNum [X3E,P3E,ValidateEchoNum] = NNF(F,Q,P30,H,R,x30,z,Gama); ValidateEchoNum X1(:,t) = X1E; X2(:,t) = X2E; X3(:,t) = X3E; x10 = X1E; x20 = X2E; x30 = X3E; P10 = P1E; P20 = P2E; P30 = P3E; end x1p=X1(1,:);x1v=X1(2,:);y1p=X1(3,:);y1v=X1(4,:); x2p=X2(1,:);x2v=X2(2,:);y2p=X2(3,:);y2v=X2(4,:); x3p=X3(1,:);x3v=X3(2,:);y3p=X3(3,:);y3v=X3(4,:); %function lost=judgelost(TrueX,xp,yp) %---------for 1th target---------- Flag = judgelost(TrueX1,x1p,y1p,deta); if Flag ==0 X1p=[X1p;x1p];X1v=[X1v;x1v]; Y1p=[Y1p;y1p];Y1v=[Y1v;y1v]; end %--------for 2th target----------- Flag = judgelost(TrueX2,x2p,y2p,deta); if Flag ==0 X2p=[X2p;x2p];X2v=[X2v;x2v]; Y2p=[Y2p;y2p];Y2v=[Y2v;y2v]; end %---------for 3th target---------- Flag = judgelost(TrueX3,x3p,y3p,deta); if Flag ==0 X3p=[X3p;x3p];X3v=[X3v;x3v]; Y3p=[Y3p;y3p];Y3v=[Y3v;y3v]; end end [LostTrack1N,x1p_RSME,x1v_RSME,y1p_RSME,y1v_RSME,x1p_AllRSME,x1v_AllRSME,y1p_AllRSME,y1v_AllRSME,x1p_AllME,y1p_AllME]= statistic(TrueX1,X1p,X1v,Y1p,Y1v,MonteN); [LostTrack2N,x2p_RSME,x2v_RSME,y2p_RSME,y2v_RSME,x2p_AllRSME,x2v_AllRSME,y2p_AllRSME,y2v_AllRSME,x2p_AllME,y2p_AllME]= statistic(TrueX2,X2p,X2v,Y2p,Y2v,MonteN); [LostTrack3N,x3p_RSME,x3v_RSME,y3p_RSME,y3v_RSME,x3p_AllRSME,x3v_AllRSME,y3p_AllRSME,y3v_AllRSME,x3p_AllME,y3p_AllME]= statistic(TrueX3,X3p,X3v,Y3p,Y3v,MonteN); % %printf('') % a=Z{N}; % figure(1) % plot(X1(1,:),X1(3,:),'r'); % hold on % plot(TrueX1(1,:),TrueX1(3,:),'b') % hold on % plot(X2(1,:),X2(3,:),'r'); % hold on % plot(TrueX2(1,:),TrueX2(3,:),'b') % hold on % plot(X3(1,:),X3(3,:),'r'); % hold on % plot(TrueX3(1,:),TrueX3(3,:),'b') % hold on % plot(a(1,:),a(2,:),'+'); % % % figure(2) % plot(x1p_RSME,'r'); % hold on % plot(y1p_RSME,'b') % % figure(3) % plot(x2p_RSME,'r'); % hold on % plot(y2p_RSME,'b') % % figure(4) % plot(x3p_RSME,'r'); % hold on % plot(y3p_RSME,'b')