123

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);
end
for t = 2:100
    x2_true(1:4,t) = F1 * x2_true(1:4,t-1);
end
X = 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 Model
h1       = 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;
end
for i = 11:90
    True_Num(i) = 2;
end
for 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
    end
end

fprintf(‘Time of P-PHD-Div:%fs\n‘,mean( mean(time_PPHD_D) )*T);