[AMR] updated first 4.5 sections

2026-09-10 19:15:25 +02:00 · 2026-08-08 12:31:01 +02:00
parent 7cdba7dca9
commit c1bdbc64bf
20 changed files with 103 additions and 55 deletions
@@ -2,7 +2,7 @@
 \label{sec:ms-lin}
 $\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.
+\shortdefinition[Jacobian] $\mat{J}_{\vec{f}}$ rows for eq of $\vec{f}$; cols for vars of each eq.
 It may also be a single value (if just one var in the state)

 \shortdefinition[Gradient] $\nabla \vec{f}$ is vec, each comp. for par diff of var
@@ -5,9 +5,11 @@ Alt: compute sum of squared errors $S$, then min. $S$, i.e. solve system $\pardi

 \bi{Man. sol.}: comp. $M = A^\top A$, $b' = A^\top b$, then $Mx = b'$.

+\bi{Numpy}: \texttt{numpy.linalg.lstsq}
+
 \bi{Prob. sol.}: $\text{argmax} \; \P(\vec{x} \divider \vec{z})$ with Maximum \dots
 \begin{itemize}
    \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}
+{\scriptsize Gauss-Dist (non)-linear meas. processed in batches, MLE is weighted non-linear Least Squares}
@@ -1,3 +1,4 @@
+\newpage
 \subsection{Non-Linear Least Squares}
 Find $\vec{x}^* = \text{argmax}\; \P(\vec{x} | \vec{z}) = \argmin{}(-\log(\P(\vec{x}|\vec{z})))$

@@ -1,7 +1,13 @@
 \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.).
 Typically care about current state: alternate predict \& update.
+Prediction with Product/Sum rule for $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1:k - 1}^d]$,
+Update with Bayes' Theorem for $\P[\vec{x}_k^R \divider \vec{u}_{1:k}^p, \vec{z}_{1:k}^d]$
+
 % 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$
+% Pred: {\scriptsize $\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$}
+%
+% Update: {\scriptsize $\P[\vec{x_k^R \divider \vec{u}_{1:k}}^p, \vec{z}_{1:k}^d] = \eta \P[\vec{x}_{1:k}^p] $}
+% with $\eta$ a normalizer
@@ -1,6 +1,7 @@
 \subsection{Particle Filter}
-Is a bayes filter approximating the state distribution with a set of random samples. Dist. not necessarily unimodal.
-Update step:
+Is a Bayes filter approximating the state with $S$ weighted particles $\{ x_{k, s}, w_{k, s} \}$. Distribution not necessarily unimodal.
+
+Predict: Push each particle through transition model; 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}$
@@ -17,3 +17,4 @@ $\vec{x}_k = \mat{F}\vec{x}_{k - 1} + \mat{G}\vec{u}_k + \mat{L}\vec{w}_k$ with
    \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}
+All with $G$ the input model, $L$ noise map, $\tilde{\vec{z}}_k$ actual measurement, $K$ trust measurement ($R_k$ small vs $P_{k | k - 1}$ small).
@@ -13,5 +13,13 @@ With $\mat{F}_k = \frac{\partial \vec{f}}{\partial \vec{x}}$ and $\mat{L}_k = \f
 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?
+\bi{Limitations} Linearization error (shifts bias or sharpens it), fixed by UKF (by passing charact. points around mean through nonlin. func instead of Jac) or Iterative EKF
+
+\shortexample[Diff-Drive Lawn Mower] Known: Radii ($r_l$ and $r_r$) and track $w$. Measurements: wheel increments ($\Delta \varphi_l$ and $\Delta \varphi_r$).
+State $[x, y, \theta]^\top$. Can compute distance, heading change \& turn radius.
+
+For \bi{State Transition}:
+$D = 0.5(r_l \Delta \varphi_l + r_r \Delta \varphi_r)$, rest same as in \ref{sec:diff-drive-kin}.
+$F_k$ is of form $[1, 0, \vec{d}_1; 0, 1, \vec{d}_2; 0, 0, 1]$, with $\vec{d} = b_s$ or $\vec{d} = b_t$ for straight and turning, respectively.
+
+\bi{Updates} are linear (GPS: $[x, y]^\top$ and compass: $\theta$, matrix: $\mat{I}$)