diff --git a/mr/ub8/assignment08.pdf b/mr/ub8/assignment08.pdf new file mode 100644 index 0000000..28f47ec --- /dev/null +++ b/mr/ub8/assignment08.pdf Binary files differ diff --git a/mr/ub8/localize_ekf/get_angle_bracket.m b/mr/ub8/localize_ekf/get_angle_bracket.m new file mode 100644 index 0000000..982c4cf --- /dev/null +++ b/mr/ub8/localize_ekf/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 = 2; % 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/ub8/localize_ekf/get_ellipse.m b/mr/ub8/localize_ekf/get_ellipse.m new file mode 100644 index 0000000..4f1cfb0 --- /dev/null +++ b/mr/ub8/localize_ekf/get_ellipse.m @@ -0,0 +1,25 @@ +function [ p ] = get_ellipse( mean, cov ) +%GET_ELLIPSE Compute points on the 95% uncertainty ellipse. + +% 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 +scale = 2.; % in multiples of sigma +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/ub8/localize_ekf/localize.m b/mr/ub8/localize_ekf/localize.m new file mode 100644 index 0000000..0730931 --- /dev/null +++ b/mr/ub8/localize_ekf/localize.m @@ -0,0 +1,80 @@ +close all; + +% ## General parameters +% Fix random seed so everyone gets the same results. +rng(230) +% Sampling time: +dt = 0.2; +% System noise: +R = diag([0.02^2, 0.02^2, 0.02^2]); +% Measurement noise (1D measurements): +%Q = 0.5^2; +Q=1*10^(-1000) +% Construct vector with control input: +uhc = repmat([0.1; 0.105], 1, 63); +ust = repmat([0.1; 0.1], 1, 30); +u = [uhc, ust, uhc, ust]; + +% ## Compute the true trajectory: +% 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 + +% ## 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) + % Update 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); \ No newline at end of file diff --git a/mr/ub8/localize_ekf/measurement.m b/mr/ub8/localize_ekf/measurement.m new file mode 100644 index 0000000..fddf5e8 --- /dev/null +++ b/mr/ub8/localize_ekf/measurement.m @@ -0,0 +1,15 @@ +function [ z H ] = measurement( x ) +%MEASUREMENT Measurement Model: Measure the robot's distance from origin. + +% YOUR CODE STARTS HERE + +z = sqrt(x(1)^2+x(2)^2); % fill this with the measurement result +%H = zeros(1,3); % fill this with the measurement Jacobian +H=[x(1)/z; + x(2)/z; + 0]; + +% YOUR CODE ENDS HERE + +end + diff --git a/mr/ub8/localize_ekf/motion_diff.m b/mr/ub8/localize_ekf/motion_diff.m new file mode 100644 index 0000000..1faaaa7 --- /dev/null +++ b/mr/ub8/localize_ekf/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); + +% YOUR CODE STARTS HERE % +deltaTheta= (sr-sl)/l; +r= (sl+sr)/(sr-sl) *l/2; +% fill this with the result of applying the motion model +if(sr==sl) +xnew= x+ [r*(sin(theta+deltaTheta)-sin(theta)); + r*(-cos(theta+deltaTheta)-cos(theta)); + deltaTheta]; +else +xnew= x+[sl*cos(theta); + sl*sin(theta); + 0]; +end +% fill this with the motion Jacobian G +% G=zeros(3,3) +G = [1,0,r*(cos(theta+deltaTheta)-cos(theta)); + 0,1,r*(sin(theta+deltaTheta)-sin(theta)); + 0,0,1]; + + +% YOUR CODE ENDS HERE % + +end + diff --git a/mr/ub8/localize_ekf/plot_trajectory_cov.m b/mr/ub8/localize_ekf/plot_trajectory_cov.m new file mode 100644 index 0000000..5e11454 --- /dev/null +++ b/mr/ub8/localize_ekf/plot_trajectory_cov.m @@ -0,0 +1,46 @@ +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/ub8/mr8.pdf b/mr/ub8/mr8.pdf new file mode 100644 index 0000000..4a38336 --- /dev/null +++ b/mr/ub8/mr8.pdf Binary files differ diff --git a/mr/ub8/mr8.tex b/mr/ub8/mr8.tex new file mode 100644 index 0000000..ff50405 --- /dev/null +++ b/mr/ub8/mr8.tex @@ -0,0 +1,109 @@ +\documentclass[a4paper,12pt]{scrartcl} +\usepackage[ngerman]{babel} +\usepackage{graphicx} %BIlder einbinden +\usepackage{amsmath} %erweiterte Mathe-Zeichen +\usepackage{amsfonts} %weitere fonts +\usepackage[utf8]{inputenc} %Umlaute & Co +\usepackage{hyperref} %Links +\usepackage{ifthen} %ifthenelse +\usepackage{enumerate} +\usepackage{pdfpages} +\usepackage{algpseudocode} %Pseudocode +\usepackage{dsfont} % schöne Zahlenräumezeichen +\usepackage{amssymb, amsthm} %noch stärker erweiterte Mathe-Zeichen +\usepackage{tikz} %TikZ ist kein Zeichenprogramm +\usetikzlibrary{trees,automata,arrows,shapes} +\usepackage{qtree} + +\pagestyle{empty} + + +\topmargin-50pt + +\newcounter{aufgabe} +\def\tand{&} + +\newcommand{\makeTableLine}[2][0]{% + \setcounter{aufgabe}{1}% + \whiledo{\value{aufgabe} < #1}% + {% + #2\tand\stepcounter{aufgabe}% + } +} + +\newcommand{\aufgTable}[1]{ + \def\spalten{\numexpr #1 + 1 \relax} + \begin{tabular}{|*{\spalten}{p{1cm}|}} + \makeTableLine[\spalten]{A\theaufgabe}$\Sigma$~~\\ \hline + \rule{0pt}{15pt}\makeTableLine[\spalten]{}\\ + \end{tabular} +} + +\def\header#1#2#3#4#5#6#7{\pagestyle{empty} +\begin{minipage}[t]{0.47\textwidth} +\begin{flushleft} +{\bf #4}\\ +#5 +\end{flushleft} +\end{minipage} +\begin{minipage}[t]{0.5\textwidth} +\begin{flushright} +#6 \vspace{0.5cm}\\ +% Number of Columns Definition of Columns second empty line +% \begin{tabular}{|*{5}{C{1cm}|}}\hline A1&A2&A3&A4&$\Sigma$\\\hline&&&&\\\hline\end{tabular}\\\vspace*{0.1cm} +\aufgTable{#7} +\end{flushright} +\end{minipage} +\vspace{1cm} +\begin{center} +{\Large\bf Assignment #1} + +{(Hand-in date #3)} +\end{center} +} + + + +%counts the exercisenumber +\newcounter{n} + +%Kommando für Aufgaben +%\Aufgabe{AufgTitel}{Punktezahl} +\newcommand{\Aufgabe}[2]{\stepcounter{n} +\textbf{Exercise \arabic{n}: #1} (#2 Punkte)\\} + + + + +\begin{document} + %\header{BlattNr}{Tutor}{Abgabedatum}{Vorlesungsname}{Namen}{Semester}{Anzahl Aufgaben} + \header{7}{}{2015-06-16}{Mobile Robots}{ + \textit{Jan-Peter Hohloch}\\ \textit{Maximus Mutschler} + }{SS 15}{3} + \vspace{1cm} + + \Aufgabe{Excercise 1}{10} + TODO Abbildungen\\ + d) Measurement uncertainty $Q\rightarrow0 + \\ K_t = \overline{\Sigma}_tC^TC^{-1}\overline{\Sigma}_t^{-1}(C^T)^{-1}\\ + =C^{-1} vllt \rightarrow\\ + \Sigma_t=(I-I)*\overline{\Sigma}_t=0\\ +$ +\Aufgabe{Excercise2}{8} +\begin{enumerate}[(a)] + \item $p(x_t|z_{t,\dots1},u_{t,\dots1})\sim N(x_t,\mu_{x,t},\sigma_{x,t})\\ + \mu_{x,t}= a\mu_{x,t-1}+bu_t+\epsilon_t\\ + \sigma_{x,t}= |a|\sigma_{x,t-1}\\ + \\ + p(x_{t}|z_{t,\dots1},u_{t,\dots1})\sim N(x_{t-1},\mu_{x,t-1},\sigma_{x,t-1})\\ + \mu_{x,t-1}= a\mu_{x,t-2}+bu_{t-1}+\epsilon_{t-1}\\ + \sigma_{x,t-1}= |a|\sigma_{x,t-2}\\ + \\ + p(z_t|x-t)\sim N(z_t,\mu_z,\sigma_z)\\ + \mu_z=c*\mu_{x,t-1}+\delta_t \\ + \sigma_z=|c|\sigma_{x,t-1} + $ +\end{enumerate} + +\end{document} +