% We aims to estimate the final state.
% Assume the detections are saved in Kinect detections
% disp(['At time frame ',num2str(k),', the estimated # person is ',num2str(round(nu_k_p))]);

if m_k>0 && N_k_p>0,
    dis = zeros(m_k,N_k_p);
    for i = 1:m_k,
        % find the persistent particles near the detected positions
        % this is appromixated distance------not real distance
        dis(i,:) = sum(abs(repmat(Z(:,i),[1 N_k_p])-x_km1_p([1 2 3],:)),1);
    end
    
    [~,index] = min(dis,[],1);
    disThresIndex = dis<0.5;
    
    wei = zeros(1,m_k);
    for i = 1:m_k,
        useIndex1 = (index==i) & disThresIndex(i,:);
        if any(useIndex1)
            wei(i) = sum(w_km1_p(useIndex1));
        end
    end
    
    currentHeadpose = Z(:,wei>0.5);
    disp(['At time frame ',num2str(k),', the estimated # person is ',num2str(size(currentHeadpose,2))]);
    
    %% address the ID swap problem here
    
    % assign the IDs
    %[currentHeadpose, IDstatus] = assignIDs(currentHeadpose,previouseHeadpose,IDstatus,NewHeadPoses);
    assignIDs;
    PlotTargets;
    
    %     IDstatus
    %     currentHeadpose
    % save the results
    NewHeadPoses{frameIndex} = currentHeadpose;
    % assign the IDs
    previouseHeadpose = currentHeadpose;
end





