[AMR] Many learnings from exercises

electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex

@@ -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

electives/amr/parts/03_multi-sensor-estimation/01_least-squares.tex

@@ -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}

electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex

@@ -1,10 +1,11 @@
 \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
-\begin{minted}[
+{\small
+    \begin{minted}[
     breaklines,
     breakindentnchars=2,
 ]{python}
@@ -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.}

electives/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex

@@ -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]

electives/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex

@@ -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}$

electives/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex

@@ -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$

electives/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex

@@ -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?