JPDAF.rar_JPDAF_jpda

function JPDAF(target_position,n,T,MC_number,c) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序功能 :采用JPDA数据关联算法实现两个个匀速运动目标的点迹与航迹的关联 % 输入变量 : % -target_position: 目标的初始位置 % - n: 采样次数 % - T: 采样间隔 % -MC_number:仿真次数 % - c: 目标个数 % 输出变量 : % 无 % 参考文献 : % 黄玲,数据挖掘及融合技术研究与应用,西北工业大学硕士学位论文,2004年 % 声明 ： % 该代码为作者毕业设计内容，鉴于学术交流的角度，现在公开发布该代码 % 该代码非本人原创，修改自网上另一位作者的JPDA代码 % 该代码仅用于学术交流，请勿用于任何其它商业用途，请大家自觉遵守 % 如果有人用该代码进行不合适的用途，该代码作者不承担任何责任 % 请遵守作者的劳动成果，转载请标明 % 作者邮箱 ： % wangzexun@gmail.com %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% %%%%% 参数定义 %%%%% %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% Pd=1; %检测概率 Pg=0.99; %正确量测落入跟踪门内得概率 g_sigma=9.21; %门限 lambda=2; gamma=lambda*10^(-6); %每一个单位面积(km^2)内产生lambda个杂波 Target_measurement=zeros(c,2,n); %目标观测互联存储矩阵 target_delta=[100 100]; %目标对应的观测标准差 P=zeros(4,4,c); %协方差矩阵 P1=[target_delta(1)^2 0 0 0;0 0.01 0 0;0 0 target_delta(1)^2 0;0 0 0 0.01]; %初始协方差矩阵 P(:,:,1)=P1; P(:,:,2)=P1; A = [1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; %状态转移矩阵 C = [1 0 0 0;0 0 1 0]; %观测矩阵 R=[target_delta(1)^2 0;0 target_delta(1)^2]; %观测协方差矩阵 Q=[4 0;0 4]; %系统过程噪声协方差 G=[T^2/2 0;T 0;0 T^2/2;0 T]; %过程噪声矩阵 x_filter=zeros(4,c,n); %存储目标的各时刻的滤波值 x_filter1=zeros(4,c,n,MC_number); %MC_number次Montle Carlo仿真所得全部结果存储矩阵 data_measurement=zeros(c,2,n); %观测存储矩阵 data_measurement1=zeros(c,4,n); %实际位置坐标x,y矩阵 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%% 产生目标的实际位置 %%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% data_measurement1(:,:,1)=target_position; %实际位置矩阵初始化 for i=1:c for ii=2:n %实际位置 data_measurement1(i,:,ii)=(A*data_measurement1(i,:,ii-1)')'+(G*sqrt(Q)*(randn(2,1)))'; end end a=zeros(1,n); b=zeros(1,n); for i=1:n a(i)=data_measurement1(1,1,i); b(i)=data_measurement1(1,3,i); end plot(a,b,'b-'); hold on; a=zeros(1,n); b=zeros(1,n); for i=1:n a(i)=data_measurement1(2,1,i); b(i)=data_measurement1(2,3,i); end plot(a,b,'r-'); xlabel('x(m)'),ylabel('y(m)'); legend('目标a的实际位置','目标b的实际位置',1); grid; %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% %%%%% 程序主体 %%%%% %%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%% for M=1:MC_number %%%%%%%%%%%%%%%%%%%% %%% 1.产生路径 %%% %%%%%%%%%%%%%%%%%%%% Noise=[]; for i=1:n for j=1:c %各传感器观测的位置 data_measurement(j,1,i)=data_measurement1(j,1,i)+rand(1)*target_delta(j); data_measurement(j,2,i)=data_measurement1(j,3,i)+rand(1)*target_delta(j); end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% 2.产生杂波,并确定有效观测 %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% S=zeros(2,2,c); Z_predic=zeros(2,2); %存储两个目标的观测预测值,即只包括x,y坐标 x_predic=zeros(4,2); %存储两个目标的状态预测值,即包括x,y坐标和x,y方向速度 ellipse_Volume=zeros(1,2); NOISE_sum_a=[]; %存储目标1的杂波 NOISE_sum_b=[]; %存储目标2的杂波 for t=1:n y1=[]; y=[]; Noise=[]; NOISE=[]; for i=1:c if t~=1 x_predic(:,i) = A*x_filter(:,i,t-1); %用前一时刻的滤波值来预测当前的值(kalman滤波的第一个表达式) else x_predic(:,i)=target_position(i,:)'; %第一次采样我们用真实位置当预测值 即就是初始位置 end P_predic=A*P(:,:,i)*A'+G*Q*G'; %更新x_predic协方差矩阵(kalman滤波的第二个表达式) Z_predic(:,i)=C*x_predic(:,i); %提取预测值的x,y坐标，舍弃x,y速度 R=[target_delta(i)^2 0; 0 target_delta(i)^2]; S(:,:,i)=C*P_predic*C'+R; %定义中间变量S ellipse_Volume(i)=pi*g_sigma*sqrt(det(S(:,:,i))); %计算椭圆跟踪门的面积 number_returns=floor(ellipse_Volume(i)*gamma+1); %椭圆跟踪门内的错误回波数 floor(A)将A内的元素取整 side=sqrt((ellipse_Volume(i)*gamma+1)/gamma)/2; %将椭圆跟踪门等效为正方形，并求出正方形边长的二分之一 Noise_x=x_predic(1,i)+side-2*rand(1,number_returns)*side; %在预测值周围产生多余回波。注意：当某一次number_returns小于等于0时会出错，再运行一次即可。 Noise_y=x_predic(3,i)+side-2*rand(1,number_returns)*side; Noise=[Noise_x;Noise_y]; NOISE=[NOISE Noise]; if i==1 NOISE_sum_a=[NOISE_sum_a Noise]; else NOISE_sum_b=[NOISE_sum_b Noise]; end end b=zeros(1,2); b(1)=data_measurement(1,1,t); %x b(2)=data_measurement(1,2,t); %y y1=[NOISE b']; %将接收到的所有的回波存在y1中,包括杂波和观测 b(1)=data_measurement(2,1,t); b(2)=data_measurement(2,2,t); y1=[y1 b']; %当有一个杂波回波时 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% 3.产生观测确认矩阵Q2 %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% m1=0; %记录有效观测个数 [n1,n2]=size(y1); Q1=zeros(100,3); %Q1为聚矩阵 for j=1:n2 flag=0; for i=1:c d=y1(:,j)-Z_predic(:,i); %用所有回波位置-预测位置，即求残差向量 D=d'*inv(S(:,:,i))*d; %求残差向量的范数 if D<=g_sigma %“最近邻”法 flag=1; Q1(m1+1,1)=1; %从第一行开始给Q1赋值 Q1(m1+1,i+1)=1;