diff --git a/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf index 35cf1f3..9379579 100644 Binary files a/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf and b/electives/others/amr/autonomous-mobile-robots-cheatsheet.pdf differ diff --git a/electives/others/amr/autonomous-mobile-robots-cheatsheet.tex b/electives/others/amr/autonomous-mobile-robots-cheatsheet.tex index 2e9a19b..2bb6d4b 100644 --- a/electives/others/amr/autonomous-mobile-robots-cheatsheet.tex +++ b/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} diff --git a/electives/others/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex b/electives/others/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex new file mode 100644 index 0000000..19e807f --- /dev/null +++ b/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$ diff --git a/electives/others/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex b/electives/others/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex new file mode 100644 index 0000000..eafe105 --- /dev/null +++ b/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} diff --git a/electives/others/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex b/electives/others/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex new file mode 100644 index 0000000..438a497 --- /dev/null +++ b/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} diff --git a/electives/others/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex b/electives/others/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex new file mode 100644 index 0000000..f5bb87e --- /dev/null +++ b/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$