diff --git a/ea/project/overview.html b/ea/project/overview.html
new file mode 100644
index 0000000..199461e
--- /dev/null
+++ b/ea/project/overview.html
@@ -0,0 +1,1055 @@
+overviewEA Projekt
+Problemstellung
+
+- Volleyball: Erziehle ein Ass (Punkt durch Angabe ohne Gegnerberührung)
+
+Lösungsrepräsentation
+
+- Ziel (x,y), wo stehen wenige Spieler
+- Kraft + Spin f, schränkt Zielgenauigkeit ein (noise)
+- jeweils reelle Zahlen
+- 0 <= x <= 9
+- 0 <= y <= 9
+- 0 < f <= 10
+
+Fitnessfunktion
+
+- Verteilung der Spieler auf dem Feld
+- jeder Spieler fängt Bälle in seiner Umgebung normalverteilt ab
+- Fitnessfunktion ist also die Summe der Gauß-Glocken der Spieler
+
+Schwierigkeit
+
+- mehrere Minima, in die optimiert werden könnte
+- noise durch Kraft + Spin des Aufschlags lässt
+
+Techniken
+
+- Evolutionsstrategien, zur Optimierung der drei reellen Zahlen der Lösung
+
+Opetaroren
+Mutation
+
+Crossover
+
+- sinnvoll für gute Position und gute Kraft
+- problematisch bei gutem x- und gutem y-Wert
+
\ No newline at end of file
diff --git a/ea/project/overview.md b/ea/project/overview.md
new file mode 100644
index 0000000..f5ffbbf
--- /dev/null
+++ b/ea/project/overview.md
@@ -0,0 +1,31 @@
+# EA Projekt
+## Problemstellung
+- Volleyball: Erziehle ein Ass (Punkt durch Angabe ohne Gegnerberührung)
+
+## Lösungsrepräsentation
+- Ziel (x,y), wo stehen wenige Spieler
+- Kraft + Spin f, schränkt Zielgenauigkeit ein (noise)
+- jeweils reelle Zahlen
+- 0 <= x <= 9
+- 0 <= y <= 9
+- 0 < f <= 10
+
+## Fitnessfunktion
+- Verteilung der Spieler auf dem Feld
+- jeder Spieler fängt Bälle in seiner Umgebung normalverteilt ab
+- Fitnessfunktion ist also die Summe der Gauß-Glocken der Spieler
+
+### Schwierigkeit
+- mehrere Minima, in die optimiert werden könnte
+- noise durch Kraft + Spin des Aufschlags lässt
+
+## Techniken
+- Evolutionsstrategien, zur Optimierung der drei reellen Zahlen der Lösung
+
+## Opetaroren
+### Mutation
+- Bei ES...
+
+### Crossover
+- sinnvoll für gute Position und gute Kraft
+- problematisch bei gutem x- und gutem y-Wert
diff --git a/mr/ub9/loc_jp/get_angle_bracket.m b/mr/ub9/loc_jp/get_angle_bracket.m
new file mode 100644
index 0000000..89daaf0
--- /dev/null
+++ b/mr/ub9/loc_jp/get_angle_bracket.m
@@ -0,0 +1,19 @@
+function [ p ] = get_angle_bracket( mean, cov )
+%GET_ANGLE_BRACKET Compute points visualizing 95% orientation uncertainty
+
+scale = 3; % in multiples of sigma
+length = 0.5; % length of both arms of the "bracket"
+
+p = zeros(2,3);
+
+theta = mean(3);
+sigmaTheta = sqrt(cov(3,3)); % marginalized standard deviation of theta
+th1 = theta - scale*sigmaTheta;
+th3 = theta + scale*sigmaTheta;
+
+p(:,2) = mean(1:2);
+p(:,1) = mean(1:2) + length*[cos(th1); sin(th1)];
+p(:,3) = mean(1:2) + length*[cos(th3); sin(th3)];
+
+end
+
diff --git a/mr/ub9/loc_jp/get_ellipse.m b/mr/ub9/loc_jp/get_ellipse.m
new file mode 100644
index 0000000..83342f4
--- /dev/null
+++ b/mr/ub9/loc_jp/get_ellipse.m
@@ -0,0 +1,28 @@
+function [ p ] = get_ellipse( mean, cov, scale )
+%GET_ELLIPSE Compute points on uncertainty ellipse
+
+if nargin < 3
+ scale = 3.0;
+end
+
+% Compute eigenvalues and eigenvectors of marginalized position covariance
+[V D] = eig(cov(1:2,1:2));
+[D order] = sort(diag(D),'descend'); %sort eigenvalues in descending order
+V = V(:,order);
+evec_max = V(:, 1); % eigenvector with alrgest eigenvalue
+
+% Construct uncertainty ellipse in parallel to eigenvectors
+a = scale*sqrt(D(1));
+b = scale*sqrt(D(2));
+theta_grid = linspace(0,2*pi);
+ellipse_x_r = a*cos( theta_grid );
+ellipse_y_r = b*sin( theta_grid );
+
+% Transform to world frame
+phi = atan2(evec_max(2), evec_max(1)); % Angle between x and largest EV
+R = [cos(phi) -sin(phi); sin(phi) cos(phi)];
+p = R*[ellipse_x_r;ellipse_y_r];
+p = p + repmat(mean(1:2),1,size(p,2));
+
+end
+
diff --git a/mr/ub9/loc_jp/localize_ekf.m b/mr/ub9/loc_jp/localize_ekf.m
new file mode 100644
index 0000000..03bdad9
--- /dev/null
+++ b/mr/ub9/loc_jp/localize_ekf.m
@@ -0,0 +1,57 @@
+% ### Run some initialization initialization steps common to all filters
+localize_init;
+
+% ## Evaluate EKF without measurements:
+% Estimates are based on control input alone.
+curr_mean = [0; 0; 0];
+curr_cov = diag([0, 0, 0]);
+
+nomeas_mean = curr_mean;
+nomeas_cov = reshape(curr_cov, 9, 1);
+for i = 1:size(u,2)
+ % Update mean and Jacobian G:
+ [curr_mean G] = motion_diff(curr_mean, u(:,i));
+
+ % Update covariance matrix
+ curr_cov = G*curr_cov*(G') + R;
+
+ % Store result for plotting
+ nomeas_mean = [nomeas_mean, curr_mean];
+ nomeas_cov = [nomeas_cov, reshape(curr_cov, 9, 1)];
+end % for each u
+plot_trajectory_cov(x_true, nomeas_mean, nomeas_cov, dt);
+
+
+% ## Full EKF: Estimates based on control input and measurements.
+curr_mean = [0; 0; 0];
+curr_cov = diag([0. 0, 0]);
+
+ekf_mean = curr_mean;
+ekf_cov = reshape(curr_cov, 9, 1);
+for i = 1:size(u,2)
+ % Prediction step:
+ [curr_mean G] = motion_diff(curr_mean, u(:,i));
+ curr_cov = G*curr_cov*(G') + R;
+
+ % Correction step:
+ % Expected measurement:
+ [z_exp, H] = measurement(curr_mean);
+
+ % Kalman gain:
+ K = curr_cov * (H') * inv(H*curr_cov*(H') + Q);
+ curr_mean = curr_mean + K*(z_meas(:,i) - z_exp);
+ curr_cov = (eye(3) - K*H)*curr_cov;
+
+ % Store result for plotting
+ ekf_mean = [ekf_mean, curr_mean];
+ ekf_cov = [ekf_cov, reshape(curr_cov, 9, 1)];
+end % for each u
+
+plot_trajectory_cov(x_true, ekf_mean, ekf_cov, dt);
+
+disp('final result WITH measurements')
+final_mean = curr_mean
+final_cov = curr_cov
+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
diff --git a/mr/ub9/loc_jp/localize_init.m b/mr/ub9/loc_jp/localize_init.m
new file mode 100644
index 0000000..9686363
--- /dev/null
+++ b/mr/ub9/loc_jp/localize_init.m
@@ -0,0 +1,36 @@
+close all
+
+% ## General parameters
+% Fix random seed so everyone gets the same true trajectory.
+rng(51)
+% Sampling time:
+dt = 0.2;
+% System noise:
+R = diag([0.02^2, 0.02^2, 0.02^2]);
+% Measurement noise (1D measurements):
+Q = eye(4)*1.0^2;
+% Construct vector with control input:
+uhc = repmat([0.1; 0.105], 1, 100);
+uhc2 = repmat([0.105; 0.1], 1, 100);
+ust = repmat([0.1; 0.1], 1, 30);
+u = [uhc, ust, uhc2, ust];
+
+% ## Compute the true trajectory and measurements:
+% Evaluate kinematic model according to control input
+% and add some sampled system noise.
+currx = [0; 0; 0];
+x_true = currx;
+z_meas = [];
+for i = 1:size(u,2)
+ % Evaluate kinematic Model:
+ currx = motion_diff(currx, u(:,i));
+ % Add system noise:
+ currx = currx + mvnrnd([0; 0; 0], R)';
+ % Simulate a measurement:
+ z_meas = [z_meas, measurement(currx) + mvnrnd(zeros(size(Q,1),1),Q)'];
+ % Store result for plotting
+ x_true = [x_true, currx];
+end
+
+% Reinitialize random seed generator for random sampling
+rng('shuffle')
\ No newline at end of file
diff --git a/mr/ub9/loc_jp/localize_pf.m b/mr/ub9/loc_jp/localize_pf.m
new file mode 100644
index 0000000..47c92fa
--- /dev/null
+++ b/mr/ub9/loc_jp/localize_pf.m
@@ -0,0 +1,71 @@
+% ### 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))
\ No newline at end of file
diff --git a/mr/ub9/loc_jp/localize_ukf.m b/mr/ub9/loc_jp/localize_ukf.m
new file mode 100644
index 0000000..557b336
--- /dev/null
+++ b/mr/ub9/loc_jp/localize_ukf.m
@@ -0,0 +1,42 @@
+% ### Run some initialization initialization steps common to all filters
+localize_init;
+
+% ## Evaluate UKF without measurements:
+% Estimates are based on control input alone.
+curr_mean = [0; 0; 0];
+curr_cov = diag([0, 0, 0]);
+
+nomeas_mean = curr_mean;
+nomeas_cov = reshape(curr_cov, 9, 1);
+for i = 1:size(u,2)
+ [curr_mean, curr_cov] = ukf_prediction(curr_mean, curr_cov, u(:,i), R, @motion_diff);
+
+ % Store result for later plotting
+ nomeas_mean = [nomeas_mean, curr_mean];
+ nomeas_cov = [nomeas_cov, reshape(curr_cov, 9, 1)];
+end % for each u
+
+plot_trajectory_cov(x_true, nomeas_mean, nomeas_cov, dt);
+
+% ## Evaluate full UKF with measurements:
+curr_mean = [0; 0; 0];
+curr_cov = diag([0, 0, 0]);
+ukf_mean = curr_mean;
+ukf_cov = reshape(curr_cov, 9, 1);
+for i = 1:size(u,2)
+ [curr_mean, curr_cov] = ukf_prediction(curr_mean, curr_cov, u(:,i), R, @motion_diff);
+ [curr_mean, curr_cov] = ukf_correction(curr_mean, curr_cov, z_meas(:,i), Q, @measurement);
+
+ % Store result for later plotting
+ ukf_mean = [ukf_mean, curr_mean];
+ ukf_cov = [ukf_cov, reshape(curr_cov, 9, 1)];
+end % for each u
+
+plot_trajectory_cov(x_true, ukf_mean, ukf_cov, dt);
+
+disp('final result WITH measurements')
+final_mean = curr_mean
+final_cov = curr_cov
+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
diff --git a/mr/ub9/loc_jp/measurement.m b/mr/ub9/loc_jp/measurement.m
new file mode 100644
index 0000000..73a8a17
--- /dev/null
+++ b/mr/ub9/loc_jp/measurement.m
@@ -0,0 +1,29 @@
+function [ z H ] = measurement( x )
+%MEASUREMENT Measurement Model:
+% Measure the robot's distance from several landmarks.
+
+% landmarks:
+l = [0, -1; -3, -2; 0, 6; -3, 6]';
+
+nl = size(l,2);
+nx = size(x,3);
+
+z = zeros(nl,1,nx);
+H = zeros(nl,3);
+
+pos = x(1:2,:,:);
+for i = 1:nl
+ diff = pos - repmat(l(:,i),1,1,size(pos,3));
+ z(i,:,:) = sqrt(sum(diff.*diff,1));
+end % for
+
+% EKF case
+if size(x,3) == 1
+ for i = 1:nl
+ diff = pos - l(:,i);
+ H(i,:) = [ diff(1)/(z(i,1))^(1/2), diff(2)/(z(i,1))^(1/2), 0;];
+ end % for
+end
+
+end
+
diff --git a/mr/ub9/loc_jp/motion_diff.m b/mr/ub9/loc_jp/motion_diff.m
new file mode 100644
index 0000000..1e5e8f9
--- /dev/null
+++ b/mr/ub9/loc_jp/motion_diff.m
@@ -0,0 +1,33 @@
+function [ xnew G] = motion_diff( x, u )
+%MOTION_DIFF Compute differential drive motion model
+
+l = 0.1; % distance between both wheels
+
+theta = x(3,:,:);
+sl = u(1);
+sr = u(2);
+
+r = l*(sl + sr)/(2*(sr-sl));
+
+G = eye(3);
+if r == Inf
+ % Special case: Robot is driving straight:
+ d = (sl+sr)/2;
+ xnew = x + [d*cos(theta); ...
+ d*sin(theta); ...
+ zeros(1,1,size(x,3))];
+else
+ dTh = repmat((sr - sl)/l,1,1,size(x,3));
+ xnew = x + [r*sin(theta + dTh) - r*sin(theta); ...
+ -r*cos(theta + dTh) + r*cos(theta); ...
+ dTh];
+
+ if (size(x,3) == 1)
+ G = [1, 0, r*(cos(theta + dTh) - cos(theta)); ...
+ 0, 1, r*(sin(theta + dTh) - sin(theta)); ...
+ 0, 0, 1];
+ end
+end % if
+
+end
+
diff --git a/mr/ub9/loc_jp/pf_prediction.m b/mr/ub9/loc_jp/pf_prediction.m
new file mode 100644
index 0000000..430ce9d
--- /dev/null
+++ b/mr/ub9/loc_jp/pf_prediction.m
@@ -0,0 +1,14 @@
+function [ xnew ] = pf_prediction( x, u, R, motion)
+%PF_PREDICTION Particle Filter prediction step
+
+% Number of particles:
+m = size(x, 3);
+
+% YOUR CODE STARTS HERE:
+
+xnew = x;
+
+% YOUR CODE ENDS HERE:
+
+end
+
diff --git a/mr/ub9/loc_jp/pf_resample.m b/mr/ub9/loc_jp/pf_resample.m
new file mode 100644
index 0000000..e356eee
--- /dev/null
+++ b/mr/ub9/loc_jp/pf_resample.m
@@ -0,0 +1,22 @@
+function [ nextx ] = pf_resample( currx, w )
+%PF_RESAMPLE Basic resampling
+
+% number of samples:
+M = size(currx, 3);
+
+% Redraw particles:
+nextx = zeros(3, 1, M);
+
+% Cumulative sum of weights:
+cum = cumsum(w);
+
+for p = 1:M
+ r = rand();
+ % 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);
+end
+
+end
+
diff --git a/mr/ub9/loc_jp/pf_resample_sys.m b/mr/ub9/loc_jp/pf_resample_sys.m
new file mode 100644
index 0000000..fb01f81
--- /dev/null
+++ b/mr/ub9/loc_jp/pf_resample_sys.m
@@ -0,0 +1,11 @@
+function [ nextx ] = pf_resample_sys( currx, w )
+%PF_RESAMPLE_LV Systematic or low variance resampling
+
+% YOUR CODE STARTS HERE:
+
+nextx = currx;
+
+% YOUR CODE END HERE
+
+end
+
diff --git a/mr/ub9/loc_jp/pf_reweight.m b/mr/ub9/loc_jp/pf_reweight.m
new file mode 100644
index 0000000..f450fcf
--- /dev/null
+++ b/mr/ub9/loc_jp/pf_reweight.m
@@ -0,0 +1,14 @@
+function [ wnew ] = pf_reweight( x, w, z, Q, measurement )
+%PF_CORRECTION Particle Filter correction step
+
+nSamples = size(x, 3);
+
+% Reweighting based on expected and actual measurement:
+z_exp = measurement(x);
+
+tmp = num2cell(z_exp,1);
+wnew = w.*reshape(cellfun(@(x) mvnpdf(x, z, Q) ,tmp), nSamples, 1, 1);
+wnew = wnew / sum(wnew);
+
+end
+
diff --git a/mr/ub9/loc_jp/plot_trajectory_cov.m b/mr/ub9/loc_jp/plot_trajectory_cov.m
new file mode 100644
index 0000000..a9d98f2
--- /dev/null
+++ b/mr/ub9/loc_jp/plot_trajectory_cov.m
@@ -0,0 +1,45 @@
+function [ ] = plot_trajectory_cov(xtrue, mean, cov, dt )
+%PLOT_TRAJECTORY_COV Plot the 2d trajectory of a robot with uncertainty
+% estimates
+
+% Create a new figure for this plot
+figure
+
+% Plot ground truth trajectory:
+plot(xtrue(1,:), xtrue(2,:), 'r');
+hold on % keep drawing into the same figure
+
+% Draw the estimate of the same trajectory.
+plot(mean(1,:), mean(2,:))
+
+% Only at some times: Visualize the estimate uncertainty.
+ellipsetime = 4/dt; % Once in 4 seconds
+em = mean(:,1:ellipsetime:end);
+ecov = cov(:,1:ellipsetime:end);
+
+% Draw uncertainty ellipses
+for i = 1:size(ecov,2)
+ s = reshape(ecov(:,i),3,3);
+
+ % Compute and plot uncertainty ellipse for position covariance.
+ p_pos = get_ellipse(em(:,i), s);
+ plot(p_pos(1,:), p_pos(2,:),'k');
+
+ % Compute and plot angle interval showing orientation uncertainty.
+ p_th = get_angle_bracket(em(:,i), s);
+ plot(p_th(1,:), p_th(2,:),'k');
+end
+
+% Description & legend
+axis equal
+xlabel('x in m')
+ylabel('y in m')
+title('robot trajectory with uncertainty estimates')
+legend('true trajectory', ...
+ 'estimate (mean)', ...
+ 'estimate (covariance)', ...
+ 'location', 'SouthEast');
+
+
+end
+
diff --git a/mr/ub9/loc_jp/plot_trajectory_particles.m b/mr/ub9/loc_jp/plot_trajectory_particles.m
new file mode 100644
index 0000000..48387ae
--- /dev/null
+++ b/mr/ub9/loc_jp/plot_trajectory_particles.m
@@ -0,0 +1,45 @@
+function [ ] = plot_trajectory_particles( xtrue, x, w, dt )
+%PLOT_TRAJECTORY_PARTICLES Summary of this function goes here
+% Detailed explanation goes here
+
+% Create a new figure for this plot
+figure
+
+% Plot ground truth trajectory:
+plot(xtrue(1,:), xtrue(2,:), 'r');
+hold on % keep drawing into the same figure
+
+% Draw the estimate of the same trajectory.
+% Normalize weights
+s = sum(w,3);
+w = w ./ repmat(s,1,1,size(w,3));
+mux = sum(x.*repmat(w,3,1,1),3);
+plot(mux(1,:), mux(2,:), 'b')
+
+% Only at some times: Draw all particles
+ptime = 4/dt; % Once in 4 seconds
+
+xdraw = x(:,1:ptime:end,:);
+
+% Draw particles
+nSamples = size(x,3);
+for i = 1:size(xdraw,2)
+ currx = xdraw(:,i,:);
+ quiver(reshape(currx(1,:,:),nSamples,1,1), ...
+ reshape(currx(2,:,:),nSamples,1,1), ...
+ cos(reshape(currx(3,:,:),nSamples,1,1)), ...
+ sin(reshape(currx(3,:,:),nSamples,1,1)));
+end
+
+% Description & legend
+axis equal
+xlabel('x in m')
+ylabel('y in m')
+title('robot trajectory with uncertainty estimates')
+legend('true trajectory', ...
+ 'estimate (mean)', ...
+ 'location', 'SouthEast');
+
+
+end
+
diff --git a/mr/ub9/loc_jp/ukf_correction.m b/mr/ub9/loc_jp/ukf_correction.m
new file mode 100644
index 0000000..ea3677f
--- /dev/null
+++ b/mr/ub9/loc_jp/ukf_correction.m
@@ -0,0 +1,12 @@
+function [ mean, cov ] = ukf_correction( pred_mean, pred_cov, z, Q, measurement )
+%UKF_CORRECTION UKF correction step
+
+% YOUR CODE STARTS HERE:
+
+mean = pred_mean;
+cov = pred_cov;
+
+% YOUR CODE ENDS HERE:
+
+end
+
diff --git a/mr/ub9/loc_jp/ukf_estimate_normal.m b/mr/ub9/loc_jp/ukf_estimate_normal.m
new file mode 100644
index 0000000..e3a82ac
--- /dev/null
+++ b/mr/ub9/loc_jp/ukf_estimate_normal.m
@@ -0,0 +1,28 @@
+function [ mu, cov ] = ukf_estimate_normal( S, w )
+%UKF_ESTIMATE_NORMAL Estimate mean, covariance from set of sigma points
+
+% Dimension (3 for 2D localization)
+dim = size(S,1);
+
+% Number of sigma points (= number of weights):
+n = size(w,1);
+
+mu = zeros(dim,1);
+cov = zeros(dim,dim);
+
+% YOUR CODE STARTS HERE:
+for i=1:n
+ mu=mu+w(i)*S(i);
+end
+
+for i=1:n
+ cov=cov+w(i)*(S(i)-mu)*(S(i)-mu)';
+end
+w
+S
+mu
+cov
+% YOUR CODE END HERE:
+
+end
+
diff --git a/mr/ub9/loc_jp/ukf_prediction.m b/mr/ub9/loc_jp/ukf_prediction.m
new file mode 100644
index 0000000..a01e9c0
--- /dev/null
+++ b/mr/ub9/loc_jp/ukf_prediction.m
@@ -0,0 +1,20 @@
+function [ newmean, newcov ] = ukf_prediction( mean, cov, u, R, motion)
+%UKF_PREDICTION Computes the UKF prediction step
+
+% YOUR CODE STARTS HERE:
+[S w]=ukf_sigma_points(mean,cov,1/3);
+n=size(w,1);
+Snew=zeros(size(S));
+
+for i=1:n
+ [Snew(:,i), G]=motion(S(:,i),u);
+end
+
+[newmean, newcov]=ukf_estimate_normal(Snew,w);
+newcov=newcov+R;
+
+
+% YOUR CODE ENDS HERE:
+
+end
+
diff --git a/mr/ub9/loc_jp/ukf_sigma_points.m b/mr/ub9/loc_jp/ukf_sigma_points.m
new file mode 100644
index 0000000..6e7218d
--- /dev/null
+++ b/mr/ub9/loc_jp/ukf_sigma_points.m
@@ -0,0 +1,39 @@
+function [ S w] = ukf_sigma_points( mu, cov, w0 )
+%UKF_SIGMA_POINTS Compute sigma points from mean and covariance matrix.
+
+% Use default value for w0 if it is not provided as a parameter:
+if nargin < 3
+ w0 = 1/3;
+end
+
+nx = size(mu,1); % Dimension (3 for 2D localization)
+
+n = 2*nx + 1; % n sigma points
+
+% Store sigma points in (nx x n) matrix
+% Sigma point i is S(:,i)
+S = zeros(nx, n);
+
+% Store weights in (n x 1) matrix:
+% Weight of sigma point i is w(i);
+w = zeros(n,1);
+
+% YOUR CODE STARTS HERE
+w(1)=w0;
+S(:,1)=mu;
+
+offset=sqrt(nx/(1-w0))*sqrtm(cov);
+weight=(1-w0)/(2*nx);
+
+for i=2:nx+1
+ S(:,i)=mu+offset(:,i-1);
+ w(i)=weight;
+ S(:,i+nx)=mu-offset(:,i-1);
+ w(i+nx)=weight;
+end
+
+
+% YOR CODE ENDS HERE
+
+end
+