function NewHeadPoses = IDcorrection(headPoses,allTimeStamp,axislim)
% [time_stamp person_ID good_head x y z  Qr Qx Qy Qz]
% The time stamp is increasing

% simMeasurementHistory = [];

% Initialisation of the SMC_PHD filters
% the dynamic model : langevin motion model
T = 1/30; % the frame length
% langevin_v1 = 0.72;% exp(-beta*T) beta = 10
% langevin_v2 = 0.7;% nu*sqrt(1-exp(-2*beta*T)), nu = 1

I3 = eye(3);O3 = zeros(3);
F = [I3,I3*T;O3,I3];
Q = [I3*T^3/3,I3*T^2/2;I3*T^2/2,I3*T]*0.1;

% Used in the likelihood model
sigma_meas = 0.02; % The distance of consecutive frame for the fasted person is 0.02.

M_p = 400; % # of particles per persisting target
M_b = 400; % # of particles per new born target

v_k_km1_b = 0.1; % the prior expected number of target births
% uniform clutter intensity
% k_z = 0.2;%v_k_km1_b*5;

P_D = 0.9;
P_S = 0.98;


% initalise the particles at time frame k=0, no persistent particles nor
% newborn particles.
% D0|0(y,0) = 0, persistent particles
w_km1_p = []; x_km1_p = [];
% D0|0(y,1) = 0, newborn particles
w_km1_b = []; x_km1_b = [];


% PlotSetup;




frameIndex = 1;

NewHeadPoses = cell(1,length(allTimeStamp)-1);
for k = 1:length(allTimeStamp)-1
    % Measurements in the current frame as Z, dim*number, e.g., 2*100
    GetDataOfCurrentFrame;
    % Store history
    % simMeasurementHistory{k} = Z;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% prediction step
    % Union of input particle sets
    w_km1 = [w_km1_p,w_km1_b];
    x_km1 = [x_km1_p,x_km1_b];
    N_km1 = length(w_km1); % #of particles in the previous frame
    
    % Random sample approximation of D_k_km1_p(y,0)
    x_k_km1_p = zeros(6,N_km1);
    w_k_km1_p = zeros(1,N_km1);
    for n = 1:N_km1,
        % draw persistent particles using the Langevin transition model
        particle = x_km1(:,n);
        %         temp = zeros(6,1);
        %         temp(1) = particle(1)+T*particle(4);
        %         temp(2) = particle(2)+T*particle(5);
        %         temp(3) = particle(3)+T*particle(6);
        %         temp(4) = langevin_v1*particle(4) + langevin_v2*randn(1);
        %         temp(5) = langevin_v1*particle(5) + langevin_v2*randn(1);
        %         temp(6) = langevin_v1*particle(6) + langevin_v2*randn(1);
        %         x_k_km1_p(:,n) = temp;
        x_k_km1_p(:,n) = F*particle+mvnrnd(zeros(1,6),Q)';
        % Compute weight of each particle
        w_k_km1_p(n) = w_km1(n)*P_S; % now we assume the the surviving probability is position-independent.
    end
    
    % Sample approximation of D_k_km1_b
    N_k_b = M_b*m_k;
    x_k_km1_b = zeros(6,N_k_b);
    w_k_km1_b = zeros(1,N_k_b);
    if N_k_b>0,
        for j = 1:m_k,
            z = Z(:,j);
            x_k_km1_b([1 2 3],(j-1)*M_b+(1:M_b)) = repmat(z,1,M_b)+[randn(1,M_b)*0.02;randn(1,M_b)*0.02;randn(1,M_b)*0.02];
        end
        % the velocity
        x_k_km1_b(4,:) = randn(1,N_k_b)*0.4;
        x_k_km1_b(5,:) = randn(1,N_k_b)*0.2;
        x_k_km1_b(6,:) = randn(1,N_k_b)*0.4;
        
        w_k_km1_b = 1/N_k_b*v_k_km1_b*ones(1,N_k_b);
    end
    
    
    PlotPredictedParticles;
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% update step
    if m_k==0,
        % When there is no measurements in current frame, i.e. m_k=0
        % L(z) = 0;
        % for persistent particles
        if N_km1>1, % there is some persistent particles
            w_k_k_p = (1-P_D)*w_k_km1_p;
            nu_k_p = sum(w_k_k_p);
            % resample N_k_p times from persisting particles
            N_k_p = round(M_p*nu_k_p);
            [x_km1_p,w_km1_p] = resampleParticle(x_k_km1_p,w_k_k_p/nu_k_p,N_k_p,nu_k_p);
        else % there is no persistent particle
            N_k_p = 0;
            x_km1_p = [];
            w_km1_p = [];
        end
        % no birth
        N_k_b = 0;
        x_km1_b = [];
        w_km1_b = [];
    else
        % When there is some measurements in current frame, i.e. m_k>0, so is N_k_b>0
        temp = zeros(N_km1, m_k);
        if N_km1>0
            for j = 1:m_k,
                z = Z(:,j);
                temp2 = repmat(z(:)',N_km1,1)-x_k_km1_p([1 2 3],:)';
                temp2_dis = sqrt(sum(temp2.^2,2));
                temp(:,j) = P_D*mvnpdf(temp2_dis,0,sigma_meas).*w_k_km1_p(:);
                %temp(:,j) = P_D*mvnpdf(temp2,[0 0 0],[0.01,0,0;0,0.01,0;0,0,0.01]).*w_k_km1_p(:);
            end
        end
        
        sum_w_k_km1_b = sum(w_k_km1_b); % second term in equation 42
        
        % total likelihood calculation
        L = k_z+sum_w_k_km1_b+sum(temp,1);
        
        % compute the normalised weight of w_k_k_p
        if N_km1>0
            temp2 = sum(temp./repmat(L,N_km1,1),2);
            temp2 = temp2';
            w_k_k_p = (1-P_D)*w_k_km1_p + temp2;
        else
            w_k_k_p = [];
        end
        
        %         % compute the normalised weight of w_k_k_b
        %         if m_k>1
        %             w_k_k_b = sum(repmat(w_k_km1_b,m_k,1)./repmat(L(:),1,N_k_b),1);
        %         else
        %             w_k_k_b = w_k_km1_b/L(1);
        %         end
        % compute the normalised weight of w_k_k_b
        %         if all(L>0.5)
        tempValue = sum(w_k_km1_b(1)./L);
        w_k_k_b = tempValue*ones(1,N_k_b);
        %         else
        %             w_k_k_b = w_k_km1_b;
        %         end
        
        
        % Estimate number of persistent and new born objects
        if N_km1>0
            % estimated number of persistent objects
            nu_k_p = sum(w_k_k_p);
            N_k_p = round(M_p*nu_k_p);
        else
            nu_k_p = 0;
            N_k_p = 0;
        end
        % estimated number of newborn objects
        nu_k_b = sum(w_k_k_b);
        N_k_b = round(M_b*nu_k_b);
        
        % Resample persistent and new born objects
        if nu_k_p>0,
            % resample N_k_p times from persisting particles
            [x_km1_p,w_km1_p] = resampleParticle(x_k_km1_p,w_k_k_p/nu_k_p,N_k_p,nu_k_p);
        else
            x_km1_p = [];w_km1_p=[];
        end
        
        if nu_k_b>0,
            % resample N_k_b times from newborn particles
            [x_km1_b,w_km1_b] = resampleParticle(x_k_km1_b,w_k_k_b/nu_k_b,N_k_b,nu_k_b);
        else
            x_km1_b = [];w_km1_b=[];
        end
    end
    
    
    PlotUpdatedParticles;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Prune and Estimate step
    %  disp(['At time frame ',num2str(k),', the estimated # person is ',num2str(round(nu_k_p))]);
    EstimateStates;
    
    frameIndex = frameIndex+1;
end

