diff --git a/mr/ub9/loc_framework/localize_ukf.m b/mr/ub9/loc_framework/localize_ukf.m index 557b336..5ddd841 100644 --- a/mr/ub9/loc_framework/localize_ukf.m +++ b/mr/ub9/loc_framework/localize_ukf.m @@ -18,7 +18,7 @@ plot_trajectory_cov(x_true, nomeas_mean, nomeas_cov, dt); -% ## Evaluate full UKF with measurements: +%## Evaluate full UKF with measurements: curr_mean = [0; 0; 0]; curr_cov = diag([0, 0, 0]); ukf_mean = curr_mean; diff --git a/mr/ub9/loc_framework/pf_prediction.m b/mr/ub9/loc_framework/pf_prediction.m index 430ce9d..18d0e05 100644 --- a/mr/ub9/loc_framework/pf_prediction.m +++ b/mr/ub9/loc_framework/pf_prediction.m @@ -6,7 +6,16 @@ % YOUR CODE STARTS HERE: -xnew = x; + +% 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); + +xnew = x+motion(x,u)+; % YOUR CODE ENDS HERE: diff --git a/mr/ub9/loc_framework/ukf_correction.m b/mr/ub9/loc_framework/ukf_correction.m index 0c194d2..b526e96 100644 --- a/mr/ub9/loc_framework/ukf_correction.m +++ b/mr/ub9/loc_framework/ukf_correction.m @@ -7,8 +7,7 @@ %wie bring ich hier das u unter? Z=zeros(size(z,1),size(X,2)); for i =1:size(X,2) - a=measurement(X(:,i)); - Z(:,i)= a(1); + Z(:,i)= measurement(X(:,i)); end [expX,covX]=ukf_estimate_normal( X, w ); [expZ,CovOfIn]=ukf_estimate_normal( Z, w ); diff --git a/mr/ub9/loc_framework/ukf_estimate_normal.m b/mr/ub9/loc_framework/ukf_estimate_normal.m index e183e93..d59915a 100644 --- a/mr/ub9/loc_framework/ukf_estimate_normal.m +++ b/mr/ub9/loc_framework/ukf_estimate_normal.m @@ -11,6 +11,7 @@ %mu = zeros(dim,1); + mu=S*w; diff --git a/mr/ub9/loc_framework/ukf_prediction.m b/mr/ub9/loc_framework/ukf_prediction.m index 2a25d6a..52732be 100644 --- a/mr/ub9/loc_framework/ukf_prediction.m +++ b/mr/ub9/loc_framework/ukf_prediction.m @@ -7,13 +7,14 @@ %Sm= rowfun(motion,S) % By default, rowfun returns the first output of func %wie bring ich hier das u unter? Sm=zeros(size(S)); + for i =1:size(S,2) - a=motion(S(:,i),u); - Sm(:,i)= a(1); + Sm(:,i)= motion(S(:,i),u); end [newmean,newcov]=ukf_estimate_normal( Sm, w ); newcov = newcov+R; + % YOUR CODE ENDS HERE: end diff --git a/mr/ub9/loc_framework/ukf_sigma_points.m b/mr/ub9/loc_framework/ukf_sigma_points.m index 9624a93..a185737 100644 --- a/mr/ub9/loc_framework/ukf_sigma_points.m +++ b/mr/ub9/loc_framework/ukf_sigma_points.m @@ -22,13 +22,13 @@ S(:,1)=mu; w(1)=1/3; -w(2:n)=(1-w0)/(2*n); +w(2:n)=(1-w0)/(2*nx); -RootN= sqrt(n/(1-w0))*sqrtm(cov); -RootN2= sqrt(n/(1-w0))*sqrtm(cov); -for i =2:nx - S(:,i) = mu+RootN(:,i); - S(:,i+nx) = mu-RootN2(:,i); +RootN= sqrt(nx/(1-w0))*sqrtm(cov); + +for i =1:nx + S(:,i+1) = mu+RootN(:,i); + S(:,i+nx+1) = mu-RootN(:,i); end