PHD.rar_PHD tracking matlab_PHD 多目标跟踪_PHD多目标跟踪_phd 目标跟踪_多目标跟踪phd

clear all; clc; close all; %===============初始参数设定=============== T=1; %采样时间间隔 N=40; %测量时间 M_number=1; %程序运行次数 number_PHD=zeros(1,N); q_noise1=0.5; %状态噪声方差 q_noise2=0.1; %状态噪声方差 q_noise=[q_noise1;q_noise2]; T_prun=1e-5; %修剪门限 U_merg=4; %合并门限 J_max=100; %最大高斯数 N_max=20; %最大目标数 c_ospa=70; %势误差与状态误差的调节因子 p_ospa=2; %OSPA取2阶距离 F=[1,T,0,0 0,1,0,0 0,0,1,T 0,0,0,1]; %存活目标转移矩阵 G=[T^2/2,0 T,0 0,T^2/2 0,T]; %状态噪声的转移矩阵 H=[1,0,0,0 0,0,1,0]; %量测矩阵 r_noise=[0.5;0.5]; %量测噪声标准差 R=[r_noise(1)^2,0 0,r_noise(2)^2]; ps=0.99; %目标存活概率 pd=0.9; %目标检测概率 r=5; %杂波平均数 %===================目标的真实运动轨迹==================== target_position=TARGET_FORM(F,G,q_noise); %目标的真实运动轨迹 target_position.m(2).s=[zeros(4,7),target_position.m(2).s]; target_position.m(3).s=[zeros(4,11),target_position.m(3).s]; target_position.m(4).s=[zeros(4,25),target_position.m(4).s]; %====================存活目标初始化======================= initial.m=target_position.m(1).s(:,1); %目标的初始位置 Q_noise=[q_noise1^2,0 0,q_noise2^2]; initial.P=diag([1 2 1 2]); %目标的初始协方差矩阵 initial.w=1; initial.J=1; %====================新生目标初始化======================= birth.m(:,1)=[0,0,0,0]'; %birth对应于目标出现强度函数 birth.m(:,2)=[-10,0,0,0]'; birth.m(:,3)=[0,0,-5,0]'; birth.P=diag([5 1 5 1]); birth.P=cat(3,birth.P,birth.P,birth.P); %P为4*4*3 double birth.w(1)=0.1; birth.w(2)=0.1; birth.w(3)=0.1; birth.J=3; %新生目标高斯分量数 %======================目标随机集==================================== for i=1:40 x(i).m=[]; for j=1:4 if target_position.n(j,i)==1 x(i).m=[x(i).m,target_position.m(j).s(:,i)];%目标位置的更新 end end end d_PHD=zeros(M_number,N); % for k=1:M_number k=1; %======================生成量测报告======================== Y_measure=MEASUREMENT(H,r_noise,target_position,N,pd,r); %产生测量数据 %========================================================== % 主程序 %========================================================== x_number_PHD=zeros(1,N); x_number_PHD(1)=1; %目标的初始个数 x_filter_PHD=initial; tic for i=2:N; %========================PHD目标状态预测=========================== x_predic_PHD=PREDIC(x_filter_PHD,F,ps,birth,q_noise,G); %========================PHD目标更新=========================== x_filter_PHD=UPDATA_PHD(x_predic_PHD,Y_measure(i),pd,H,R,r); %=====================PHD修剪合并高斯项======================== x_filter_PHD=PRUN_MERG(x_filter_PHD,T_prun,U_merg,J_max); %======================PHD估计目标数目========================= x_number_PHD(i)=(sum(x_filter_PHD.w)); %======================PHD目标状态估计========================= x_estimate_PHD(i).m=PHD_ESTIMATE(x_filter_PHD); if (~isempty(x(i).m)) && (~isempty(x_estimate_PHD)) d_PHD(k,i)=OSPA(x_estimate_PHD(i).m,x(i).m,c_ospa,p_ospa); end end toc number_PHD=number_PHD+x_number_PHD; avg_d_PHD=mean(d_PHD,1); number_PHD=number_PHD/M_number; num=sum(target_position.n); figure(3) hold on plot(num) plot(number_PHD,'k-*') legend('true number','PHD estimate number') xlabel('time'),ylabel('number') axis([0 50 0 5]); figure(4) plot(avg_d_PHD(1:N),'k-'),hold on xlabel('time'),ylabel('d') legend('PHD OPSA Distance')