diff --git a/electives/amr/autonomous-mobile-robots-cheatsheet.pdf b/electives/amr/autonomous-mobile-robots-cheatsheet.pdf index 4b248b0..aa72754 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 74ae56d..673c1c0 100644 --- a/electives/amr/parts/00_basics/00_probability.tex +++ b/electives/amr/parts/00_basics/00_probability.tex @@ -10,7 +10,7 @@ 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)$ -\shortdefinition[Cond. Indep.] iff $\P(\cX \cap \cY | \cZ) = \P(\cX | \cY) \P(\cY | \cZ)$ +\shortdefinition[Cond. Indep.] iff $\P(\cX \cap \cY | \cZ) = \P(\cX | \cZ) \P(\cY | \cZ)$ \shortdefinition $\E[\vec{x}] = \int_{-\8}^{\8} \vec{x} \P(\vec{x}) \dx \vec{x}$, also for $\vec{x} = \vec{f(x)}$ @@ -19,6 +19,6 @@ e.g. $\sum_{X} \P(X) = 1$ becomes $\int \P(x) \dx = 1$ \shortdefinition[Gauss. Dist.] $\vec{x} \sim \cN(\vec{\mu}, \mat{\Sigma})$ ($\vec{\mu}$ mean, $\mat{\Sigma}$ cov.),\\ PDF: $f(\vec{x}) = \frac{1}{\sqrt{(2\pi)^k \det(\mat{\Sigma})}} \text{exp}\left( -\frac{1}{2}(\vec{x} - \vec{\mu})^\top \mat{\Sigma}^{-1} (\vec{x} - \vec{\mu}) \right)$ -$\Sigma^{-1}$ for $\Sigma$ diagonal, inverse of diag els (e.g. $\sigma^{-1}$) +$\Sigma^{-1}$ for $\Sigma$ diagonal, inverse of diag els (e.g. $\sigma^{-2}$ for $\sigma^2$ on diags) \hl{Always Normalize (i.e. sum of all probabilities is 1)} diff --git a/electives/amr/parts/01_kinematics/00_intro.tex b/electives/amr/parts/01_kinematics/00_intro.tex index 058b5ec..ed3ee33 100644 --- a/electives/amr/parts/01_kinematics/00_intro.tex +++ b/electives/amr/parts/01_kinematics/00_intro.tex @@ -38,7 +38,7 @@ $\begin{bmatrix} $\begin{smallmatrix} \psi = \arcsin\left( R_{21} \div \sqrt{1 - R_{31}^2} \right)\\ \theta = \arcsin(-R_{31})\\ - \varphi = \arcsin\left( R_{31} \div \sqrt{1 - R_{31}^2} \right)\\ + \varphi = \arcsin\left( R_{32} \div \sqrt{1 - R_{31}^2} \right)\\ \end{smallmatrix}$ $[\vec{n}]^\times = \begin{bmatrix} 0 & -n_3 & n_2 \\ @@ -51,7 +51,7 @@ For pitch axis $\theta = \pm 90\deg$, \bi{Gimbal Lock}, Jacobian \bi{singular}. \shortdefinition[Angle-Axis] $\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 \bi{quaternion}: $\vec{q} = [\vec{n}, \alpha]$ +To \bi{quaternion}: $\vec{q} = [\vec{n} \sin(\alpha \div 2), \cos(\alpha \div 2)]$ \shortdefinition[Quaternions] $q = q_w + q_x i + q_y j + q_z k$ with\\ @@ -63,7 +63,7 @@ $\vec{q} = \begin{bmatrix} \bi{Mult} {\scriptsize $\vec{q} \otimes \vec{p} = \begin{bmatrix} - a(\vec{q}) \vec{v}(\vec{p}) + a(\vec{p}) + \vec{v}(\vec{q}) \times \vec{v}(\vec{p}) \\ + a(\vec{q}) \vec{v}(\vec{p}) + a(\vec{p})\vec{v}(\vec{q}) + \vec{v}(\vec{q}) \times \vec{v}(\vec{p}) \\ a(\vec{q}) a(\vec{p}) - \vec{v}(\vec{q})^\top \vec{v}(\vec{p}) \end{bmatrix}$ } diff --git a/electives/amr/parts/01_kinematics/01_forward.tex b/electives/amr/parts/01_kinematics/01_forward.tex index 3a7b3e7..87e6e72 100644 --- a/electives/amr/parts/01_kinematics/01_forward.tex +++ b/electives/amr/parts/01_kinematics/01_forward.tex @@ -8,10 +8,10 @@ $ \begin{bmatrix} L_1 \sin(\theta_1) + L_2 \sin(\theta_1 + \theta_2) \end{bmatrix}$\\ } -Workspace $W$: $\theta_1, \theta_2 \in [-\pi, \pi]$. +Configuration space: $\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}: See \ref{sec:manipulator-velocity-kinematics}. +\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}. diff --git a/electives/amr/parts/01_kinematics/02_inverse.tex b/electives/amr/parts/01_kinematics/02_inverse.tex index c548e36..22a2c92 100644 --- a/electives/amr/parts/01_kinematics/02_inverse.tex +++ b/electives/amr/parts/01_kinematics/02_inverse.tex @@ -12,7 +12,7 @@ $\theta_1 = \varphi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$ 2. 2 sols from 2R for shoulder + elbow\\ 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}$ +\bi{For Inv. velocity}: $\vec{\dot{\theta}} = J_S^{-1}(\vec{\theta}) {_W} \dot{\vec{t}}_{WE}$ % \shortdefinition[Denavit-Hartengerb (DH)] Serial chains % \[ diff --git a/electives/amr/parts/01_kinematics/03_temporal-models.tex b/electives/amr/parts/01_kinematics/03_temporal-models.tex index d1d8a29..510b2c4 100644 --- a/electives/amr/parts/01_kinematics/03_temporal-models.tex +++ b/electives/amr/parts/01_kinematics/03_temporal-models.tex @@ -11,7 +11,7 @@ With: $\pardiff{t}\vec{x}(t) = f_C(\vec{x}(t), \vec{u}(t))$ the model for the ro \hrule \vspace{0.5mm} -\bi{Linearised} at $\vec{f}_C(\vec{\overline{x}}, \vec{\overline{y}}) = 0$, at \bi{equilibrium} {\scriptsize (if $\neq 0$, then add it)}:\\ +\bi{Linearised} at $\vec{f}_C(\vec{\overline{x}}, \vec{\overline{u}}) = 0$, at \bi{equilibrium} {\scriptsize (if $\neq 0$, then add it)}:\\ $\delta \vec{\dot{x}}(t) = \mat{F}_C \delta \vec{x}(t) + \mat{G}_C \delta \vec{u}(t) + \mat{L}_C \vec{w}(t)$; $\delta \vec{z}(t) = \mat{H} \delta \vec{x}(t) + \vec{v}(t)$. @@ -19,23 +19,17 @@ $\mat{F}_C$ system (often a Jac. / Taylor, vars are $x_i$. If no $x$ in $\mat{F} $\mat{G}$ input gain (often Jac / Taylor, vars are $u_i$, if no $u_i$, then lin.), $\vec{w}$ process noise, $\mat{H}$ is measurement, $\vec{v}$ measurement noise, both zero-mean \bi{Gauss. White Noise Proc.}. $\vec{u}_k$ inputs at time $k$. -% TODO: Taylor approximation -\vspace{0.5mm} -\hrule -\vspace{0.5mm} - - \bi{Discretized} $\vec{x}_k = \vec{f}(\vec{x}_{k - 1}, \vec{u}_k, \vec{w}_k)$; $\vec{z}_k = \vec{h}(\vec{x}_k) + \vec{v}_k$ \bi{Linearised}: $\delta \vec{x}_k = \mat{F} \delta \vec{x}_{k - 1} + \mat{G}_k \delta \vec{u}_k + \mat{L}_k \vec{w}_k$; -$\delta \vec{z}_k = \mat{H}_k \delta \vec{x}_k$ +$\delta \vec{z}_k = \mat{H}_k \delta \vec{x}_k + \vec{v}(t)$ For discretized, iterative integral from $t_{k - 1}$ to $t_k$ -Linearization happens typically with one of the below: +Numerical integration typically happens with one of the below: \shortdefinition[Euler-Forward] $\vec{x}_k = \vec{x}_{k - 1} + \Delta t \vec{f}_C(\vec{x}_{k - 1}, \vec{u}_{k - 1}, t_{k - 1})$ 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 4c457b7..3e54fcf 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 @@ -13,7 +13,7 @@ \item For left pertubing\\ $\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$ + $\mat{\dot{R}}_{IB} = \mat{R}_{IB} [{_B} \omega_{IB}]^\times$ \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} @@ -41,7 +41,7 @@ }$ \end{itemize} -\bi{IMU} (Outputs {\color{blue} ${_S}\vec{\tilde{a}}$} (accel.), {\color{red} ${_S}\vec{\tilde{\omega}}$} (rot. accel.))\\ +\bi{IMU} (Outputs {\color{blue} ${_S}\vec{\tilde{a}}$} (accel.), {\color{red} ${_S}\vec{\tilde{\omega}}$} (rot. vel.))\\ ${_W}\vec{\dot{t}}_S = {_W} \vec{v}$; $\quad \displaystyle \vec{\dot{q}}_{WS} = \frac{1}{2} \vec{q}_{WS} \otimes {\scriptsize 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 307bc4a..8b9009e 100644 --- a/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex +++ b/electives/amr/parts/01_kinematics/05_rigid-body-dynamics.tex @@ -2,7 +2,7 @@ \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} & = m({_B} \vec{\dot{v}}_{CoM}) + m_B \vec{\omega} \times {_B}\vec{v}_{CoM} \\ + {_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*} @@ -25,7 +25,7 @@ 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!} +{\scriptsize Legged robots don't need to maintain static stability!} \shortdefinition[Static Walking] 3 or more legs on ground, stable if frozen, safe, slow, inefficient. diff --git a/electives/amr/parts/02_Sensors-Actuators/02_actuators.tex b/electives/amr/parts/02_Sensors-Actuators/02_actuators.tex index 977a7cb..70e8c30 100644 --- a/electives/amr/parts/02_Sensors-Actuators/02_actuators.tex +++ b/electives/amr/parts/02_Sensors-Actuators/02_actuators.tex @@ -14,4 +14,4 @@ (Induced V, Faraday) $U_i = k_i \omega$ (Mech. pow. $=$ electric power)\\ -$U_i I_a = k_i \omega I_a = T_\omega = k_T I_a \omega \Rightarrow k_i = k_T =: k$ +$U_i I_a = k_i \omega I_a = T \omega = k_T I_a \omega \Rightarrow k_i = k_T =: k$ diff --git a/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex b/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex index 1062f33..9b34665 100644 --- a/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex +++ b/electives/amr/parts/02_Sensors-Actuators/03_cameras.tex @@ -53,7 +53,7 @@ $\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}$ (To unit plane) -$\vec{x''} = \vec{k}^{-1}(\vec{u}) = [f_u^{-1}, f_v^{-1}]^\top (\vec{u} - \vec{c})$ +$\vec{x''} = \vec{k}^{-1}(\vec{u}) = \text{diag}([f_u^{-1}, f_v^{-1}]) (\vec{u} - \vec{c})$ (Un-distort) $\vec{x'} = \vec{d}^{-1}(\vec{x''})$ (usually comp. numerically) 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 c9ac545..c134dec 100644 --- a/electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex +++ b/electives/amr/parts/03_multi-sensor-estimation/00_linearization.tex @@ -8,6 +8,6 @@ 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 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) +or central differences (vector of $\frac{\vec{f}(\vec{\overline{x}} + h_i \vec{e_i}) - \vec{f}(\vec{\bar{x}} - h_i \vec{e_i})}{h_i}$, with $\vec{e_i}$ unit vec) % TODO: Expand this 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 b7acc51..f73dadc 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 @@ -23,10 +23,10 @@ def gauss_newton(x: np.ndarray, F, DF, tol=1e-6): \bi{Levenberg-Marquardt} \bi{(1)} Pick start point $\overline{\vec{x}}^0$ and start param $\lambda^0 = \max \text{diag}(\mat{A})$ and $v$ (e.g. $v = 2$).; -\bi{(2)} Modified GN sys: $\mat{A} + \lambda \text{diag}(\mat{A})) \Delta \vec{x} = \vec{b}$; +\bi{(2)} Modified GN sys: $\mat{A} + \lambda \text{diag}(\mat{A}) \Delta \vec{x} = \vec{b}$; \bi{(3)} Solve for $\Delta \vec{x}$; -\bi{(4)} Update: $\overline{\vec{x}}^{k + 1} = \overline{\vec{x}}^k + \Delta \vec{x}$ (if cost reduced), -else: $\overline{\vec{x}}^{k + 1} = \overline{\vec{x}}^k$, $\lambda^{k + 1} = \lambda^k v$, go to step 3; +\bi{(4)} Update: $\overline{\vec{x}}^{k + 1} = \overline{\vec{x}}^k + \Delta \vec{x}$ and $\lambda^{k + 1} = \lambda^k \div v$ (if cost reduced), +else: $\overline{\vec{x}}^{k + 1} = \overline{\vec{x}}^k$ and $\lambda^{k + 1} = \lambda^k v$, go to step 3; \bi{(5)} Check convergence, else go to step 2 \bi{Robust Cost Functions} Account for outliers, by mod. err. terms 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 bc82564..24cadfc 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,5 +1,5 @@ \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.). +$\vec{x}_k^R$ state at time k, $\vec{z}^d$ 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]$ 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 49cd2d0..96ef2f0 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,4 +17,5 @@ $\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). +All with $G$ the input model, $L$ noise map, $\tilde{\vec{z}}_k$ actual measurement, +$K$ trusts measurement when $R_k$ small (or large, but not in between) relative to $P_{k | k - 1}$. 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 9b32ad4..87315e7 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 @@ -20,6 +20,6 @@ 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. +$F_k$ is of form $[1, 0, \vec{d}_1; 0, 1, \vec{d}_2; 0, 0, 1]$, with $\vec{d} = b_s$ (with first row negated) 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 53b62ab..9b1b26b 100644 --- a/electives/amr/parts/04_vision/00_keypoints.tex +++ b/electives/amr/parts/04_vision/00_keypoints.tex @@ -20,8 +20,8 @@ $\mat{M} = \sum_{x, y \in P} \shade{gray}{Blob Detection} ($I$ is the image) -\bi{Laplacian of Gaussian} (LoG): $L = g(x, y, t) \cdot I(x, y)$. -Then apply Laplacian Operator $\nabla_\text{norm}^2 L = t\left( \frac{\partial^2 L}{\partial x^2} + \frac{\partial L}{\partial y^2} \right)$ +\bi{Laplacian of Gaussian} (LoG): $L = g(x, y, t) * I(x, y)$. +Then apply Laplacian Operator $\nabla_\text{norm}^2 L = t\left( \frac{\partial^2 L}{\partial x^2} + \frac{\partial^2 L}{\partial y^2} \right)$ \bi{Diff. of Gaussians} (DoG): $\Delta L = L(x, y, t) - L(x, y, kt)$ diff --git a/electives/amr/parts/04_vision/01_bootstrapping.tex b/electives/amr/parts/04_vision/01_bootstrapping.tex index d929599..e756077 100644 --- a/electives/amr/parts/04_vision/01_bootstrapping.tex +++ b/electives/amr/parts/04_vision/01_bootstrapping.tex @@ -14,7 +14,7 @@ Find good point in 3D. Fast sol: \bi{Midpoint Method}: \bi{1} Find p. along ray w/ min. dist (Lin. Least Squares) \[ - \vec{\lambda}\! =\! [\lambda_1 \; \lambda_2]^\top\! = \! \argmin{} ||({_W}\vec{t}_{C_2} + \lambda_2 {_W}\vec{e}_2) - ({_W}\vec{t}_{C_1} + \lambda_1 {_W}\vec{e}_2)||^2 + \vec{\lambda}\! =\! [\lambda_1 \; \lambda_2]^\top\! = \! \argmin{} ||({_W}\vec{t}_{C_2} + \lambda_2 {_W}\vec{e}_2) - ({_W}\vec{t}_{C_1} + \lambda_1 {_W}\vec{e}_1)||^2 \] \rmvspace @@ -38,4 +38,4 @@ Find good point in 3D. Fast sol: \bi{Midpoint Method}: \quad C_i \text{ cam} \] -\bi{3} Pick midp. ${_W}\vec{t}_P \! = \! 0.5(\tau_1 \! + \! \tau_2)$; $\tau_n \! = \! {_W}\vec{t}_{C_n} + \lambda_n{_W}\vec{e}_n)$ +\bi{3} Pick midp. ${_W}\vec{t}_P \! = \! 0.5(\tau_1 \! + \! \tau_2)$; $\tau_n \! = \! {_W}\vec{t}_{C_n} + \lambda_n {_W}\vec{e}_n$ diff --git a/electives/amr/parts/04_vision/03_mapping.tex b/electives/amr/parts/04_vision/03_mapping.tex index c585eb9..7fe3969 100644 --- a/electives/amr/parts/04_vision/03_mapping.tex +++ b/electives/amr/parts/04_vision/03_mapping.tex @@ -35,9 +35,9 @@ $\displaystyle s_k = \frac{w_{k - 1} s_{k - 1} + \tilde{s}_k}{w_{k - 1} + 1}$ wi \subsubsection{Iterative Closest Point} Build \textit{correspondences}: associate all live scan points $l_i$ to closest map points $m_i$. Error term: -$\vec{e} = \vec{T}_{W L_l L_l} m_i - {_W}\vec{l}_i$. +$\vec{e} = \vec{T}_{W L_l} {_{L_l}}m_i - {_W}\vec{l}_i$. Minimize this via Gauss-Newton, then re-associate, iterate. -\bi{Photometric}: $u_{KF} = \pi(\mat{T}_{WC_{KF}}^{-1} \mat{T}_{WC_l} \pi^{-1}(\vec{u}_{KF}, \mat{D}_{KF}[\vec{u}_{KF}]))$, +\bi{Photometric}: $u_{L} = \pi(\mat{T}_{WC_{KF}}^{-1} \mat{T}_{WC_l} \pi^{-1}(\vec{u}_{KF}, \mat{D}_{KF}[\vec{u}_{KF}]))$, error term $\vec{e} = \mat{I}_{KF}[\vec{u}_{KF}] - \mat{I}_L[\vec{u}_{L}]$, where all subscript $L$ are from live image, all subscript $KF$ key frame. diff --git a/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex b/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex index a7c4b87..17ec1d2 100644 --- a/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex +++ b/electives/amr/parts/05_planning-control/00_feedback-control/00_siso-mimo.tex @@ -3,4 +3,4 @@ $z$ Sys. Out. (e.g. speed of car) \bi{M{\scriptsize ultiple-}I{\scriptsize nput-}M{\scriptsize ultiple-}O{\scriptsize utput}}: $r$ Reference (typ: trajectory), $u$ Sys. Input (e.g. 4 rotor speeds), -$x$ internal states (pos, orient, speed, rot. speed), $z$ Sys. Output (e.g. speed of car) +$x$ internal states (pos, orient, speed, rot. speed), $z$ Sys. Output (e.g. pose, speed) diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex index 49703cf..dd5d0e7 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/00_intro.tex @@ -8,7 +8,6 @@ \bi{Visibility graph}: Connect corners, goal outside obstacles \bi{Voronoi Diagram}: Edges at max. dist. from obst. (benefit: safer paths). -Also tends to be faster than Dijkstra. \bi{Discrete} {\scriptsize via graph/grid}: {\color{ForestGreen} complete solution}, {\color{red} Curse of dimensions} diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex index 125fa35..f7d99e7 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/01_a-star-algorithm.tex @@ -1,5 +1,6 @@ \subsubsection{A* Algorithm} -Function \texttt{h(\dots)} is a lower bound of optimal cost. +Function \texttt{h(\dots)} is a lower bound of optimal cost. +Tends to be faster than Dijkstra. % TODO: If space constrained, move to below % Init vert. with \texttt{dist}, \texttt{totDistEst} to $\8$, \texttt{prev} \texttt{undef}, \texttt{start} \begin{algorithm} diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_rrt.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_rrt.tex index 649d32f..89166b2 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_rrt.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/03_rrt.tex @@ -19,7 +19,7 @@ \State \Comment{Extension of \texttt{RRT}* goes here} \State \texttt{insertEdge}{\texttt{Graph}, $x_n, x_f$} \EndIf - \If{$x_f \texttt{==} x_n$} + \If{$x_f \texttt{==} \text{goal}$} \State \Return{\texttt{SUCCESS}, \texttt{Graph}} \EndIf \EndFor @@ -33,11 +33,19 @@ Extension to RRT* to make path better: \small \begin{algorithmic}[1] \State $X_{\text{near}} \gets$ \Call{neighbours}{Graph, $x_f$, $R$} - \State $x_{\min} \gets$ \Call{neighbours}{Graph, $x_f$, $R$} + \State $x_{\min} \gets x_n$ \State $c_{\min} \gets$ \Call{cost}{$x_n$} + \Call{edgeCost}{$x_n, x_f$} \For{each $x_{\text{near}}$ in $X_\text{near}$} \If{\Call{edgeCollisionFree}{$x_\text{near}, x_f$} and\\ \Call{cost}{$x_\text{near}$} + \Call{edgeCost}{$x_\text{near}, x_f$} $< c_{\min}$} - \State \Call{insertEdge}{\texttt{Graph}, $x_f, x_{\text{near}}$} + \State $x_{\min} \gets x_\text{near}$ + \State $c_{\min} \gets$ \Call{cost}{$x_\text{near}$} + \Call{edgeCost}{$x_\text{near}$, $x_f$} + \EndIf + \EndFor + \State \Call{insertEdge}{\texttt{Graph}, $x_{\min}, x_f$} + \State \Call{cost}{$x_f$} $\gets c_{\min}$ + \For{each $x_{\text{near}}$ in $X_\text{near}$} + \If{\Call{edgeCollisionFree}{$x_f, x_\text{near}$} and\\ \Call{cost}{$x_f$} + \Call{edgeCost}{$x_f, x_\text{near}$} $<$ \Call{cost}{$x_\text{near}$}} + \State \Call{insertEdge}{\texttt{Graph}, $x_f, x_{\text{near}}$} \Comment{new connection} \State $x_\text{parent} \gets$ \Call{parent}{\texttt{Graph}, $x_{\text{near}}$} \State \Call{removeEdge}{\texttt{Graph}, $x_\text{parent}, x_{\text{near}}$} \EndIf @@ -46,4 +54,4 @@ Extension to RRT* to make path better: \end{algorithm} \bi{Informed RRT*} extension: Once conn. betw. start and goal found, restrict sampling to (hyper)ellipsoid. -$b = 0.5 \sqr{d^2 - ||x_s - x_g||^2}$. +$b = 0.5 \sqrt{d^2 - ||x_s - x_g||^2}$. diff --git a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex index b74b1d8..06f8761 100644 --- a/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex +++ b/electives/amr/parts/05_planning-control/01_motion-planning-exploration/04_collision-avoidance.tex @@ -7,7 +7,7 @@ Compute arcs with coll. {\color{ForestGreen} Accounts for Kino-Dyn}, {\color{red \bi{Potential Field Methods} Define {\color{gray} \textit{repulsive}} and {\color{purple} attractive} potential $c = {\color{purple} c_{\text{att}}} + {\color{gray} c_{\text{rep}}}$. With e.g. ${\color{purple} c_\text{att}} = \frac{1}{2} k_\text{att} ||\vec{x} - \vec{x}_\text{goal}||^2$ and ${\color{gray} c_\text{rep}} = \begin{cases} - \frac{1}{2} k_\text{rep} \left( \frac{1}{\rho(\vec{x})} + \frac{1}{\rho_{\lim}} \right) & \rho \leq \rho_{\lim} \\ - 0 & \text{else} + \frac{1}{2} k_\text{rep} \left( \frac{1}{\rho(\vec{x})} + \frac{1}{\rho_{\lim}} \right)^2 & \rho \leq \rho_{\lim} \\ + 0 & \text{else} \end{cases}$ {\color{ForestGreen} Simple control laws}, {\color{red} may trap in loc. min., no diff. const, no guar. to avoid coll} diff --git a/electives/amr/parts/05_planning-control/02_learning-to-act/02_reinforcement-learning.tex b/electives/amr/parts/05_planning-control/02_learning-to-act/02_reinforcement-learning.tex index eaaa800..979109d 100644 --- a/electives/amr/parts/05_planning-control/02_learning-to-act/02_reinforcement-learning.tex +++ b/electives/amr/parts/05_planning-control/02_learning-to-act/02_reinforcement-learning.tex @@ -2,9 +2,9 @@ \bi{Passive} \textit{Direct Evaluation} Act according to policy $\vec{\pi}$, store sum of discounted rewards, average them. (But too simple) \textit{Sample-Based} Use $V_{i + 1}^{\vec{\pi}}(\vec{x}) = Q(\vec{x}, \vec{\pi}(\vec{x}))$ w/ $V_0^\pi(\vec{x}) = 0$. -We need state trans. model, instead $\tilde{R}_j$ (approx. prob. w/ statistics) and thus +We need state transition model for that (we don't have that), instead $\tilde{R}_j$ (approx. probability w/ statistics) and thus \[ - V_{i + 1}^{\vec{x}}(\vec{x}) = \frac{1}{N} \sum_{j = 1}^{N} \tilde{R}_j(\vec{x}, \vec{\pi}(\vec{x}), \vec{x}_+) + \gamma V_i^{\vec{\pi}}(\vec{x}_+) + V_{i + 1}^{\vec{\pi}}(\vec{x}) = \frac{1}{N} \sum_{j = 1}^{N} \left( \tilde{R}_j(\vec{x}, \vec{\pi}(\vec{x}), \vec{x}_+) + \gamma V_i^{\vec{\pi}}(\vec{x}_+) \right) \] \bi{Active} Find optimal policy $\vec{\pi}$ instead of state values $V(\vec{x})$.