function [estimate_bestparticle,estimate_weightedmean,estimate_robustmean]=estimate(S)

M=length(S(4,:));
%estimate
% the best particle
[maximum,index]=max(S(4,:));
estimate_bestparticle=S(:,index);
%weighted mean
sum_weightedmean=0;
%robus mean
sum_particles=0;
sum_weights=0;
delta_pos=0.5;
delta_ang=10*pi/180;
count=0;
for j=1:M   
    %weighted mean
    sum_weightedmean=sum_weightedmean+S(:,j)*S(4,j);
    %robus mean
    if sqrt((estimate_bestparticle(1)-S(1,j))^2+(estimate_bestparticle(2)-S(2,j))^2)<delta_pos & abs(estimate_bestparticle(3)-S(3,j))<delta_ang
        sum_particles=sum_particles+S(:,j)*S(4,j);
        sum_weights=sum_weights+S(4,j);
        count=count+1;
    end
end
%weighted mean
estimate_weightedmean=sum_weightedmean;
%robus mean
if count==0
    estimate_robustmean=NaN*ones(4,1);
else    
    estimate_robustmean=sum_particles/sum_weights;
end