phdfilter.zip_序贯蒙特卡洛_概率假设密度_滤波算法_粒子群代码_蒙特卡洛算法

%==== PHD filtering: % run this routine after you call 'declare_problem' and signal generation % has modified for multiple birth Lmax= 1e5; %max. allowable particles rho= 1000; %no. of particles per survived target Jk= L_b*rho; %no. of particles for birth targets %--- Lk= 1; hat_N= zeros(K,1); %hard estimate of the number of targets hat_N_soft= zeros(K,1); %soft estimate of the number of targets hat_X= cell(K,1); particle_X= cell(K,1); %cell for storing all particles particle_W= cell(K,1); %cell for storing the update weights tilde_Xk= zeros(x_dim,1); Wk_resample= 0; for k=1:K time_start= cputime; %---particle generation tilde_Xk= [ gen_phistate_intensity(model,tilde_Xk,P_death) gen_birthstate_intensity(model,Jk) ]; Wk_predict= [ Wk_resample*(sum(model.lambda_s)+(1-P_death)); ones(Jk,1)*sum(model.lambda_b)/Jk ]; %---weight update Zk_len= size(Z{k},2); Gk= zeros(Zk_len,Lk+Jk); %likelihood function matrix Ck_z= zeros(Zk_len,1); Uk_z= zeros(Zk_len,1); %Uk_Z is denoted to be the denominator of the update eqn. for i=1:Zk_len Gk(i,:)= compute_likelihood(model,Z{k}(:,i),tilde_Xk);%g= [g(z|x_1), ... , g(z|x_M)] Ck_z(i)= P_D*(Gk(i,:)*Wk_predict); Uk_z(i)= lambda_c/prod(range_c(:,2)-range_c(:,1))+Ck_z(i);% k(z)+<phi(x),alpha> %note that we assume z always lies in the clutter region end; Wk_update= (P_D*(Gk'*(1./Uk_z))+ (1-P_D)).*Wk_predict;% update psi(x).(1-P_D)=v(x);phi(x)=P_D*g(z|x) %particle_X{k}= tilde_Xk; %particle_W{k}= Wk_update; %---resampling hat_N_soft(k)= sum(Wk_update); if hat_N_soft(k)> 1e-12, %resampling Lk= min(ceil(hat_N_soft(k))*rho,Lmax); idx= resample(Wk_update/hat_N_soft(k),Lk);%return the index of weight member tilde_Xk_resample= tilde_Xk(:,idx); tilde_Xk= tilde_Xk_resample; Wk_resample= hat_N_soft(k)*ones(Lk,1)/Lk;%? else %no resampling; everything reinitialize Lk= 1; tilde_Xk= zeros(x_dim,1); Wk_resample= 0; end; particle_X{k}= tilde_Xk; particle_W{k}= Wk_resample; %---clustering and hard decision [x_c,I_idx]= our_kmeans(particle_X{k},particle_W{k},1); hat_N(k)= 0; hat_N_temp= size(x_c,2); for el=1:hat_N_temp if sum(particle_W{k}(I_idx{el})) > .5,%?? hat_N(k)= hat_N(k)+1; hat_X{k}= [ hat_X{k} x_c(:,el) ]; end; end; %--- display disp(['time= ',num2str(k),' Soft hat_N(k)= ',num2str(hat_N_soft(k)),... ' Hard hat_N(k)= ',num2str(hat_N(k)), ' True N(k)= ',num2str(N_true(k)), ... ' Neff= ',num2str(1/sum(Wk_update.^2)),... ' Ns= ',num2str(length(Wk_update)), ... ' CPU time= ',num2str(cputime-time_start)]); end;