diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf index d7b0219..179160d 100644 Binary files a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf and b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf differ diff --git a/electives/amr/parts/00_basics/00_probability.tex b/electives/amr/parts/00_basics/00_probability.tex index 9d228e3..74ae56d 100644 --- a/electives/amr/parts/00_basics/00_probability.tex +++ b/electives/amr/parts/00_basics/00_probability.tex @@ -5,7 +5,7 @@ \shorttheorem[Bayes] $\displaystyle \P(Y_i | X) = \frac{\P(X | Y_i) \P(Y_i)}{\sum_{j = 1}^n \P(X | Y_j) \P(Y_j)}$ -\shortdefinition[Cont. Var] Sums become integrals\\ +\shortdefinition[Cont. Var] Sums become integrals (and vice-versa)\\ e.g. $\sum_{X} \P(X) = 1$ becomes $\int \P(x) \dx = 1$ \shortdefinition[Indep.] $x, y$ indep. iff $\P(\cX \cap \cY) = \P(\cX) \P(\cY)$ diff --git a/electives/amr/parts/00_basics/02_trigonometry.tex b/electives/amr/parts/00_basics/02_trigonometry.tex index 19fe25e..0aabaa9 100644 --- a/electives/amr/parts/00_basics/02_trigonometry.tex +++ b/electives/amr/parts/00_basics/02_trigonometry.tex @@ -5,6 +5,8 @@ \shortdefinition[Orthogonal vec] $v^\top w = 0$; \shortdefinition[Vec Line Eq] $\vec{p} + \lambda \vec{d}$ -\shortdefinition[Determinant] $ad - bc$ for mat $[a, b; c, d]$ +\shortdefinition[Determinant] $ad - bc$ for mat $[a, b; c, d]$. -\shortdefinition[Eigenvalues] Solve $\det(M - \lambda I) = 0$ +\shortdefinition[Eigenvalues] Solve $\det(M - \lambda I) = 0$. + +\shortremark[Matrix Inverse] For diagonal, inverse of diag els diff --git a/electives/amr/parts/01_kinematics/00_intro.tex b/electives/amr/parts/01_kinematics/00_intro.tex index eaac7e7..058b5ec 100644 --- a/electives/amr/parts/01_kinematics/00_intro.tex +++ b/electives/amr/parts/01_kinematics/00_intro.tex @@ -41,17 +41,17 @@ $\begin{smallmatrix} \varphi = \arcsin\left( R_{31} \div \sqrt{1 - R_{31}^2} \right)\\ \end{smallmatrix}$ $[\vec{n}]^\times = \begin{bmatrix} - 0 & -a_3 & a_2 \\ - a_3 & 0 & -a_1 \\ - -a_2 & a_1 & 0 + 0 & -n_3 & n_2 \\ + n_3 & 0 & -n_1 \\ + -n_2 & n_1 & 0 \end{bmatrix}$ -For pitch axis $= \pm 90\deg$, Gimbal Lock, Jacobian singular. +For pitch axis $\theta = \pm 90\deg$, \bi{Gimbal Lock}, Jacobian \bi{singular}. \shortdefinition[Angle-Axis] -$\vec{\alpha} = \alpha \vec{n}$ ($\vec{n}$ normal); Convert to rot. mat\\ +$\vec{\alpha} = \alpha \vec{n}$ ($\vec{n}$ normal); To \bi{rotation matrix}:\\ $\mat{R}(\alpha, \vec{n}) = \mat{I}_3 + \sin(\alpha)[\vec{n}]^\times + (1 - \cos(\alpha))([\vec{n}]^\times)^2$\\ -To quat: $\vec{q} = [\vec{n}, \alpha]$ +To \bi{quaternion}: $\vec{q} = [\vec{n}, \alpha]$ \shortdefinition[Quaternions] $q = q_w + q_x i + q_y j + q_z k$ with\\ @@ -68,14 +68,14 @@ $\vec{q} = \begin{bmatrix} \end{bmatrix}$ } -\bi{To Rot Mat} {\scriptsize +\bi{To Rot Mat} { $R_{AB} = \mat{I}_3 + 2a(\vec{q}_{AB}) [\vec{v}(\vec{q}_{AB})]^\times + 2([\vec{v}(\vec{q}_{AB})]^\times)^2$ } \shortdefinition[Transf. M] {\scriptsize - $\mat{T}_{AC} = \mat{T}_{AB} \mat{T}_{BC}$ R.-Handed typ; Aero: Left-H\\ + $\mat{T}_{AC} = \mat{T}_{AB} \mat{T}_{BC}$ Right-Handed typically; Aero: Left-H\\ $\mat{T}_{AB} = \begin{bmatrix} \mat{R}_{AB} & {_A}\vec{t}_B \\ \mat{0}_{1\times 3} & 1 diff --git a/electives/amr/parts/01_kinematics/01_forward.tex b/electives/amr/parts/01_kinematics/01_forward.tex index dac53b4..3a7b3e7 100644 --- a/electives/amr/parts/01_kinematics/01_forward.tex +++ b/electives/amr/parts/01_kinematics/01_forward.tex @@ -11,7 +11,8 @@ $ \begin{bmatrix} Workspace $W$: $\theta_1, \theta_2 \in [-\pi, \pi]$. \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}. +\bi{For velocity kinematics}: See \ref{sec:manipulator-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$ diff --git a/electives/amr/parts/01_kinematics/02_inverse.tex b/electives/amr/parts/01_kinematics/02_inverse.tex index f0c1119..c548e36 100644 --- a/electives/amr/parts/01_kinematics/02_inverse.tex +++ b/electives/amr/parts/01_kinematics/02_inverse.tex @@ -13,3 +13,13 @@ $\theta_1 = \varphi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$ 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}$ + +% \shortdefinition[Denavit-Hartengerb (DH)] Serial chains +% \[ +% \mat{T}_{L_j L_{j + 1}} = \mat{T}^{R_z}(\theta_j) \mat{T}^{t_z}(d_j) \mat{T}^{t_x}(a_j) \mat{T}^{R_x}(\alpha_j) +% \] +% with $ + +\shortdefinition[Underactuated] workspace $<6$D; IK solvable + +\shortdefinition[Overactuated] $\8$ IK sol; Reduced singularities; Num. sol. diff --git a/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex b/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex index 51a91bb..4c457b7 100644 --- a/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex +++ b/electives/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex @@ -6,7 +6,7 @@ \bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \dot{\alpha} \cdot {_I}\vec{n}$ -\bi{Velocity point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$ +\bi{Vel. point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$ \bi{Rotation Matrices} \begin{itemize} @@ -14,8 +14,8 @@ $\mat{\dot{R}}_{IB} = [{_I} \omega_{IB}]^\times \mat{R}_{IB}$ \item For right pertubing $\mat{\dot{R}}_{IB} = \mat{R}_{IB} [{_I} \omega_{IB}]^\times$ - \item Constant angular velocity ($\exp{[\Delta \vec{\alpha}]^\times} = \delta \mat{R}(\Delta \vec{\alpha})$)\\ - $\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \vec{\alpha}]^\times} \mat{R}_{IB}(t) \quad \vec{\alpha} = {_I}\vec{\omega}_{IB} \delta t$ + \item Constant angular velocity ($\exp{[\Delta \vec{\alpha}]^\times} = \delta \mat{R}(\Delta \vec{\alpha}) \quad \vec{\alpha} = {_I}\vec{\omega}_{IB} \delta t$)\\ + $\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \vec{\alpha}]^\times} \mat{R}_{IB}(t)$ \end{itemize} \bi{Quaternions} @@ -56,4 +56,4 @@ where {\color{gray} gray parts} only IRL (in theor. models, leave out), with $\v \bi{IMU Sensor Model}: $\vec{\tilde{z}} = \vec{b}_C + s\mat{M}\vec{z} + \vec{b} + \vec{n} + \vec{o}$ where bias $\vec{b}$ and scale $s$ often modelled time-varying $\dot{b}(t) = \sigma_C n(t)$. -$\vec{b}_C$ const. calib; $\mat{M}$ Misalignment; $\vec{n}$ noise; $\vec{o}$ other infl. +$\vec{b}_C$ const. calibration / bias; $\mat{M}$ Misalignment; $\vec{n}$ noise; $\vec{o}$ other infl. diff --git a/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex b/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex index cb567e8..307bc4a 100644 --- a/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex +++ b/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex @@ -2,14 +2,12 @@ \label{sec:rigid-body-dynamics} \shortdefinition[Newton II] For fin. body w/ mass $m$ and inertia mat. $I$, with force $\vec{F}$ and torque $\vec{T}$ on \bi{Centre of Mass} (CoM), in body frame: \begin{align*} - {_B}\vec{F} & = \sum {_B}\vec{F}_i = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM} \\ - {_B}\vec{T} & = \sum {_B}\vec{T}_i = \mat{I}({_B} \vec{\dot{\omega}}) + {_B} \vec{\omega} \times \mat{I}_B\vec{\omega} + {_B}\vec{F} & = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM} \\ + {_B}\vec{T} & = \mat{I}({_B} \vec{\dot{\omega}}) + {_B} \vec{\omega} \times \mat{I}_B\vec{\omega} \end{align*} ${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. inert. frame - {\scriptsize Legged robots don't need to maintain stability!} - To determine dynamics: \bi{(1)} Define control inputs (e.g. wheel speeds), \bi{(2)} Assumptions / Simplifications about state \& inputs @@ -17,3 +15,18 @@ To determine dynamics: \bi{(4)} Express as func of states / inp \bi{(5)} Reduce to the CoM \bi{(6)} Plug into Newton-Euler eq combined with rigid body kinematics + +\subsection{Manipulator Velocity Kinematics} +\label{sec:manipulator-velocity-kinematics} +Differentiate FK $x = f(\vec{\theta})$, with $\dot{\vec{x}} = [{_W}\vec{v}, {_W} \vec{\omega}]^\top = \mat{J}(\vec{\theta})\dot{\vec{\theta}}$, +with $\mat{J} = \frac{\partial f}{\partial \vec{\theta}}$ manipulator Jacobian. Then $\dot{\vec{\theta}} = J^{-1} \vec{\dot{x}}$ or $\vec{\dot{\theta}} = J^+ \vec{\dot{x}}$, +if over-actuated, with $J^+ = J^\top(J J^\top)^{-1}$ (Moore Penrose-Inverse) + + +\newpage +\subsection{Legged robots} +{\scriptsize Legged robots don't need to maintain stability!} + +\shortdefinition[Static Walking] 3 or more legs on ground, stable if frozen, safe, slow, inefficient. + +\shortdefinition[Dynamic Walking] $<$ 3 legs on ground, falls when not moving, fast, efficient, hard. diff --git a/electives/amr/parts/01_kinematics/06_wheeled-robot.tex b/electives/amr/parts/01_kinematics/06_wheeled-robot.tex index 2d34b28..0ebf23a 100644 --- a/electives/amr/parts/01_kinematics/06_wheeled-robot.tex +++ b/electives/amr/parts/01_kinematics/06_wheeled-robot.tex @@ -1,11 +1,6 @@ -\newpage \subsection{Wheeled robot Kinematics} -\begin{wrapfigure}[7]{r}{0.2\columnwidth} - \includegraphics[width=0.2\columnwidth]{assets/wheel-constraints.png} -\end{wrapfigure} \bi{Non-holonomic} systems \textbf{not integrable}, no inst. move in every direct. - -\bi{Wheel constraints} $v_i = \omega_i r_i$ ($r_i$ constraints) +\bi{Wheel constraints} $v_i = \omega_i r_i$ ($r_i$ constraints, (wheel radii)) \begin{itemize} \item \textit{Driving straight} all $\vec{v}$ equal (ICR: R.Cent.) \item \textit{Turning} Wheel axis must intersect the \bi{Instant Centre of Rotation} (ICR) of vehicle, @@ -40,16 +35,20 @@ $c_i = [\sin(\alpha + \beta) \div r, -\cos(\alpha + \beta) \div r, -\cos(\beta) \end{tabular} \end{scriptsize} -\shortdefinition[Differential Drive Kinematics] +\subsubsection{Differential Drive Kinematics} +\label{sec:diff-drive-kin} \bi{State vec} $\vec{x} = [x_1, x_2, \theta]^\top$, -\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, radius of right (left) wheel $r_r$ ($r_l$), $w$ width of robot +\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, wheel radii $r_l, r_r$, $w$ width of robot \bi{Gen. eq. of Motion} $\dot{x}_1 = v\cos(\theta)$, $\dot{x}_2 = v\sin(\theta)$, $\dot{\theta} = \Omega$, -with $v = 0.5\cdot(\omega_l r_l + \omega_r + r_r)$, $\Omega = \frac{\omega_r r_r - \omega_l r_l}{w}$ +with $v = 0.5\cdot(\omega_l r_l + \omega_r r_r)$, $\Omega = \frac{\omega_r r_r - \omega_l r_l}{w}$ -% TODO: Consider adding wheel constraints (planar) here as well (from W05 slides) -\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.\\ +\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$. + +\textit{Turning}: $\Omega = (\omega_l r_l) / R_l\! =\! (\omega_r r_r) / R_r$, $R\! =\! v / \Omega$, $\Delta \theta\! =\! \Omega \Delta t$ + +\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} + b_i$ with $i \in \{s, t\}$, respectively, with $\vec{b}_s = {\scriptsize \begin{bmatrix} D \cos(\theta) \\ D \sin(\theta) \\ @@ -59,11 +58,7 @@ $\vec{b}_s = {\scriptsize \begin{bmatrix} R(\sin(\Delta \theta + \theta) - \sin(\theta)) \\ -R(\cos(\Delta \theta + \theta) - \cos(\theta)) \\ \Delta \theta - \end{bmatrix}}$ - -\textit{Turning}: $\Omega = (\omega_l r_l) / R_l\! =\! (\omega_r r_r) / R_r$, $R\! =\! v / \Omega$, $\Delta \theta\! =\! \Omega \Delta t$ - -\textbf{Discretized}: $\vec{x}_k = \vec{x}_{k - 1} + b_i$ with $i \in \{s, t\}$, respectively + \end{bmatrix}}$; $R$ dist ICR \textbf{Swedish Wheels}: Have 3 DoF, typ. (3 pcs) equally spaced on circle. Typ. Param. each: $\alpha, \beta, \gamma$ ($z$, $x$, $y$ axes, oriented along $x$, $z$ up) diff --git a/electives/amr/parts/02_Sensors-Actuators/00_intro.tex b/electives/amr/parts/02_Sensors-Actuators/00_intro.tex index 81ce047..f5d5a5b 100644 --- a/electives/amr/parts/02_Sensors-Actuators/00_intro.tex +++ b/electives/amr/parts/02_Sensors-Actuators/00_intro.tex @@ -7,3 +7,7 @@ Probabilistic M.M: use random variable in definition, and s. \ref{sec:error-prop \bi{Motor encoders} Typ. 64-2048 increments per rev; Estimate rot \bi{Rolling-Shutter} Most CMOS sensors don't take full image at once, need time stamp for each row + +\shortdefinition[Proprioceptive] Robot-internal states (e.g. IMU, encoders) + +\shortdefinition[Exteroceptive] Environment (e.g. LIDAR, Cameras) diff --git a/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex b/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex index a847392..1062f33 100644 --- a/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex +++ b/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex @@ -35,12 +35,13 @@ $\vec{u} = If p. in diff frame, e.g. $W$-frame then $\vec{u} = \mat{K}[\mat{R}_{CW}\; {_C}\vec{t}_{CW}] {_W}\vec{t}_P$ \subsubsection{Pinhole Camera Projection with distortion} -\shortdefinition Model: $\vec{u} = \vec{k}(\vec{d}(\vec{p}({_C}\vec{t}_P)))$, with ($\vec{c}$ as above): +\shortdefinition Model: $\vec{u} = \vec{k}(\vec{d}(\vec{p}({_C}\vec{t}_P)))$, with $\vec{c}$ as above, $k_i$ radial distortion params, optional for $i > 2$, +$p_i$ tangential distortion parameter, $f_u, f_v$ $x$/$y$ focal lengths in pixels. These should be given by task (Projection) $\vec{x'} = \vec{p}({_C}\vec{t}_P) = t_z^{-1} \cdot [t_x, t_y]^\top$ (Distortion model) $r^2 = x'^2 + y'^2$, $\vec{x''} = \vec{d}(\vec{x'})$\\ -$\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}; \vec{x'} +$\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} \cdot \vec{x'} + {\scriptsize \begin{bmatrix} @@ -51,9 +52,6 @@ $\vec{x''} = \! \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k (Scale and Centre) $\vec{u} = \vec{k}(\vec{x''}) = \text{diag}([f_u, f_v]) \cdot \vec{x''} + \vec{c}$ -All with $k_i$ radial distortion params, optional for $i > 2$, $p_i$ tang. dist. param, $f_u, f_v$ focal length in pixels - -\shade{green}{Inverse} ${_C}\vec{r} = [\vec{d}^{-1}(\vec{k^{-1}(\vec{u})}), 1]^\top$\\ (To unit plane) $\vec{x''} = \vec{k}^{-1}(\vec{u}) = [f_u^{-1}, f_v^{-1}]^\top (\vec{u} - \vec{c})$ @@ -65,8 +63,8 @@ ${_C}\vec{r} = [\vec{x'}, 1]^\top$ \subsubsection{Undestorting a whole image} -$\vec{u}_i = \vec{k}(\vec{d}(\vec{k_{\text{new}}}^{-1}(\vec{u}_{i, \text{new}})))$ -where $\vec{u}_{i, \text{new}}$ is the place of pixel in output, $\vec{u}_i$ is the input +$\vec{u}_i = \vec{k}(\vec{d}(\vec{k_{\text{new}}}^{-1}(\vec{u}_{i, \text{new}})))$, +where $\vec{u}_{i, \text{new}}$ is the place of pixel in output, $\vec{u}_i$ is the input, with $k_{\text{new}}$ the new focal lengths and image centre \bi{Omnidir. Cam} undistortion model with $f(u, v) = \sum_{i = 0}^{N} a_i \rho^i$ with $\rho = \sqrt{(u - c_u)^2 + (v - c_v)^2}$, $N = 4$ accurately describes it for most fisheye and catadioptric cameras diff --git a/electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex b/electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex index bbe4ce4..c9ac545 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex @@ -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 diff --git a/electives/amr/parts/03_multi-sensor-estimation/01_least-squares.tex b/electives/amr/parts/03_multi-sensor-estimation/01_least-squares.tex index 2f3fa4a..84508be 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/01_least-squares.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/01_least-squares.tex @@ -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} diff --git a/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex b/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex index cd57477..b7acc51 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex @@ -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})))$ diff --git a/electives/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex b/electives/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex index 014f3c8..bc82564 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/03_bayes-filters.tex @@ -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 diff --git a/electives/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex b/electives/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex index 6522dff..322c739 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/04_particle-filter.tex @@ -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}$ diff --git a/electives/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex b/electives/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex index d4e73de..49cd2d0 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/05_kalman-filter.tex @@ -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). diff --git a/electives/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex b/electives/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex index 329b762..9b32ad4 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/06_extended-kalman-filter.tex @@ -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}$) diff --git a/electives/amr/parts/04_vision/00_keypoints.tex b/electives/amr/parts/04_vision/00_keypoints.tex index 01eeaa5..bd1c2ee 100644 --- a/electives/amr/parts/04_vision/00_keypoints.tex +++ b/electives/amr/parts/04_vision/00_keypoints.tex @@ -1,13 +1,20 @@ \subsection{Keypoints} \bi{Corner det.} $SSD(\Delta x, \Delta y) \approx [\Delta_x \; \Delta_y] \mat{M} [\Delta_x \; \Delta_y]^\top$ -with $\mat{M} = \mat{R}^\top \text{diag}(\lambda_1, \lambda_2) \mat{R}$; $\lambda_i$ E.V. of $M$; $\kappa$ const 0.04-0.15;\\ +$\lambda_i$ E.V. of $M$; $\kappa$ const 0.04-0.15; $\mat{R} = \det(M) - \kappa \cdot \text{TR}(M)^2 = \lambda_1\lambda_2 - \kappa(\lambda_1 + \lambda_2)^2$;\\ -$M = \sum_{x, y \in P} +$\mat{M} = \sum_{x, y \in P} {\scriptsize \begin{bmatrix} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{bmatrix} + = + R^\top + \begin{bmatrix} + \lambda_1 & 0 \\ + 0 & \lambda_2 + \end{bmatrix} + R }$ diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex index 87f4943..a11dd0f 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/02_rrt.tex @@ -1,4 +1,3 @@ -\newpage \subsubsection{Rapidly-Exploring Random Tree (RRT)} \begin{algorithm} \small