[AMR] Many learnings from exercises

2026-09-10 19:15:25 +02:00 · 2026-08-03 15:50:56 +02:00
parent d50e6bd547
commit a106331872
21 changed files with 81 additions and 38 deletions
@@ -44,7 +44,6 @@
 \end{center}


-% TODO: Graphics
 \section{Introduction}
 \input{parts/00_basics/00_probability.tex}
 \input{parts/00_basics/01_measurement-models.tex}
@@ -80,6 +79,7 @@
 % \input{parts/03_multi-sensor-estimation/}

 \section{SLAM to Spatial AI}
+{\scriptsize SLAM = Simultaneous Localization and Mapping}
 \input{parts/04_vision/00_keypoints.tex}
 \input{parts/04_vision/01_bootstrapping.tex}
 \input{parts/04_vision/02_place-recognition.tex}
@@ -6,3 +6,5 @@
 \shortdefinition[Vec Line Eq] $\vec{p} + \lambda \vec{d}$

 \shortdefinition[Determinant] $ad - bc$ for mat $[a, b; c, d]$
+
+\shortdefinition[Eigenvalues] Solve $\det(M - \lambda I) = 0$
@@ -1,4 +1,4 @@
 \subsection{Error Propagation}
 \label{sec:error-propagation}
 For functions $\vec{f}(\vec{x}) = \mat{A}\vec{x}$, the \bi{linear error propagation}
-is given by $\Sigma^f = A \Sigma^x \A^\top$, with $\Sigma^x$ the uncertanty of $\vec{x}$ (covariance mat.)
+is given by $\Sigma^f = A \Sigma^x A^\top$, with $\Sigma^x$ the uncertanty of $\vec{x}$ (covariance mat.)
@@ -9,9 +9,9 @@ $ \begin{bmatrix}
    \end{bmatrix}$\\
 }
 Workspace $W$: $\theta_1, \theta_2 \in [-\pi, \pi]$.
-Similar for $n$R sys (more angles, more lengths).
-For 2D move in 3D space, last dim is sum of angles (or equiv).
-The Jacobian of this $nD$ position is typically computed ($nD$ pos is a vec).
+\bi{Computation} Similar for $n$R sys (more angles, more lengths).
+Last dim is typically sum of angles (or equiv).
+\bi{For velocity kinematics}: Comp. Jacobian of this $nD$ position ($nD$ pos is vec). ${_W}\vec{t}_{WE} \mat{J}(\vec{\theta}) \cdot \vec{\dot{\theta}}$
 \bi{Jacobian}: see \ref{sec:ms-lin}.

 \shortdefinition[Singularity] Loss of deg of Freedom $\det(\mat{J}(\vec{\theta})) = 0$
@@ -12,3 +12,4 @@ $\theta_1 = \varphi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$
 2. 2 sols from 2R for shoulder + elbow\\
 3. Solve for wrist joints (no infl. on pos)

+\bi{For Inv. velocity}: $\vec{\dot{\theta}} = J_S^{-1}(\vec{\theta}) {_W} \vec{t}_{WE}$
@@ -1,18 +1,45 @@
 \subsection{Temporal Models}
-Model \bi{robot dyn} as \bi{Cont-time n.-lin. system of ODE} $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t), \vec{w}(t))$, with meas. $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.\\
-Linearize 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)$.
-$\mat{H}$ is meas., $\mat{F}_C$ system, $\mat{G}$ input gain, $\vec{w}$ process noise, $\vec{v}$ measurement noise,
-both zero-mean \bi{Gaussian White Noise Process}.
+\label{sec:temporal-models}
+\inlinenotation Linearization typically written as $\delta X$.

-To \bi{discretize}, integrate from $t_{k - 1}$ to $t_k$:\\
+Model \bi{robot dyn} as \bi{Cont-time non-lin. system of ODE}:\\
+$\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t), \vec{w}(t))$, measurement $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.
+
+With: $\pardiff{t}\vec{x}(t) = f_C(\vec{x}(t), \vec{u}(t))$ the model for the robot state update and $\vec{h}(\vec{x}(t))$ the model for the measurements (e.g. for IMU)
+
+\vspace{0.5mm}
+\hrule
+\vspace{0.5mm}
+
+\bi{Linearised} at $\vec{f}_C(\vec{\overline{x}}, \vec{\overline{y}}) = 0$, at \bi{equilibrium} {\scriptsize (if $\neq 0$, then add it)}:\\
+$\delta \vec{\dot{x}}(t) = \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)$.
+
+$\mat{F}_C$ system (often a Jac. / Taylor, vars are $x_i$. If no $x$ in $\mat{F}$, then linear),
+$\mat{G}$ input gain (often Jac / Taylor, vars are $u_i$, if no $u_i$, then lin.), $\vec{w}$ process noise, $\mat{H}$ is measurement, $\vec{v}$ measurement noise,
+both zero-mean \bi{Gauss. White Noise Proc.}. $\vec{u}_k$ inputs at time $k$.
+
+% TODO: Taylor approximation
+\vspace{0.5mm}
+\hrule
+\vspace{0.5mm}
+
+
+\bi{Discretized}
 $\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$;
+$\vec{z}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$
+
+\bi{Linearised}:
+$\delta \vec{x}_k = \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$

+For discretized, iterative integral from $t_{k - 1}$ to $t_k$
+
+Linearization happens typically with one of the below:
+
+\shortdefinition[Euler-Forward] $\vec{x}_k = \vec{x}_{k - 1} + \Delta t \vec{f}_C(\vec{x}_{k - 1}, \vec{u}_{k - 1}, t_{k - 1})$
+
+\newpage
 \shortdefinition[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:\\
@@ -1,4 +1,3 @@
-\newpage
 \subsection{Rigid body \& IMU kinematics}
 \begin{wrapfigure}[5]{r}{0.3\columnwidth}
    \includegraphics[width=0.3\columnwidth]{assets/rigid-body-6d.png}
@@ -19,7 +19,6 @@ To compute $\vec{c}$ in $\vec{c} \cdot {_B}\vec{v}_{WB} = \omega$
 $c_i = [\sin(\alpha + \beta) \div r, -\cos(\alpha + \beta) \div r, -\cos(\beta) \cdot l \div r]$


-\newpage
 \bi{Maneuverability}
 \begin{itemize}
    \item Deg. of Mobility: $\delta_m = 3 - $\#constrained directions
@@ -3,6 +3,7 @@

 \bi{Pneumatic} {\color{ForestGreen} price, shock abs., speed}; {\color{red} acc., loud, maint.}

+\newpage
 \subsubsection{DC Motor}
 \begin{wrapfigure}[4]{r}{0.32\columnwidth}
    \includegraphics[width=0.32\columnwidth]{assets/dc-motor.png}
@@ -4,5 +4,9 @@ $\vec{f}(\vec{x}) \approx \vec{f}(\vec{\overline{x}}) + \mat{J}_{\vec{f}} \big|_

 \shortdefinition[Jac.] $\mat{J}_{\vec{f}}$ rows for eq of $\vec{f}$; cols for vars of each eq.

+\shortdefinition[Gradient] $\nabla \vec{f}$ is vec, each comp. for par diff of var
+
 Part. diff; Approx. 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)
+
+% TODO: Expand this
@@ -8,3 +8,4 @@
    \item \bi{Likelihood} $\P(\vec{x} | \vec{z}) \propto \P(\vec{z} | \vec{x}) = \prod_{i = 1}^N \P(\vec{z}_i | \vec{x})$
    \item \bi{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}
+{\scriptsize Gauss-Dist (non)-lineaer meas. processed in batches, MLE is weighted non-linear Least Squares}
@@ -1,9 +1,10 @@
 \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} Need func $F$ and its Jacobian (or derivative) $D$
-\rmvspace
+\bi{Gauss-Newton} Need func $F$ and its Jacobian (or derivative) $DF$
+\rmvspace[0.3]
 % TODO: Error propagation laws
+{\small
    \begin{minted}[
    breaklines,
    breakindentnchars=2,
@@ -13,13 +14,15 @@ def gauss_newton(x: np.ndarray, F, DF, tol=1e-6):
    x = x-s; k = 1
    while np.linalg.norm(s) > tol * np.linalg.norm(x):
        s = np.linalg.lstsq(DF(x), F(x))[0]
-        x = x-s; k += 1 # k opt for max iter
+        x = x-s; k += 1 # k optional for max iter
    return x, k
 \end{minted}
-\rmvspace
+}
+\rmvspace[0.4]

 \bi{Levenberg-Marquardt}
-Minimize $||F(x^{(k)}) + DF(x^{(k)})|| + \lambda||s||$.
+Minimize $||F(x^{(k)}) + DF(x^{(k)})||^2 + \lambda||s||^2$,
+with $s = \lambda_k DF(x^{(k)})$, $\lambda_k$ is called the \textit{learning rate}
 % TODO: Do we need these?

 \bi{Local Param.}
@@ -1,6 +1,6 @@
 \subsection{Bayes Filter (in DAG)}
 $\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. 
+Typically care about current state: alternate predict \& 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]
@@ -1,5 +1,5 @@
 \subsection{Particle Filter}
-Is a bayes filter approximating the state distribution with a set of random samples.
+Is a bayes filter approximating the state distribution with a set of random samples. Dist. not necessarily unimodal.
 Update step:
 \begin{itemize}
    \item Apply Bayes rule $w'_{k, s} = \P[\vec{z}_i \divider \vec{x}_{k, s}] w_{k - 1, s}$
@@ -3,7 +3,7 @@ Bayes Filter for Gauss. dist of R.V. \& \hl{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$)):\\
+\bi{Prediction} With lin. state trans. model ($\vec{u}_k$ wheel spd, $\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$
@@ -2,13 +2,16 @@
 \hl{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{Covariance}: $\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}}$
+    \item \bi{Covariance}: $\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$
 \end{itemize}
-\bi{Update} Non-Linear 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 KF: $\mat{H}$ becomes $\mat{H}_k$, and $\mat{H}^\top$ is $\mat{H}_k^\top$
+With $\mat{F}_k = \frac{\partial \vec{f}}{\partial \vec{x}}$ and $\mat{L}_k = \frac{\partial \vec{f}}{\partial \vec{w}}$ (computed as are Jacobians)

-% TODO: Consider adding examples
+\bi{Update} Non-Linear measurement: $\tilde{\vec{z}}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$:
+\begin{itemize}
+    \item \bi{Measurement residual}: $\vec{y}_k = \tilde{\vec{z}}_k - \vec{h}(\hat{\vec{x}}_{k | k - 1})$
+\end{itemize}
+Difference to KF: $\mat{H}$ becomes $\mat{H}_k$, and $\mat{H}^\top$ is $\mat{H}_k^\top$.
+They are linearizations of $\vec{h}$, see \ref{sec:temporal-models}
+
+\shortexample D-D Rob, measurements: $\Delta \varphi_l, \varphi_r$, GPS pos, yaw angle.
+% TODO: Finish this?
@@ -1,11 +1,10 @@
 \subsection{Bootstrapping}

-\bi{PnP Problem} {\scriptsize Persp. $n$-P.}
+\bi{PnP Problem} {\scriptsize Perspective $n$-Point}
 Find sol. for camera pose \textit{directly}

 \bi{RANSAC} {\scriptsize RANdom SAmpling Consensus} for find. outliers \& correct
-% TODO: Do we need to know this algorithm / be able to implement it?
-% -> likely not
+Also great for finding an initial guess for pose. (due to robustness)

 \bi{Stereo Triang.} Given two rays (known poses for points in 2D).
 Find good point in 3D. Fast sol: \bi{Midpoint Method}:
@@ -3,6 +3,7 @@ Function \texttt{h(\dots)} is a lower bound of optimal cost.
 % TODO: If space constrained, move to below
 % Init vert. with \texttt{dist}, \texttt{totDistEst} to $\8$, \texttt{prev} \texttt{undef}, \texttt{start}
 \begin{algorithm}
+    \small
    \begin{algorithmic}[1]
        \Procedure{Astar}{\texttt{Graph, start, goal}}
            \For{each \texttt{v} in \texttt{Graph.Verticies}}
@@ -1,5 +1,7 @@
+\newpage
 \subsubsection{Rapidly-Exploring Random Tree (RRT)}
 \begin{algorithm}
+    \small
    \begin{algorithmic}[1]
        \Procedure{RRT}{\texttt{start, goal}}
            \State \Call{insertVertex}{\texttt{start, Graph}}
@@ -28,6 +30,7 @@
 Returns a collision-free path as graph. Need nearest neighbour search.
 Extension to RRT* to make path better:
 \begin{algorithm}
+    \small
    \begin{algorithmic}[1]
        \State $X_{\text{near}} \gets$ \Call{neighbours}{Graph, $x_f$, $R$}
        \State $x_{\min} \gets$ \Call{neighbours}{Graph, $x_f$, $R$}
@@ -21,7 +21,7 @@ Then update: $Q_{i + 1}(\vec{x}, \vec{u}) = (1 - \alpha)Q_i(\vec{x}, \vec{u}) +
 Called off-policy learning, needs exploration. Simplest is random actions ($\varepsilon$-greedy):
 $\varepsilon$ is prob. to act randomly, $1 - \varepsilon$ is prob. to act on pol. {\color{red} Space explored, still doing random stuff}

-\bi{Approaches} 
+\bi{DL-Approaches}
 \textit{Model-based} (estimate trans. model, e.g. Dyna),
 Value-based (estimate val or $Q$-func and extract pol., e.g. Q-Learn),
 Actor-Critic (estim. val or $Q$ of curr. pol., improve pol., e.g. A3C, SAC),