% ### Run some initialization initialization steps common to all filters
localize_init;

% ### Important parameters:
nSamples = 100;
ESSmin = 0.7*nSamples;

% ### Evaluate PF without measurements: 
%    Estimates are based on control input alone.

currx = zeros(3,1,nSamples);
w = ones(nSamples,1)/nSamples;
pf_x = currx;
pf_w = permute(w, [2 3 1]);

% Pay attention to the representation of particles in pf_x: 
% A trajectory is stored as a 2D matrix [x_0, x_1, x_2, x_3]
% Columns: different time steps
% Trajectories of all particles are concatenated along a third dimension,
% the result is a 3D tensor.
% The pose (3x1 vector) of particle m at timestep i can be accessed using:
%    pf_x(:,i,m);

for i = 1:size(u,2)
    currx = pf_prediction(currx, u(:,i), R, @motion_diff);
  
    pf_x = cat(2,pf_x, currx);
    currw = permute(w, [2 3 1]);
    pf_w = cat(2,pf_w, currw);
end % for each control input

plot_trajectory_particles(x_true, pf_x, pf_w, dt);


% ### Evaluate full PF with measurements:
currx = zeros(3,1,nSamples);
w = ones(nSamples,1)/nSamples;
pf_x = currx;
pf_w = permute(w, [2 3 1]);

for i = 1:size(u,2)
    % Prediction step:
    currx = pf_prediction(currx, u(:,i), R, @motion_diff);

    % Reweighting step:
    w = pf_reweight(currx, w, z_meas(:,i), Q, @measurement);
 
    % Save current estimates to trajectory. Important:
    % It is best to do this after reweighting (to incorporate the latest
    % measurement) but before resampling (because of the sample
    % impoverishment problem).
    pf_x = cat(2,pf_x, currx);
    currw = permute(w, [2 3 1]);
    pf_w = cat(2,pf_w, currw);
    
    % Resampling step:
    currx = pf_resample(currx,w);
    % Do not forget to reset weights after resampling:
    w = ones(nSamples,1)/nSamples;
    
end % for each control input

plot_trajectory_particles(x_true, pf_x, pf_w, dt);

tmp = permute(currx, [3, 1, 2]).*repmat(w,1,3,1)*nSamples;
disp('final result WITH measurements')
final_cov = cov(tmp)
final_mean = mean(tmp)'
final_err = final_mean - x_true(:,end);
err_pos = norm(final_err(1:2))
err_yaw = abs(final_err(3))