[AMR] Almost finish multi-sensor-evaluation

electives/others/amr/autonomous-mobile-robots-cheatsheet.tex

@@ -65,6 +65,10 @@
 \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/}
 
 \end{document}

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

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

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

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

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

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

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

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