diff --git a/mr/ub9/loc_framework/localize_pf.m b/mr/ub9/loc_framework/localize_pf.m index 47c92fa..8e8ce21 100644 --- a/mr/ub9/loc_framework/localize_pf.m +++ b/mr/ub9/loc_framework/localize_pf.m @@ -1,71 +1,87 @@ +sume=zeros(3,1); +runs = 1; +for i=10:runs % ### Run some initialization initialization steps common to all filters -localize_init; + localize_init; -% ### Important parameters: -nSamples = 100; -ESSmin = 0.7*nSamples; + % ### Important parameters: + nSamples = 20; + ESSmin = 0.1*nSamples; -% ### Evaluate PF without measurements: -% Estimates are based on control input alone. + % ### 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: + currx = zeros(3,1,nSamples); w = ones(nSamples,1)/nSamples; - -end % for each control input + pf_x = currx; + pf_w = permute(w, [2 3 1]); -plot_trajectory_particles(x_true, pf_x, pf_w, dt); + % 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); -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)) \ No newline at end of file + 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(final_err(1:2)) +merr_yaw = abs(final_err(3)) +%_________________________________ +%Ergebnisse 2 e +%1) ESSmin = 0.7*20; errpos: 0.7532 err_yaw= 0.0112 +%2) ESSmin = 0.4*20; errpos: 0.6017 erryaw=0.0120 +%3)ESSmin = always > ess; errpos: 0.1254 erryaw=0.0654 +%4)ESSmin = 0.1*10= errpos= 0.4363 err_yae 0.0393 +%Sollen wir heir die Zeit auch noch messen? \ No newline at end of file diff --git a/mr/ub9/loc_framework/pf_prediction.m b/mr/ub9/loc_framework/pf_prediction.m index 9049785..1abbba3 100644 --- a/mr/ub9/loc_framework/pf_prediction.m +++ b/mr/ub9/loc_framework/pf_prediction.m @@ -22,20 +22,28 @@ %todo neue matrix mit einem zeitschritt mehr -x = horzcat(x, zeros(size(x,1),1,size(x,3))); -%size(x,2) - j =size(x,2)-1 +% x = horzcat(x, zeros(size(x,1),1,size(x,3))); steht halt echt nirgends +% dass ihr erst danach concatenated +% size(x) +% j =size(x,2)-1; +% for k=1: size(x,3) +% xl=x(:,j,k); +% r=mvnrnd(zeros(size(R,1)),R); +% r2= r(:,1); +% x(:,j+1,k)= xl+motion(xl,u)+r2; +% end +% +% xnew=x; + for k=1: size(x,3) - xl=x(:,j,k); - r=mvnrnd(zeros(size(R,1)),R); - r2= r(:,1); - x(:,j+1,k)= xl+motion(xl,u)+r2; + xl=x(:,1,k); + r= mvnrnd(zeros(1,size(R,2)), R)'; + x(:,1,k)=motion(xl,u)+r; end - - - + xnew=x; + % YOUR CODE ENDS HERE: end diff --git a/mr/ub9/loc_framework/pf_resample.m b/mr/ub9/loc_framework/pf_resample.m index e356eee..87211da 100644 --- a/mr/ub9/loc_framework/pf_resample.m +++ b/mr/ub9/loc_framework/pf_resample.m @@ -15,7 +15,10 @@ % Find first index for which the cumulative sum is >= r. If this ever % fails due to unmerical inaccuracies, we should take the last element. ind = min(find (cum >= r, 1, 'first'), M); - nextx(:,:,p) = currx(:,:,ind); + + + nextx(:,:,p) = currx(:,:,ind); + end end diff --git a/mr/ub9/loc_framework/pf_resample_sys.m b/mr/ub9/loc_framework/pf_resample_sys.m index fb01f81..fc0f86e 100644 --- a/mr/ub9/loc_framework/pf_resample_sys.m +++ b/mr/ub9/loc_framework/pf_resample_sys.m @@ -1,10 +1,29 @@ -function [ nextx ] = pf_resample_sys( currx, w ) +function [ nextx ] = pf_resample_sys( currx, w ,essMin) %PF_RESAMPLE_LV Systematic or low variance resampling % YOUR CODE STARTS HERE: -nextx = currx; +ess= 1/sum(arrayfun(@(x) x^2,w)); +M = size(currx, 3); +if(ess < essMin) + + nextx = zeros(3, 1, M); + + r = rand()*1/M; + c= w(1); + i=1; + for m = 1:M + U= r+(m-1)*1/M; + while U>c + i=i+1; + c=c+w(i); + end + nextx(:,:,m)=currx(:,:,i); + end +else + nextx=currx; +end % YOUR CODE END HERE end