clear all;close all;clc;% warning off all;T_step   = 1;	Q_delte  = 1e-4;	%8e1;%1e-4;Q_deturn = 1e-10;	%1e0;%1e-10;fai_theta = 0.5*pi/180;	%0.5*pi/180;fai_r = 10;         %10;Np       = 300;Lamdanoise = 4e-4;	%30;%10;%15;%4*1e-4;no_run   = 1;     % 蒙特卡洛次数D    = 5; %T    = T_step;T0   = 100;x1_true = zeros(D,T0);x2_true = zeros(D,T0);x1_true(:,1) = [0;5;0;-2;0];   % 第一帧时刻的 位置 和 速度 信息 x2_true(:,1) = [530;1;600;-5;0];F1 = [ 1 T_step 0 0;       0   1     0 0;       0   0     1 T_step;       0   0     0 1 ];%%  真实轨迹for t = 2:100    x1_true(1:4,t) = F1 * x1_true(1:4,t-1);endfor t = 2:100    x2_true(1:4,t) = F1 * x2_true(1:4,t-1);endX = cat(3,x1_true,x2_true);      % a=cat(3,A,B) 左括号后的3表示构造出的矩阵维数 合成figure(‘color‘,‘w‘);plot(x1_true(1,:),x1_true(3,:),‘g-o‘,x2_true(1,:),x2_true(3,:),‘r-o‘);T        = 100;D        = 6;Ps       = 0.99;    % 存在概率Pd       = 1.00;   %   N2       = Np/2;Nb       = 30;      % 新生粒子数Nt       = 2;       % 目标个数X_o      = -5000;Y_o      = -5000;A1       = [T_step^3/3 T_step^2/2;            T_step^2/2   T_step  ];F1       = [1 T_step;0 1];   F_const  = blkdiag(F1,F1,1);                                % 状态转移矩阵，多加了一维幅度Q_const  = blkdiag(A1,A1,0)*Q_delte;                        % Q_turn   = blkdiag(A1*Q_delte,A1*Q_delte,T_step*Q_deturn);Matrxi_T = [0.9 0.1;0.1 0.9];Q        = cat( 3,sqrtm(Q_const),sqrtm(Q_turn) );zabopdf  = Lamdanoise/(10000*pi);                           % 杂波%  Measurement Modelh1       = inline(‘sqrt((x(1,:)-X_o).^2+(x(D-3,:)-Y_o).^2) ‘,‘x‘,‘D‘,‘X_o‘,‘Y_o‘);h2       = inline(‘ atan((x(D-3,:)-Y_o)./(x(1,:)-X_o)) ‘,‘x‘,‘D‘,‘X_o‘,‘Y_o‘);R        = diag([fai_r fai_theta]);True_Num = zeros(1,T);for i = 1:10    True_Num(i) = 2;endfor i = 11:90    True_Num(i) = 2;endfor i = 91:100    True_Num(i) = 2;end%  Probability Model概率模型Pe       = inline(‘1/( sqrt(2*pi)*deta )*exp( -0.5*x.^2/deta^2 )‘,‘x‘,‘deta‘);Pe_AP    = inline(‘1/( sqrt(2*pi)^D*sqrt(det(deta)) )*exp( -0.5*ctranspose(x)/deta*x )‘,‘x‘,‘deta‘,‘D‘);%新生目标高斯和% Jrk = 2;%wr = zeros(1,2);%wr(1) = 0.01; wr(2) = 0.01;%mr = zeros(5,2);%mr(:,1) = x1_true(:,1); %mr(2,1) = 0;mr(4,1) = 0;mr(5,1) = 0;%mr(:,2) = x2_true(:,1);      % Pr = diag([100^2,5^2,100^2,5^2,0.001^2]‘);%pai_new = [0.5,0.5];                      %新生目标对应两个模型的概率V_max = 5;time_PPHD_D  = zeros(no_run,T);            % 存储时间矩阵for ll = 1 : no_run    rand(‘state‘,sum(100*clock));    randn(‘state‘,sum(100*clock));    %% 第一帧初始化        Num_noise = poissrnd(Lamdanoise);    X0        =  x1_true(:,1);                                      %假设已知第一时刻的位置？？？    M         = size( X0,2 );                               % 初始目标数，目标数从列数可以看出    ZD_ini    = [ feval(h1,X0,D,X_o,Y_o);feval(h2,X0,D,X_o,Y_o) ] + R*randn(2,M);  % R是噪声协方差。初始时刻的观测量，这里分别是目标到观测站的距离和角度    ZD_noise  = [25000*rand(1,Num_noise);pi/2*rand(1,Num_noise)];                  % 第一帧的虚警    ZD_old    = [0;0];    InitialParticle1  = x1_true(:,1)*ones(1,Np) + Q(:,:,1)*randn(D-1,Np);   % 目标1 的粒子 5*300  Q(:,:,1)=sqrtm(Q_const)    InitialParticle2  = x2_true(:,1)*ones(1,Np) + Q(:,:,2)*randn(D-1,Np);   % 目标2 的粒子 5*300  Q(:,:,2)=sqrtm(Q_turn)    PHD_D     = [ InitialParticle1,InitialParticle2 ];                      % 所有初始化 粒子 5*600    PHD_D     = cat(1,PHD_D,[1*ones(1,Np),2*ones(1,Np)]);   %    按列 5*600 + 1*600=6*600    w         = 1/Np*ones(1,2*Np);                          %    1*600    %% 第二帧到最后    for t = 2 : T        Num_noise = poissrnd(Lamdanoise);                                     ZD_noise  = [25000*rand(1,Num_noise);pi/2*rand(1,Num_noise)];  % 以后每一帧产生的随机虚警噪声        X0        = squeeze( X(:,t,:) );      %      除去size为1的维度 5*2        M         = size( X0,2 );             % M=2        ZD        = [ feval(h1,X0,D,X_o,Y_o);feval(h2,X0,D,X_o,Y_o) ] + R*randn(2,M);         ZD        = cat( 2,ZD,ZD_noise );     %2*2                tic;        A       = [1 T_step;0 1] ;        F_const = blkdiag(A,A,1);                       %状态转移矩阵        R1      = R;                                    %diag([10 0.5*pi/180]);                        M          = size(ZD_old,2);        PHD_brith_Pre = zeros(D,Nb*M);      % 新生预测        for m = 1 : M            x      = ZD_old(1,m)*cos( ZD_old(2,m) ) + X_o;            %上一时刻的观测量，距离和角度信息            y      = ZD_old(1,m)*sin( ZD_old(2,m) ) + Y_o;            var_x  = (ZD_old(1,m)*R(2,2)*sin(ZD_old(2,m)))^2 + (R(1,1)*cos(ZD_old(2,m)))^2;            var_y  = (ZD_old(1,m)*R(2,2)*cos(ZD_old(2,m)))^2 + (R(1,1)*sin(ZD_old(2,m)))^2;            cov_xy = (R(1,1)^2 - (ZD_old(1,m)*R(2,2))^2)*sin(ZD_old(2,m))*cos(ZD_old(2,m));            R_Z    = [var_x,cov_xy;cov_xy,var_y];            Begin  = (m-1)*Nb + 1;            End    = m * Nb;            Rej_S  = [x;y]*ones(1,Nb) + sqrtm(R_Z)*randn(2,Nb); % 状态，坐标            Rej_V  = 2*V_max*randn(2,Nb) - V_max;               % 速度            Rej_w  = zeros(1,Nb);   Rej_Model = zeros(1,Nb);    % 权重            for n = 1 : Nb                %  选择目标运动模型                if( 0.5 < rand )                    Rej_w(n)  = 0;                    Rej_Model(n) = 1;                else                    Rej_w(n)  = rand/25-0.02;                    Rej_Model(n) = 2;                end            end            PHD_brith_Pre(:,Begin:End) = [Rej_S(1,:);Rej_V(1,:);Rej_S(2,:);Rej_V(2,:);Rej_w;Rej_Model];        end        N_brith     = size( PHD_brith_Pre,2 );        w_brith     = 0.01/N_brith*ones(1,N_brith);   % 新生粒子的权重服从均匀分布，0.01就是新生目标的PHD        PHD_extend  = [ PHD_D,PHD_brith_Pre ];   % 两部分的粒子，        w_extend    = [ Ps*w,w_brith ];          % （1*600）两部分的权重，继续存在和新生粒子权重预测。按照公式（29）        N_extend    = length(w_extend);          % 权重的维数（600）                L_Q  = size( Q,1 );             %   行数        PHD_Pre   = zeros(D,N_extend);  %   PHD 预测 PHD就是离散化的粒子       %% 预测        for n = 1 : N_extend            %  Prediction of particle state预测            PHD_Pre(1:D-1,n) = F_const*PHD_extend(1:D-1,n) + sqrt(Q(:,:,1))*randn(L_Q,1);            PHD_Pre(end,n) = 1;  %最后一行给1        end        %% 更新        Z_Pre = [ feval(h1,PHD_Pre,D,X_o,Y_o);feval(h2,PHD_Pre,D,X_o,Y_o) ];  %2*600，一步预测的预测和角度信息        M     = size( ZD,2 );                                                 % 2，列数        w     = zeros(M+1,N_extend);        w(M+1,:) = (1-Pd)*w_extend;        for m = 1 : M            pian  = Z_Pre - ZD(:,m)*ones(1,N_extend);                         % 预测数据和测量数据的偏差            lik   = feval(Pe,pian(1,:),R(1,1)).*feval(Pe,pian(2,:),R(2,2));   % 量测似然函数            w_lik = Pd*w_extend.*lik;                                         % 分子            w(m,:)= w_lik./( zabopdf + sum(w_lik) );   % 权重更新        end        num   = sum(w,2);        N     = round( num*Np );                %%   Resampling Processing        PHD_D = [];        Count_PPHD = [];        B = 1;        Num = 0;        for m = 1:M            if (N(m) > 0)%                 [P_mid,w_pre,index] = Resample_P_PHD(PHD_Pre,w(m,:),N(m));                w_in = w(m,:);P_in = PHD_Pre;N_re = N(m);                                q             = cumsum( w_in./sum(w_in) );  l  = 1;                L             = size( w_in,2 );   k  = 1;                hp            = zeros(1,L);                %  Pick the Particles                for n = 1 : N_re                    u         = rand/N_re + (n-1)/N_re;                    while ( q(k) < u )                        k     = k + 1;                    end                        if ( l == k )                        hp(l) = hp(l) + 1;                    else                        in    = find( max(w_in(l+1:k)) == w_in(l+1:k) );                        in1   = in(1) + l;                        hp(in1)= hp(in1) + 1;                        l     = k;                    end                end                index1        = find( hp ~= 0 );                P_mid         = P_in( :,index1 );                w_pre         = w_in( index1 );                index         = hp( index1 );                                Num = Num + sum(w_pre);                L = length(w_pre);                Rej1 = [];                L1 = sum(index);                for n = 1:L                    Rej0 = repmat(P_mid(1:5,n),1,index(n)) + Q(:,:,P_mid(6,n))*randn(5,index(n));                    Rej1 = cat(2,Rej1,[Rej0;P_mid(6,n)*ones(1,index(n))]);                end                PHD_D = cat(2,PHD_D,Rej1);                Count_PPHD = cat(2,Count_PPHD,[B;B+L1-1]);                B = B + L1;            end        end        B = size(PHD_D,2);        w = Num/B*ones(1,B);        ZD_old   = ZD;        %  Estimating the state of target        L    = round( sum(w) );%对sum求和，然后四舍五入，L为目标的个数        if( L > 0 )            Num_PHD = Count_PPHD(2,:) - Count_PPHD(1,:);            PHD_S_D   = zeros(D-1,L);            for n = 1:L                N_max = max(Num_PHD);                index = find(N_max == Num_PHD);                index_PPHD = Count_PPHD(1,index(1)):Count_PPHD(2,index(1));                PHD_S_D(:,n) = mean(PHD_D(1:end-1,index_PPHD),2);                Num_PHD(index(1)) = 0;            end        else            PHD_S_D   = [];        end        time_PPHD_D(ll,t) = toc;                figure(1);        hold on;        for abc = 1:size(PHD_S_D,2)            plot(PHD_S_D(1,abc),PHD_S_D(3,abc),‘b-+‘);        end    endendfprintf(‘Time of P-PHD-Div:%fs\n‘,mean( mean(time_PPHD_D) )*T);