[Electives] Restructure

2026-05-30 16:21:19 +02:00 · 2026-05-04 11:32:59 +02:00
parent 4984dbdc70
commit d1bf81bea3
39 changed files with 0 additions and 0 deletions
@@ -0,0 +1,88 @@
+\documentclass{article}
+
+\PassOptionsToPackage{skip=0pt}{parskip}
+\input{~/projects/latex/janishutz-helpers.tex}
+
+% Configuration
+\usepackage{bm}
+\usepackage{lmodern}
+\setFontType{sans}
+
+\renewcommand{\subsectionnumbering}{section}
+\renewcommand{\numberingpreset}{off}
+\renewcommand{\definitionShortNamingEN}{Def}
+\fboxsep 1pt
+\fboxrule 0.1pt
+
+% Extra commands
+\renewcommand{\vec}[1]{\bm{#1}}
+\newcommand{\mat}[1]{\bm{#1}}
+
+\setupCheatSheet{Autonomous Mobile Robots}
+
+
+\begin{document}
+\noverticalspacing
+
+\vspace*{0mm}
+
+\begin{center}
+    \begin{Large}
+        \textbf{Autonomous Mobile Robots}
+    \end{Large}
+
+    \textsc{Cheat Sheet by {\color{MidnightBlue}\href{https://janishutz.com}{Janis Hutz}}}
+
+    \textsc{ETHZ, FS2026}
+\end{center}
+
+
+% TODO: Graphics
+\section{Introduction}
+\input{parts/00_basics/00_probability.tex}
+\input{parts/00_basics/01_measurement-models.tex}
+\input{parts/00_basics/02_trigonometry.tex}
+
+\section{Locomotion \& Kinematics}
+\input{parts/01_kinematics/00_intro.tex}
+\input{parts/01_kinematics/01_forward.tex}
+\input{parts/01_kinematics/02_inverse.tex}
+\input{parts/01_kinematics/03_temporal-models.tex}
+\input{parts/01_kinematics/04_rigid-body-imu-kinematics.tex}
+\input{parts/01_kinematics/05_rigid-body-dynamics.tex}
+\input{parts/01_kinematics/06_wheeled-robot.tex}
+% \input{parts/01_kinematics/}
+
+\section{Sensors \& Actuators}
+\input{parts/02_Sensors-Actuators/00_intro.tex}
+\input{parts/02_Sensors-Actuators/01_gps.tex}
+\input{parts/02_Sensors-Actuators/02_actuators.tex}
+\input{parts/02_Sensors-Actuators/03_cameras.tex}
+\input{parts/02_Sensors-Actuators/04_depth-range.tex}
+% \input{parts/02_Sensors-Actuators/}
+
+\section{Multi-Sensor Estimation}
+\input{parts/03_multi-sensor-estimation/00_linearization.tex}
+\input{parts/03_multi-sensor-estimation/01_least-squares.tex}
+\input{parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex}
+\input{parts/03_multi-sensor-estimation/03_bayes-filters.tex}
+\input{parts/03_multi-sensor-estimation/04_particle-filter.tex}
+\input{parts/03_multi-sensor-estimation/05_kalman-filter.tex}
+\input{parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex}
+% \input{parts/03_multi-sensor-estimation/}
+
+\section{SLAM to Spatial AI}
+\input{parts/04_vision/00_keypoints.tex}
+\input{parts/04_vision/01_bootstrapping.tex}
+\input{parts/04_vision/02_place-recognition.tex}
+\input{parts/04_vision/03_mapping.tex}
+\input{parts/04_vision/04_dense-tracking_loop-closure.tex}
+% \input{parts/04_vision/}
+
+\section{Planning \& Control}
+\input{parts/05_planning-control/00_feedback-control/00_siso-mimo.tex}
+% \input{parts/05_planning-control/00_feedback-control/}
+\input{parts/05_planning-control/01_motion-planning-exploration/00_intro.tex}
+% \input{parts/05_planning-control/01_motion-planning-exploration/}
+
+\end{document}
@@ -0,0 +1,20 @@
+\subsection{Probability}
+\shortdefinition[Sum rule] $P(X) = \sum P(X, Y) = \sum P(X \cap Y)$
+
+\shortdefinition[Prod] $P(X, Y) = P(X | Y) P(Y) = P(Y | X) P(X)$
+
+\shorttheorem[Bayes] $\displaystyle P(Y_i | X) = \frac{P(X | Y_i) P(Y_i)}{\sum_{j = 1}^n P(X | Y_j) P(Y_j)}$
+
+\shortdefinition[Cont. Var] Sums become integrals\\
+e.g. $\sum_{X} P(X) = 1$ becomes $\int p(x) \dx = 1$
+
+\shortdefinition[Indep.] $x, y$ indep. iff $p(x, y) = p(x) p(y)$
+
+\shortdefinition[Cond. Indep.] iff $p(x, y | z) = p(x|z) p(y|z)$
+
+\shortdefinition $E[\vec{x}] = \int_{-\8}^{\8} \vec{x} p(\vec{x}) \dx \vec{x}$, also for $\vec{x} = \vec{f(x)}$
+
+\shortdefinition $\text{Cov}[x] = E[\vec{x} \vec{x}^\top] - E[\vec{x}]E[\vec{x}]^\top = \mat{\Sigma}$
+
+\shortdefinition[Gauss. Dist.] $\vec{x} \sim \cN(\vec{\mu}, \mat{\Sigma})$ ($\vec{\mu}$ mean, $\mat{\Sigma}$ cov.),\\
+PDF: $p(\vec{x}) = \frac{1}{\sqrt{(2\pi)^k |\mat{\Sigma}|}} \text{exp}\left( -\frac{1}{2}(\vec{x} - \vec{\mu})^\top \mat{\Sigma}^{-1} (\vec{x} - \vec{\mu}) \right)$
@@ -0,0 +1,3 @@
+\subsection{Measurement models}
+$\vec{z} = \vec{b}_C + s\mat{M} {_S}\vec{\omega} + \vec{b} + \vec{n} + \vec{o}$:
+$\vec{b}_C$ const bias, $\vec{b}$ time bias, $\mat{M}$ missal., $\vec{n} \sim \cN(\vec{0}, \mat{R})$ noise, ${_S}\omega$ corr. meas., $\vec{o}$ other infl.
@@ -0,0 +1,3 @@
+\subsection{Trigonometry}
+% TODO: Cosine rule (at least), probably also sine rule.
+% TODO: Add convenient results (such as cos2 + sin2 = 1)
@@ -0,0 +1,53 @@
+\subsection{Positioning}
+\shortdefinition[Position Vector]
+$_{\color{blue}\fbox{W}}\,\vec{t}\,_{\color{red}\fbox{B}} = \, _{\color{blue}\fbox{W}}\,\vec{t}\,_{\color{ForestGreen}\fbox{W}}\,_{\color{red}\fbox{B}}$,
+{\color{blue} Original Frame}, {\color{red} End point}, {\color{ForestGreen} Target Frame},
+\hl{$\sin = s$, $\cos = c$}
+
+\shortdefinition[State vector] $x_R$: $x$, $v$ of rob in $W$, pos of sensors
+
+\shortdefinition[Rot. Mat.] $\mat{R}_{z}(\psi) = \begin{bmatrix}
+        c(\psi) & -s(\psi) & 0 \\
+        s(\psi) & c(\psi)  & 0 \\
+        0       & 0        & 1
+    \end{bmatrix}$\\
+$\mat{R}_y(\theta) = \begin{bmatrix}
+        c(\theta)  & 0 & s(\theta) \\
+        0        & 1 & 0       \\
+        -s(\theta) & 0 & c(\theta) \\
+    \end{bmatrix};
+    \mat{R}_x(\varphi)
+    \begin{bmatrix}
+        1 & 0       & 0        \\
+        0 & c(\varphi) & -s(\varphi) \\
+        0 & s(\varphi) & c(\varphi)
+    \end{bmatrix}$
+
+\shortremark Application: ${_W} \vec{a} = \mat{R}_{WB} {_B} \vec{a}$
+
+\shortlemma $\mat{R}_{BW} = \mat{R}_{WB}^{-1} = \mat{R}_{WB}^\top$, $\det(\mat{R}_{WB}) = 1$ (orth.)
+
+\shortremark Cols of $\mat{R}_{WB}$ are basis vec. of Frame $\underset{\rightarrow}{\cF}{_B}$ in $\underset{\rightarrow}{\cF}{_W}$
+
+\shortdefinition[Euler Angles] Yaw ($z$), Pitch ($y$), Roll ($x$), mult. rotation matrices, e.g.
+$\mat{R}_{EB} = \mat{R}_z(\psi) \cdot \mat{R}_y(\theta) \cdot \mat{R}_x(\varphi)$, \hl{bound.}.
+$\qquad [\vec{n}]^\times = \vec{n} \vec{x}^\top$ (matrix from vec + arg $\vec{x}$)
+
+\shortdefinition[Rot. Vec]
+$\vec{\alpha} = \alpha \vec{n}$ ($\vec{n}$ normal)\\
+$\mat{R}(\alpha, \vec{n}) = \mat{I}_3 + \sin(\alpha)[\vec{n}]^\times + (1 - \cos(\alpha))([\vec{n}]^\times)^2$
+
+\shortdefinition[Quaternions] $q = q_w + q_x i + q_y j + q_z k$ with\\
+$i^2 = j^2 = k^2 = -1$, ($ij = -ji = k$, same for $jk$ and $ki$)
+% TODO: Finish this
+
+\shortdefinition[Transf. M] $\mat{T}_{AB} = \begin{bmatrix}
+        \mat{R}_{AB}        & {_A}\vec{t}_B \\
+        \mat{0}_{1\times 3} & 1
+    \end{bmatrix}$\\
+$\mat{T}_{BA} = \mat{T}_{AB}^{-1} =
+    \begin{bmatrix}
+        \mat{R}_{AB}^\top    & -\mat{R}_{AB}^\top {_A}\vec{t}_B \\
+        \mat{0}_{1 \times 3} & 1
+    \end{bmatrix}$
+$\mat{T}_{AC} = \mat{T}_{AB} \mat{T}_{BC}$
@@ -0,0 +1,8 @@
+\subsection{Forward Kinematics (FK)}
+$T_{WB_n}(\theta) = \mat{T}_{WB_0} \mat{T}_{B_0B_1}(\theta_1) \cdots \mat{T}_{B_{n - 1}B_n}(\theta_n)$.\\
+For 2R system: ${_W}\vec{t}_{WE} = \begin{bmatrix}
+    L_1 \cos(\theta_1) + L_2 \cos(\theta_1 + \theta_2)\\
+    L_1 \sin(\theta_1) + L_2 \sin(\theta_1 + \theta_2)
+\end{bmatrix}$\\
+With workspace (pos) $W$ for $\theta_1, \theta_2 \in [-\pi, \pi]$
+% TODO: Example? (w02s42 possibly)
@@ -0,0 +1,15 @@
+\subsection{Inverse Kinematics (IK)}
+\begin{wrapfigure}[10]{r}{0.4\columnwidth}
+    \includegraphics[width=0.4\columnwidth]{assets/inverse-kinematics.png}
+\end{wrapfigure}
+\bi{Option}: Solve Forward Kinematics for angles.\\
+\bi{Better}: Law of cosine with polar coordinates. Compute angle using cosine rule,\\
+$\theta_1 = \phi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$
+
+(Positive for {\color{ForestGreen} Elbow Down}, Negative for {\color{red} Elbow Up})
+
+\bi{Extension to 6R}:
+1. Waist: spherical coords (2 sol.)\\
+2. 2 sols from 2R for shoulder + elbow\\
+3. Solve for wrist joints (no influence on pos)
+
@@ -0,0 +1,23 @@
+\subsection{Temporal Models}
+For \bi{Cont-time n.-lin. system of ODE} $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t))$, with measurements $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.\\
+Need linearised (around $\vec{f}_C(\vec{\overline{x}}, \vec{\overline{y}}) = 0$, at \bi{equilibrium}):\\
+$\delta \vec{\dot{x}}(t) = \vec{f}_C(\vec{\overline{x}}, \vec{\overline{u}}) + \mat{F}_C \delta \vec{x}(t) + \mat{G}_C \delta \vec{u}(t) + \mat{L}_C \vec{w}(t)$\\
+$\delta \vec{z}(t) = \mat{H} \delta \vec{x}(t) + \vec{v}(t)$.
+Herein, $\mat{H}$ is measurements, $\mat{F}_C$ system, $\mat{G}$ input gain, $\vec{w}$ process noise, $\vec{v}$ measurement noise, both zero-mean \bi{Gaussian White Noise Process}.
+
+For \bi{n-lin. cont-time system}:
+$\vec{\dot{x}}(t) = \vec{f}_C(\vec{x}(t), \vec{u}(t), \vec{w}(t))$\\
+$\vec{z}(t) = \vec{h}(\vec{x}(t)) = \vec{v})(t)$,
+linearization is the same
+
+To \bi{discretize}, integrate from $t_{k - 1}$ to $t_k$:\\
+$\vec{x}_k = \vec{f}(\vec{x}_{k - 1}, \vec{u}_k, \vec{w}_k)$
+$\vec{z}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$,
+\bi{linearised}:\\
+$\delta \vec{x}_k = \vec{f}(\vec{\overline{x}}, \vec{\overline{u}}) + \mat{F} \delta \vec{x}_{k - 1} + \mat{G}_k \delta \vec{u}_k + \mat{L}_k \vec{w}_k$;
+$\delta \vec{z}_k = \mat{H}_k \delta \vec{x}_k$
+
+\bi{Trapezoidal num. int} 
+$\Delta \vec{x}_1 = \Delta t \vec{f}_C (\vec{x}_{k - 1}, \vec{u}_{k - 1}, t_{k - 1})$\\
+$\Delta \vec{x}_2 = \Delta t \vec{f}_C (\vec{x}_{k - 1} + \Delta \vec{x}_1, \vec{u}_{k}, t_{k})$, then:\\
+$\vec{x}_k = \vec{x}_{k - 1} + 0.5 \cdot (\Delta \vec{x}_1 + \Delta \vec{x}_2)$
@@ -0,0 +1,50 @@
+\newpage
+\subsection{Rigid body \& IMU kinematics}
+\begin{wrapfigure}[5]{r}{0.3\columnwidth}
+    \includegraphics[width=0.3\columnwidth]{assets/rigid-body-6d.png}
+\end{wrapfigure}
+\bi{Velocity} ${_I}\vec{v}_{IB} = \diff{t} ({_I}\vec{t}_B)$
+
+\bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \diff{t} (\alpha)\; {_I}\vec{t}$
+
+\bi{Velocity point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$
+
+\bi{Rotation Matrices}
+\begin{itemize}
+    \item For left pertubing\\
+          $\mat{\dot{R}}_{IB} = [{_I} \omega_{IB}]^\times \mat{R}_{IB}$
+    \item For right pertubing
+          $\mat{\dot{R}}_{IB} = \mat{R}_{IB} [{_I} \omega_{IB}]^\times$
+    \item Constant angular velocity ($\exp{[\Delta \alpha]^\times} = \delta \mat{R}(\Delta \alpha)$)\\
+          $\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \alpha]^\times} \mat{R}_{IB}(t)$
+\end{itemize}
+
+\bi{Quaternions}
+\begin{itemize}
+    \item For left pertubing
+          $\displaystyle \vec{\dot{q}}_{IB} = \frac{1}{2} \begin{bmatrix}
+                  {_I}\vec{\omega}_{IB} \\
+                  0
+              \end{bmatrix}
+              \otimes \vec{q}_{IB}$
+    \item For right pertubing
+          $\displaystyle \vec{\dot{q}}_{IB} = \frac{1}{2} \vec{q}_{IB} \otimes \begin{bmatrix}
+                  {_B}\vec{\omega}_{IB} \\
+                  0
+              \end{bmatrix}$
+\end{itemize}
+
+\bi{IMU} (Outputs {\color{blue} ${_S}\vec{\tilde{a}}$} (accel.), {\color{red} ${_S}\vec{\tilde{\omega}}$} (rot. accel.))\\
+${_W}\vec{\dot{t}}_S = {_W} \vec{v}$,
+$\displaystyle \vec{\dot{q}}_{WS} = \frac{1}{2} \vec{q}_{WS} \otimes
+    \begin{bmatrix}
+        {\color{red}{_S}\vec{\tilde{\omega}}} {\color{gray} + \vec{w}_g - \vec{b}_g} \\
+        0
+    \end{bmatrix}$
+
+    ${_W}\vec{\dot{v}} = \mat{R}_{WS}\; ({\color{blue}{_S}\vec{\tilde{a}}} {\color{gray} + \vec{w}_a - \vec{b}_a}) + {_W}\vec{g}$
+    where {\color{gray} gray parts} only IRL (in theor. models, leave out), with $\vec{\dot{b}}_g = \vec{w}_{b_g}$ and $\vec{\dot{b}}_a = \vec{w}_{b_a}$
+
+\bi{IMU Sensor Model}: $\vec{\tilde{z}} = \vec{b}_C + s\mat{M}\vec{z} + \vec{b} + \vec{n} + \vec{o}$
+where bias $\vec{b}$ and scale $s$ often modelled time-varying $\dot{b}(t) = \sigma_C n(t)$.
+$\vec{b}_C$ const. calib; $\mat{M}$ Misalignment; $\vec{n}$ noise; $\vec{o}$ other infl.
@@ -0,0 +1,11 @@
+\subsection{Rigid Body Dynamics}
+\shortdefinition[Newton II] For fin. body w/ mass $m$ and inertia mat. $I$, with force $\vec{F}$ and torque $\vec{T}$ on \bi{Centre of Mass} (CoM), expressed in body frame:
+
+\rmvspace[1.5]
+\begin{align*}
+    {_B}\vec{F} &= \sum {_B}\vec{F}_i = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM}           \\
+    {_B}\vec{T} &= \sum {_B}\vec{T}_i = \mat{I}({_B} \vec{\dot{\omega}}) + {_B} \vec{\omega} \times \mat{I}_B\vec{\omega}
+\end{align*}
+
+\rmvspace
+${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. world frame
@@ -0,0 +1,58 @@
+\subsection{Wheeled robot Kinematics}
+\begin{wrapfigure}[7]{r}{0.2\columnwidth}
+    \includegraphics[width=0.2\columnwidth]{assets/wheel-constraints.png}
+\end{wrapfigure}
+\bi{Non-holonomic} systems \textbf{not integrable}, no inst. move in every direct.
+
+\bi{Wheel constraints} $v_i = \omega_i r_i$
+
+\begin{itemize}
+    \item \textit{Driving straight} all $\vec{v}$ equal
+    \item \textit{Turning} Wheel axis must intersect the \bi{Instant Centre of Rotation} (ICR),
+          speeds: $v_i \div R_i = \Omega$ ($R_i$ dist. wheel-ICR, $\Omega$, vehicle body rotation rate)
+\end{itemize}
+
+\bi{Maneuverability}
+\begin{itemize}
+    \item Deg. of Mobility: $\delta_m = 3 - $\#constrained directions
+    \item Deg. of Steerability: $\delta_s = $\#steerable wheels
+    \item Deg. of Maneuverability: $\delta_M = \delta_m + \delta_s$
+\end{itemize}
+
+\bi{Wheel Configurations}
+
+\includegraphics[width=1\columnwidth]{assets/wheel-config.png}
+
+\begin{scriptsize}
+    \begin{tabular}{llllll}
+        Bicyle         & Tricycle       & Ackermann      & Diff. Drive    & Two-Steer      & Three-Steer    \\
+        $\delta_m = 1$ & $\delta_m = 1$ & $\delta_m = 0$ & $\delta_m = 2$ & $\delta_m = 1$ & $\delta_m = 0$ \\
+        $\delta_s = 1$ & $\delta_s = 1$ & $\delta_s = 2$ & $\delta_s = 0$ & $\delta_s = 2$ & $\delta_s = 3$ \\
+        $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 3$ & $\delta_M = 3$ \\
+    \end{tabular}
+\end{scriptsize}
+
+\bi{Differential Drive Kinematics}
+
+\bi{State vec} $\vec{x} = [x_1, x_2, \theta]^\top$,
+\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, $r_r$ radius of right wheel, $w$ width of robot
+
+\bi{Gen. eq. of Motion} $\dot{x}_1 = v\cos(\theta)$, $\dot{x}_2 = v\sin(\theta)$, $\dot{\theta} = \Omega$,
+with $v = 0.5\cdot(\omega_l r_l + \omega_r + r_r)$, $\Omega = \frac{\omega_r r_r - \omega_l r_l}{w}$
+
+% TODO: Consider adding wheel constraints (planar) here as well (from W05 slides)
+\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.\\
+$\vec{b}_s = \begin{bmatrix}
+        D \cos(\theta) \\
+        D \sin(\theta) \\
+        0
+    \end{bmatrix}$
+$\vec{b}_t = \begin{bmatrix}
+    R(\sin(\Delta \theta + \theta) - \sin(\theta))\\
+    -R(\cos(\Delta \theta + \theta) - \cos(\theta))\\
+    \Delta \theta
+\end{bmatrix}$
+
+\textit{Turning}: $\Omega = (\omega_l r_l) / R_l\! =\! (\omega_r r_r) / R_r$, $R\! =\! v / \Omega$, $\Delta \theta\! =\! \Omega \Delta t$
+
+\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} b_i$ with $i \in \{s, t\}$. ($\int \ldots \dx \Delta t$)
@@ -0,0 +1,6 @@
+\bi{Meas. Model}: $\vec{z} = \vec{h}(\vec{x}) + \vec{v} + \vec{o}$, with $\vec{h}(\vec{x})$ deterministic mean, 
+$\vec{v}$ zero-mean noise, $\vec{o}$ unmodelled effects, $\vec{x}$ true state
+
+\bi{Motor encoders} Typ. 64-2048 incrm. per rev; Estim. rot
+
+\bi{Rolling-Shutter} Most CMOS sensors don't take full image at once, need time stamp for each row
@@ -0,0 +1,7 @@
+\subsection{GNSS}
+Need ultra-precise time sync ($c \approx 0.3\; m/ns$). \bi{Errors}
+\begin{itemize}
+    \item Multipath problem (signal bounce) (0.5 - 100m)
+    \item Ionosphere delays (10m)
+    \item Satellite pos. err, trop. delay (1m)
+\end{itemize}
@@ -0,0 +1,17 @@
+\subsection{Actuators}
+\bi{Hydraulic} {\color{ForestGreen} acc., easy control, power}; {\color{red} maint., speed, price}
+
+\bi{Pneumatic} {\color{ForestGreen} price, shock abs., speed}; {\color{red} acc., loud, maint.}
+
+\subsubsection{DC Motor}
+\begin{wrapfigure}[4]{r}{0.32\columnwidth}
+    \includegraphics[width=0.32\columnwidth]{assets/dc-motor.png}
+\end{wrapfigure}
+(Kirchoff) $U_a = L_a \dot{I_a} + R_a I_a + U_i$
+
+(Torque, Lorenz Force) $T = k_T I_a$
+
+(Induced V, Faraday) $U_i = k_i \omega$
+
+(Mech. pow. eq. el. pow)\\
+$U_i I_a = k_i \omega I_a = T_\omega = k_T I_a \omega \Rightarrow k_i = k_T =: k$
@@ -0,0 +1,67 @@
+\subsection{Cameras}
+\shortdefinition[Pinhole projection]
+$\begin{bmatrix}
+        u & v
+    \end{bmatrix}^\top
+    = \frac{f}{z}
+    \begin{bmatrix}
+        x & y
+    \end{bmatrix}^\top$
+with $f$ the distance to the lens and $z$ the full distance
+
+\newpage
+$u = c_u + f \cdot x'$ and $v = c_v + f \cdot y'$ where $x' = t_x \div t_z$ and $y' = t_y \div t_z$
+where $u, v$ are the pixel $x, y$ coords, $\vec{c} = [c_u, c_v]^\top$ is optical centre of cam in pixel coords, $f$ scale factor,
+and $\vec{{_C}\vec{t}_P} = [t_x, t_y, t_z]^\top$
+
+The full proj: $\vec{u} = \begin{bmatrix}
+        \lambda u \\
+        \lambda v \\
+        \lambda
+    \end{bmatrix}
+    =
+    \begin{bmatrix}
+        f & 0 & c_u \\
+        0 & f & c_v \\
+        0 & 0 & 1
+    \end{bmatrix}
+    \begin{bmatrix}
+        t_x \\t_y\\t_z
+    \end{bmatrix}
+    = \mat{K}\; {_C}\vec{t}_P$
+
+If p. in diff frame ${_W} \vec{t}_P$, then $\vec{u} = \mat{K}[\mat{R}_{CW}\; {_C}\vec{t}_{CW}] = {_W}\vec{t}_P$
+
+\subsubsection{Pinhole Camera Projection with distortion}
+\shortdefinition Model: $\vec{u} = \vec{k}(\vec{d}(\vec{p}({_C}\vec{t}_P)))$, with:
+
+(Projection) $\vec{x'} = \vec{p}({_C}\vec{t}_P) = t_z^{-1} \cdot [t_x, t_y]^\top$
+
+(Distortion model, $r^2 = x'^2 + y'^2$, $\vec{x''} = \vec{d}(\vec{x'})$\\
+$\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} \vec{x'}
+    + \begin{bmatrix}
+        2p_1 x' y' + p_2(r^2 + 2x'^2) \\
+        p_1 (r^2 + 2y'^2) + 2p_2 x'y'
+    \end{bmatrix}$
+
+(Scale and Centre) $\vec{u} = \vec{k}(\vec{x''}) = \text{diag}([f_u, f_v]) \cdot \vec{x''} + \vec{x}$
+
+All with $k_i$ radial distortion params, optional for $i > 2$, $p_i$ tang. dist. param, $f_u, f_v$ focal length in pixels
+
+\shade{green}{Inverse} ${_C}\vec{r} = [\vec{d}^{-1}(\vec{k^{-1}(\vec{u})}), 1]^\top$\\
+(To unit plane)
+$\vec{x''} = \vec{k}^{-1}(\vec{u}) = [f_u^{-1}, f_v^{-1}]^\top (\vec{u} - \vec{c})$
+
+(Un-distort)
+$\vec{x'} = \vec{d}^{-1}(\vec{x''})$ (usually comp. numerically)
+
+(Compute ray)
+${_C}\vec{r} = [\vec{x'}, 1]^\top$
+
+
+\subsubsection{Undestorting a whole image}
+$\vec{u}_i = \vec{k}(\vec{d}(\vec{k_{\text{new}}}^{-1}(\vec{u}_{i, \text{new}})))$
+where $\vec{u}_{i, \text{new}}$ is the place of pixel in output, $\vec{u}_i$ is the input
+
+\bi{Omnidir. Cam} undistortion model with $f(u, v) = \sum_{i = 0}^{N} a_i \rho^i$
+with $\rho = \sqrt{(u - c_u)^2 + (v - c_v)^2}$, $N = 4$ accurately describes it for most fisheye and catadioptric cameras
@@ -0,0 +1,34 @@
+\subsection{Depth and Range sensing}
+\subsubsection{Triangulation-based}
+\bi{Struct. Light} Single cam, single projector: {\color{ForestGreen}Spatial acc}, {\color{red} no worky in bright light, interference with other IR depth cams}
+
+\bi{Active Stereo} 2 cams, 1 proj: {\color{ForestGreen} worky in bright light}, {\color{red} need stereo matching, less accurate, error grows with distance}
+
+
+\subsubsection{Classic Stereo}
+Both images: same plane, focal length, centre, $x$-axis. Given corresponding pixels $[u_l, v]$ and $[u_r, v]$,
+$z = \frac{b \cdot f}{u_r - u_l}$ with $u_l = f\cdot \frac{x}{z} + c_u$ and $u_r = f \cdot \frac{x - b}{z} + c_u$
+
+
+\subsubsection{Time of Flight, Projection}
+{\color{ForestGreen}No occlusions/shadows}, {\color{red}Interference with other dev, multipath leading to larger distances sensed}
+
+\bi{Proj.} $\vec{z} = \begin{bmatrix}
+        \vec{u} \\ d
+    \end{bmatrix} =
+    \begin{bmatrix}
+        \vec{k}(\vec{d}(\vec{p}({_C}\vec{t}_P))) \\
+        [0, 0, 1] {_c}\vec{t}_P
+    \end{bmatrix}$
+\bi{Back}: ${_C}\vec{t}_P
+    = \begin{bmatrix}
+        d \vec{x'} \\d
+    \end{bmatrix}$
+
+
+\subsubsection{Range Sensors}
+\bi{Ultrasonic} Typ. freq: 40kHz - 180kHz, Range: 12cm - 5m, Acc: $\approx$ 2cm, rel error $\approx$ 2\%
+{\color{ForestGreen} meas. for transp. surf., cheap(ish)}, {\color{red} Cone wider, reflect. angle dep, wind / currents}
+
+\bi{LiDAR} Time-of-Flight-based, {\color{ForestGreen}Accuracy, range, works in bright light}, {\color{red} Complex, expensive, one timestamp per measurement}.
+Typical Ranges: up to 100m
@@ -0,0 +1,7 @@
+\subsection{Linearization}
+$\vec{f}(\vec{x}) \approx \vec{f}(\vec{\overline{x}}) + \mat{J}_{\vec{f}} \big|_{x = \overline{x}}(\vec{x} - \vec{\overline{x}})$, $f'$, no vec in 1D; $\vec{\overline{x}}$ lin. p.
+
+\shortdefinition[Jac.] $\mat{J}_{\vec{f}}$ rows for eq of $\vec{f}$ cols for vars of each eq.
+
+Approximation using finite differences $\frac{f(\overline{x} + h) - f(\overline{x})}{h}$,\\
+or central differences (vector of $\frac{\vec{f}(\vec{\overline{x}}) + h_i \vec{e_i}}{h_i}$, with $\vec{e_i}$ unit vec)
@@ -0,0 +1,10 @@
+\subsection{Linear Least Squares}
+\bi{Goal}: $\argmin{x \in \R^n} ||\mat{A}\vec{x} - b||^2_2$, $\mat{A}$: rows $i$-th datap. col $c$: $t_i^{c - 1}$.
+
+\bi{Man. sol.}: comp. $M = A^\top A$, $b' = A^\top b$, then $Mx = b'$.
+
+\bi{Prob. sol.}: $\text{argmax} p(\vec{x} \divider \vec{z})$ with
+\begin{itemize}
+    \item \bi{Max. Like} $p(\vec{x} | \vec{z}) \propto p(\vec{z} | \vec{x}) = \prod_{i = 1}^N p(\vec{z}_i | \vec{x})$
+    \item \bi{M a Post} $p(\vec{x} | \vec{z}) \propto p(\vec{z} | \vec{x}) p(\vec{x}) = p(\vec{x}) \prod_{i = 1}^N p(\vec{z}_i | \vec{x})$
+\end{itemize}
@@ -0,0 +1,8 @@
+\subsection{Non-Linear Least Squares}
+Find $\vec{x}^* = \text{argmax} p(\vec{x} | \vec{z}) = \argmin{}(-\log(p(\vec{x}|\vec{z})))$
+
+\bi{Gauss-Newton} % TODO: Do we really need these? If so, use from NumCS (much simpler notation)
+
+\bi{Levenberg-Marquardt}
+
+\bi{Local Param.}
@@ -0,0 +1,7 @@
+\subsection{Bayes Filter}
+$\vec{x}_k^R$ state at time k, $\vec{z}_k^p$ dist. meas., $\vec{u}^p_k$ wheel odometry (= meas.).
+Typ. care ab. curr. state: altern. pred. \& update. 
+% TODO: Do we really need the below?
+% Init prev distr $\P[\vec{x}_0^R]$.
+% Pred: $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1 : k - 1}^d] = \int \P[\vec{x}_k^R \divider \vec{u}_{k}^p, \vec{x}_{k - 1}^d]
+% \P[\vec{x}_k^R \divider \vec{u}_{1:k - 1}^p, \vec{z}_{1 : k - 1}^d] \dx \vec{x}_{k - 1}^R$
@@ -0,0 +1,8 @@
+\subsection{Particle Filter}
+Is a bayes filter approximating the state distribution with a set of random samples.
+Update step:
+\begin{itemize}
+    \item Apply Bayes rule $w'_{k, s} = \P[\vec{z}_i \divider \vec{x}_{k, s}] w_{k - 1, s}$
+    \item Renormalize: $w_{k, s} = w'_{k, s} \div \sum_{s} w'_{k, s}$
+    \item Resample: rand. sel. $S$ particles acc. to weights and $w_{k, s} = S^{-1}$
+\end{itemize}
@@ -0,0 +1,19 @@
+\subsection{Kalman Filtear (KF)}
+Bayes Filter for Gauss. dist of R.V. \& linear meas. model.
+Initial state $\vec{x}_0 \sim \cN(\hat{\vec{x}}, \mat{P}_0)$, $\mat{P}_0$ previous covariance;
+
+\bi{Prediction} With linear state transition model ($\vec{u}_k$ odometry, $\vec{w}_k$ noise (covariance $\mat{Q}_k$)):\\
+$\vec{x}_k = \mat{F}\vec{x}_{k - 1} + \mat{G}\vec{u}_k + \mat{L}\vec{w}_k$ with $\vec{w}_k \sim \cN(\vec{0}, \mat{Q}_k)$:
+\begin{itemize}
+    \item \bi{Mean} $\hat{\vec{x}}_{k | k - 1} = \mat{F} \hat{\vec{x}}_{k - 1} + \mat{G} \vec{u}_k$
+    \item \bi{Covariance} $\mat{P}_{k | k - 1} = \mat{F} \mat{P}_{k - 1 | k - 1} \mat{F}^\top + \mat{L} \mat{Q}_{k} \mat{L}^\top$
+\end{itemize}
+\bi{Update} Lin. meas.: $\tilde{\vec{z}}_k = \mat{H}\vec{x}_k + \vec{v}_k$ with $\vec{v_k} \sim \cN(\vec{0}, \mat{R}_k)$:
+\newpage
+\begin{itemize}
+    \item \bi{Meas. residual}: $\vec{y}_k = \tilde{\vec{z}}_k - \mat{H} \hat{\vec{x}}_{k | k - 1}$
+    \item \bi{Resid. Cov}: $\mat{S}_k = \mat{H}\mat{P}_{k | k - 1} \mat{H}^\top \mat{S}_k^{-1}$
+    \item \bi{Kalman gain}: $\mat{K}_k = \mat{P}_{k | k - 1} \mat{H}^\top \mat{S}_k^{-1}$
+    \item \bi{Updated mean}: $\hat{\vec{x}}_{k | k} = \hat{\vec{x}}_{k | k - 1} + \mat{K}_k \vec{y}_k$
+    \item \bi{Updated Cov.}: $\mat{P}_{k | k} = (\mat{I} - \mat{K}_k \mat{H}) \mat{P}_{k | k - 1}$
+\end{itemize}
@@ -0,0 +1,12 @@
+\subsection{Extended Kalman Filater (EKF)}
+Non-l. state trans. model $\vec{x}_k = \vec{f}(\vec{x}_{k - 1}, \vec{u}_k, \vec{w}_k)$ as above:
+\begin{itemize}
+    \item \bi{Mean}: $\hat{\vec{x}}_{k | k - 1} = \vec{f}(\hat{\vec{x}}_{k - 1 | k - 1}, \vec{u}_k)$
+    \item \bi{Cov.}: $\mat{P}_{k | k - 1} = \mat{F}_k \mat{P}_{k - 1 | k - 1} \mat{F}_k^\top + \mat{L}_k \mat{Q}_k \mat{L}_k^\top$\\
+        With $\mat{F}_k$ linearisation $\frac{\partial \vec{f}}{\partial \vec{x}}$ and $\mat{L}_k$ lin. $\frac{\partial \vec{f}}{\partial \vec{w}}$
+\end{itemize}
+\bi{Update} N-Lin. meas.: $\tilde{\vec{z}}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$:
+\begin{itemize}
+    \item \bi{Meas. residual}: $\vec{y}_k = \tilde{\vec{z}}_k - \vec{h}(\hat{\vec{x}}_{k | k - 1})$
+\end{itemize}
+Difference to above: $\mat{H}$ becomes $\mat{H}_k$, and $\mat{H}^\top$ is $\mat{H}_k^\top$
@@ -0,0 +1,18 @@
+\subsection{Keypoints}
+\bi{Corner det.} $SSD(\Delta_x, \Delta_y) \approx [\Delta_x \; \Delta_y] \mat{M} [\Delta_x \; \Delta_y]^\top$
+with $\mat{M} = \mat{R}^\top \text{diag}(\lambda_1, \lambda_2) \mat{R}$; $\lambda_i$ E.V. of $M$;
+$\mat{R} = \det(M) - \kappa \cdot \text{trace}(M)^2 = \lambda_1\lambda_2 - \kappa(\lambda_1 + \lambda_2)^2$;
+$\displaystyle M = \sum_{x, y \in P} \begin{bmatrix}
+        I_x^2   & I_x I_y \\
+        I_x I_y & I_y^2
+    \end{bmatrix}$
+
+
+\shade{gray}{Blob Detection} ($I$ is the image)
+
+\bi{Laplacian of Gaussian} (LoG): $L = g(x, y, t) \cdot I(x, y)$.
+Then apply Laplacian Operator $\nabla_\text{norm}^2 L = t\left( \frac{\partial^2 L}{\partial x^2} + \frac{\partial L}{\partial y^2} \right)$
+
+\bi{Diff. of Gaussians} (DoG): $\Delta L = L(x, y, t) - L(x, y, kt)$
+
+\bi{SIFT Detector} \bi{(1)} Subsample + Blur \bi{(2)} DoG on each res. image \bi{(3)} Keypoints extrema in DoG pyramid
@@ -0,0 +1,30 @@
+\subsection{Bootstrapping}
+
+\bi{PnP Problem} {\scriptsize Persp. n-P.}
+Find sol. for camera pose \textit{directly}
+
+\bi{RANSAC} {\scriptsize RANdom SAmpling Consensus} for find. outliers \& correct
+
+\bi{Stereo Triang.} Given two rays (known poses for points in 2D).
+Find good point in 3D. Fast sol: \bi{Midpoint Method}:
+
+\bi{1} Find p. along ray w/ min. dist (Lin. Least Squares)
+\[
+    \vec{\lambda}\! =\! [\lambda_1 \; \lambda_2]^\top\! = \! \argmin{} ||({_W}\vec{t}_{C_2} + \lambda_2 {_W}\vec{e}_2) - ({_W}\vec{t}_{C_1} + \lambda_1 {_W}\vec{e}_2)||^2
+\]
+
+\rmvspace
+\bi{2} Solve normal equation $\mat{A} \vec{\lambda} = \vec{b}$ with $\vec{q} = -{_W}\vec{e}^\top_1 {_W}\vec{e}_2$:
+\[
+    \mat{A} = \begin{bmatrix}
+        1       & \vec{q} \\
+        \vec{q} & 1
+    \end{bmatrix}
+    \quad
+    \vec{b} = \begin{bmatrix}
+        \vec{e}_1^\top \cdot ({_W}\vec{t}_{C_2} - {_W}\vec{t}_{C_1}) \\
+        -\vec{e}_2^\top \cdot ({_W}\vec{t}_{C_2} - {_W}\vec{t}_{C_1})
+    \end{bmatrix}
+\]
+
+\bi{3} Pick midp. ${_W}\vec{t}_P \! = \! 0.5(\tau_1 \! + \! \tau_2)$; $\tau_n \! = \! {_W}\vec{t}_{C_n} + \lambda_n{_W}\vec{e}_n)$
@@ -0,0 +1,5 @@
+\subsection{Place Recognition}
+Idea: Build vocab from ``visual words'' (in Training).
+\bi{Runtime} Detect keypoints; Extract descriptors; Build histogram; Query DB for similarity; If no match, insert into DB, else use to compute with e.g. RANSAC
+
+% TODO: Iterative Closest Point (ICP), explained well in Slide Deck 10 recap
@@ -0,0 +1,34 @@
+\newpage
+\subsection{Mapping}
+\bi{Problem Formulations} \textit{Localisation} {\scriptsize (always static given map)}:\\
+$\vec{x}^*_{R, k} = \text{argmax}\; \P(x_{R, k} \divider \vec{x}_M, \vec{z}_{1 : k}, \vec{u}_{1 : k})$ (recursive)\\
+$\vec{x}^*_{R, 1:k} = \text{argmax}\; \P(x_{R, 1:k} \divider \vec{x}_M, \vec{z}_{1 : k}, \vec{u}_{1 : k})$ (batch)
+
+\textit{SLAM} $\{ \vec{x}^*_{R, k}, \vec{x}^*_M \} \! = \! \text{argmax}\; \P(\vec{x}_R, \vec{x}_M \divider \vec{z}_{1 : k}, \vec{u}_{1 : k})$ (as $\uparrow$)
+
+\textit{Mapp.}: $\vec{x}_M^* \! = \! \text{argmax}\; \P(\vec{x}_M \divider \vec{x}^*_{R, 1:k}, \vec{z}_{1 : k}, \vec{u}_{1 : k})$ with given poses $\vec{x}^*_{R, 1:k}$.
+Thus temp. model $\vec{u}_{1:k}$ doesn't matter.
+
+\bi{Prob. Occ. Grid} $\P(f_j) = \P(\neg o_j) = 1 - \P(o_j)$, with $\P(o_j)$ prob. cell $j$ occupied; pairwise independent.
+
+\bi{Occ. Map. w/ depth sensor} $\P(o_j \divider \vec{x}_{R, 1:k}) =$
+\[
+    \frac{
+        {\color{MidnightBlue} \P(o_j \divider \vec{x}_{R, k}, \vec{z}_k)}
+        \P(\vec{z}_k \divider \vec{x}_{R, k})
+        {\color{ForestGreen} \P(o_j \divider \vec{x}_{R, 1 : k - 1}, \vec{z}_{1 : k - 1})}
+    }{{\color{DarkOrchid} \P(o_j)} \P(\vec{z}_k \divider \vec{x}_{R, 1 : k}, \vec{z}_{1 : k - 1})}
+\]
+{\color{DarkOrchid} map prior}; {\color{ForestGreen} Prev. occ. est.}; {\color{MidnightBlue} Occ. based on curr. range meas.};
+
+\bi{Update function}:
+$l(a) := \log(\text{Odds}(a))$ with $\text{Odds}(a) = \frac{\P(a)}{1 - \P(a)}$ (inv. sensor model):
+$l(o_j | \vec{x}_{R, 1 : k}, \vec{z}_{1 : k})$ =
+\[
+    l(o_j | \vec{x}_{R, k}, \vec{z}_k) + l(o_j | \vec{x}_{R, 1 : k - 1}, \vec{z}_{1 : k - 1}) - l(o_j)
+\]
+
+\shade{gray}{In 3D} 3D voxel $j$ as signed dist. $s$ and weight $w$, update:
+$\displaystyle s_k = \frac{w_{k - 1} s_{k - 1} + \tilde{s}_k}{w_{k - 1} + 1}$ with $w_k = \min(w_{\max}, w_{k - 1} + 1)$
+
+\bi{Impl.} Using HashTables or octree
@@ -0,0 +1,2 @@
+\subsection{Dense Tracking and Loop Closure}
+Idea: Min. sum of sq. errors over all pixels w/ Gauss-Newton.
@@ -0,0 +1,2 @@
+\subsection{SISO \& MIMO}
+\bi{S{\scriptsize ingle-}I{\scriptsize nput-}S{\scriptsize ingle-}O{\scriptsize utput}}: $r$ Reference