global mode_config
addpath 'tools'

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Configuration parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mode_config.dead_reckoning=0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% the number of particles
N=200;%size(imu,2);%5000;

% the initial weights
weight=1/N.*ones(1,N);

% initialize the particles
sample_ind=round(1:size(imu,2)/N:size(imu,2));
sample_rot=rot_t(sample_ind',:)';
sample_ind=1:N;


rot_seq=[];
mean_rot=zeros(1,3);

for i=2:size(a,1)
    % delta t
    delta_t=tstamp(i)-tstamp(i-1);
    if mode_config.dead_reckoning
        mean_rot=mean_rot+w(i-1,:).*delta_t;
    else
    % Rotation estimation
    sample_rot=sample_rot+repmat((w(i-1,:).*delta_t)',1,N);   
    % gravity estimation
    a_est=grav_est(sample_rot');
    % Error calculation
    a_err=sum((repmat(a(i,:),N,1)-a_est).^2,2).^.5;

    % Resample cretia     
    if min(a_err)>0.4
        weight=1/N.*ones(1,N);
        sample_ind=round(1:size(imu,2)/N:size(imu,2));
        sample_rot=rot_t(sample_ind,:)';
        sample_ind=1:N;
    end
    % weight set according to the error
    weight=1./a_err;
    % normalize the weights
    weight=weight./sum(weight);
    
    % particle resample
    sample_ind=residualR(sample_ind,weight);
    sample_rot=sample_rot(:,sample_ind);%rot_t(sample_ind,:)';
    % estimation set to the weighted sum of the particles
    mean_rot=[sum(sample_rot(1,:)'.*weight),sum(sample_rot(2,:)'.*weight),sum(sample_rot(3,:)'.*weight)];
    % save the sequence for analysis
    rot_seq=[rot_seq;mean_rot];
    end
    % plot the result
    plot_contrast(vicon,mean_rot,sample_rot,test_vicon(1,i).R,i);
    
end