diff --git a/mr/ub8/localize_jp/localize.m b/mr/ub8/localize_jp/localize.m index 1293290..b9c16a1 100644 --- a/mr/ub8/localize_jp/localize.m +++ b/mr/ub8/localize_jp/localize.m @@ -8,7 +8,7 @@ % System noise: R = diag([0.02^2, 0.02^2, 0.02^2]); % Measurement noise (1D measurements): -Q = 0.5^2; +Q = 0.002^2; % Construct vector with control input: uhc = repmat([0.1; 0.105], 1, 63); ust = repmat([0.1; 0.1], 1, 30); @@ -67,7 +67,7 @@ [z_exp, H] = measurement(curr_mean); % Kalman gain: - K = curr_cov * (H') * inv(H*curr_cov*(H') + Q); + 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; diff --git a/mr/ub8/localize_jp/motion_diff.m b/mr/ub8/localize_jp/motion_diff.m index 848f892..e22de18 100644 --- a/mr/ub8/localize_jp/motion_diff.m +++ b/mr/ub8/localize_jp/motion_diff.m @@ -16,8 +16,12 @@ else r=(sl+sr)/(sr-sl)*l/2; dtheta=(sr-sl)/l; - xnew=x+[r*(sin(theta+dtheta)-sin(theta));r*(-cos(theta+dtheta)+cos(theta));dtheta]; - G=jacobian([x1;x2;x3]+[r*(sin(x3+dtheta)-sin(x3));r*(-cos(x3+dtheta)+cos(x3));dtheta],[x1;x2;x3]); + xnew=x+[r*(sin(theta+dtheta)-sin(theta)); + r*(-cos(theta+dtheta)+cos(theta)); + dtheta]; + G=jacobian([x1;x2;x3]+[r*(sin(x3+dtheta)-sin(x3)); + r*(-cos(x3+dtheta)+cos(x3)); + dtheta],[x1;x2;x3]); end x3=theta; G=eval(G); diff --git a/mr/ub8/mr8.pdf b/mr/ub8/mr8.pdf index 4a38336..091b875 100644 --- a/mr/ub8/mr8.pdf +++ b/mr/ub8/mr8.pdf Binary files differ diff --git a/mr/ub8/mr8.tex b/mr/ub8/mr8.tex index ff50405..0b37338 100644 --- a/mr/ub8/mr8.tex +++ b/mr/ub8/mr8.tex @@ -14,6 +14,8 @@ \usepackage{tikz} %TikZ ist kein Zeichenprogramm \usetikzlibrary{trees,automata,arrows,shapes} \usepackage{qtree} +\usepackage{listings} +\lstset{language=Matlab} \pagestyle{empty} @@ -72,38 +74,62 @@ \newcommand{\Aufgabe}[2]{\stepcounter{n} \textbf{Exercise \arabic{n}: #1} (#2 Punkte)\\} +\newcommand{\Normal}[3]{\mathcal{N}\left(#1,#2,#3\right)} +\newcommand{\Normalf}[3]{\frac{1}{#3 \cdot\sqrt{2\pi}}e^{-\frac{\left(#1 - #2 \right)^2}{2 #3 ^2}}} +\newcommand{\Normalfs}[3]{\frac{1}{#3}e^{-\frac{\left(#1 - #2 \right)^2}{2 #3 ^2}}} + \begin{document} %\header{BlattNr}{Tutor}{Abgabedatum}{Vorlesungsname}{Namen}{Semester}{Anzahl Aufgaben} - \header{7}{}{2015-06-16}{Mobile Robots}{ + \header{8}{}{2015-06-23}{Mobile Robots}{ \textit{Jan-Peter Hohloch}\\ \textit{Maximus Mutschler} - }{SS 15}{3} + }{SS 15}{2} \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} + \includegraphics[width=\textwidth]{localize.png}\\ + %\lstinputlisting{localize_jp/motion_diff.m} + %\lstinputlisting{localize_jp/measurement.m} + d) + The uncertainty in one direction decreases. This causes the estimated trajectory to be quite wiggly (overfitting). We overtrust the measurement.\\ + For $Q_t$ near to $0$, we get: + \begin{align*} + K_t&=\overline{\Sigma}_tC^T\left(C\overline{\Sigma}_tC^T+0\right)^{-1}\\ + &=C^{-1}\\ + \mu_t&=\overline{\mu}_t+C^{-1}z_t-\overline{\mu}_t\\ + &=C^{-1}z_t\\ + \Sigma_t&=0 + \end{align*} + That is we assume no variance and fully trust the measurement.\pagebreak\\ +\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} - $ + \item %TODO Herleitung + \begin{itemize} + \item $p(x_{t-1})=\mathcal{N}\left(x_{t-1},\hat{x}_{t-1},\sigma_{t-1}\right)$ + \item $p(z_t)=\mathcal{N}\left(z_t,cx_t,r_t\right)$ + \item $p(x_t)=\mathcal{N}\left(x_t,a\mu_{t-1},q_t\right)$ + \end{itemize} + \item \begin{align*} + \Normal{x_t}{ax_{t-1}}{q_t}&=\eta\Normal{z_t}{cx_t}{r_t}\Normal{x_{t-1}}{\hat{x}_{t-1}}{\sigma_{t-1}}\\ + \Normalf{x_t}{ax_{t-1}}{q_t}&=\eta\Normalf{z_t}{cx_t}{r_t}\Normalf{x_{t-1}}{\hat{x}_{t-1}}{\sigma_{t-1}}\\ + \Normalfs{x_t}{ax_{t-1}}{q_t}&=\eta\Normalfs{z_t}{cx_t}{r_t}\Normalfs{x_{t-1}}{\hat{x}_{t-1}}{\sigma_{t-1}} + \end{align*} + %TODO + % \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}