%particle filter
% S is the set of Particles=[xj,wj:j=1..M]
clear all;
close all;

k=0;
data;   
[Estimate_bestparticle(:,k+1),Estimate_weightedmean(:,k+1),Estimate_robustmean(:,k+1)]=estimate(S);
plot_particles(S,[xm(k+1),ym(k+1),thetam(k+1)],range_pos,Estimate_bestparticle(:,k+1),Estimate_weightedmean(:,k+1),Estimate_robustmean(:,k+1),parameter);   
mov(k+1)=getframe;
W=S(4,:); % matrix of weights
M=length(W); % M is the number of particles 

while k<steps %exploring
   k=k+1;

%    if(ess(W)<beta*M) % then particle population depleted
%       % display('particle population depleted')
%       % three different kind of resampling algorithms
%       switch resample
%           case 1%select_with_replacement
%                 index=select_with_replacement(W);     
%                 S=S(:,index);
%           case 2%linear_time_resampling
%                 index=linear_time_resampling(W);      
%                 S=S(:,index);
%           case 3%liu_resampling  
%                 index=liu_resampling(W);
%                 S=S(:,index);
%       end
%       resample=1;
%    else 
%       resample=0;
%    end

resample=0;
   
   %prediction after action alpha
   %display('Prediction')
   W=S(4,:);
   M=length(W);
   for(j=1:M)
       %x_k+1(j)=prediction(x_k(j),alpha);
       % method: tutorial of internet
       S(:,j)=rotation(S(:,j),delta_theta(k),M_rot,sigma_rot);
       S(:,j)=translation(S(:,j),rho(k),M_trs,sigma_trs,M_drft,sigma_drft);
       % % method: tutorial kalman
       % S(:,j)=rotation(S(:,j),delta_theta(k),M_rot,sigma_rot);
   end
   sensed=Sense([xm(k+1),ym(k+1),thetam(k+1)],parameter,measurement_noise);   %xm,ym,thetam are the real positions of the moving robot
   
   %update the weigths
   for j=1:M
       switch memory
            case 0
                W(j)=weight(sensed,Sense(S(1:3,j),parameter,0),parameter);
           case 1
                W(j)=W(j)*weight(sensed,Sense(S(1:3,j),parameter,0),parameter);
       end
   end

   %normalize the weigths
   W=W/sum(W);
   S(4,:)=W;  
   M=length(W);
   S;
   [Estimate_bestparticle(:,k+1),Estimate_weightedmean(:,k+1),Estimate_robustmean(:,k+1)]=estimate(S);
   plot_particles(S,[xm(k+1),ym(k+1),thetam(k+1)],range_pos,Estimate_bestparticle(:,k+1),Estimate_weightedmean(:,k+1),Estimate_robustmean(:,k+1),parameter);
   if resample
       text(-range_pos/2+2,range_pos/2-5,'RESAMPLE','Fontsize',18);       
       text(0,0,'RESAMPLE','Fontsize',18);
   else
   end
   mov(k+1)=getframe;
end

plot_Estimate(Estimate_bestparticle,Estimate_weightedmean,Estimate_robustmean);
movie2avi(mov,'filmpje','fps',2,'quality',100)