sume=zeros(3,1);
runs = 1;
for i=1:runs
% ### 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]); % hier bekommt er doch immer das gleiche gewicht 
        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_sys(currx,w,ESSmin);

        % 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))
    sume= sume+final_err;
end
mae= sume/runs
merr_pos = norm(mae(1:2))
merr_yaw = abs(mae(3))
%_________________________________
%Ergebnisse 2 e
%1) ESSmin = 0.7*20;  errpos: 0.5078 err_yaw= 0.0464
%2) ESSmin = 0.4*20;  errpos: 0.6882   erryaw=0.1149
%3)ESSmin = always > ess;  errpos: 0.5342   erryaw=0.0174
%4)ESSmin = 0.1*20= errpos= 0.8369  err_yae 0.1608
%Sollen wir hier die Zeit auch noch messen?