[AMR] Finish W3 summary

electives/others/amr/parts/01_kinematics/02_inverse.tex

@@ -2,13 +2,13 @@
 \begin{wrapfigure}[10]{r}{0.4\columnwidth}
     \includegraphics[width=0.4\columnwidth]{assets/inverse-kinematics.png}
 \end{wrapfigure}
-Option: Solve Forward Kinematics for angles.\\
+\bi{Option}: Solve Forward Kinematics for angles.\\
-Better: Law of cosine with polar coordinates. Compute angle using cosine rule,\\
+\bi{Better}: Law of cosine with polar coordinates. Compute angle using cosine rule,\\
 $\theta_1 = \phi \pm \alpha$, $\theta_2 = \pm(\pi - \beta)$
 
 (Positive for {\color{ForestGreen} Elbow Down}, Negative for {\color{red} Elbow Up})
 
-Extension to 6R:
+\bi{Extension to 6R}:
 1. Waist: spherical coords (2 sol.)\\
 2. 2 sols from 2R for shoulder + elbow\\
 3. Solve for wrist joints (no influence on pos)

electives/others/amr/parts/01_kinematics/03_temporal-models.tex

@@ -1,18 +1,23 @@
 \subsection{Temporal Models}
-Often use cont-time n.-lin. system of ODE $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t))$, with measurements $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.
+For \bi{Cont-time n.-lin. system of ODE} $\dot{\vec{x}} = \vec{f}_C(\vec{x}(t), \vec{u}(t))$, with measurements $\vec{z}(t) = \vec{h}(\vec{x}(t)) + \vec{v}(t)$.\\
 Need linearised (around $\vec{f}_C(\vec{\overline{x}}, \vec{\overline{y}}) = 0$, at \bi{equilibrium}):\\
 $\delta \vec{\dot{x}}(t) = \vec{f}_C(\vec{\overline{x}}, \vec{\overline{u}}) + \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)$.
 Herein, $\mat{H}$ is measurements, $\mat{F}_C$ system, $\mat{G}$ input gain, $\vec{w}$ process noise, $\vec{v}$ measurement noise, both zero-mean \bi{Gaussian White Noise Process}.
 
-For n-lin. cont-time system:
+For \bi{n-lin. cont-time system}:
 $\vec{\dot{x}}(t) = \vec{f}_C(\vec{x}(t), \vec{u}(t), \vec{w}(t))$\\
 $\vec{z}(t) = \vec{h}(\vec{x}(t)) = \vec{v})(t)$,
 linearization is the same
 
-To discretize, integrate from $t_{k - 1}$ to $t_k$:
+To \bi{discretize}, integrate from $t_{k - 1}$ to $t_k$:\\
 $\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$,
-linearised:\\
+\bi{linearised}:\\
 $\delta \vec{x}_k = \vec{f}(\vec{\overline{x}}, \vec{\overline{u}}) + \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$
+
+\bi{Trapezoidal num. int} 
+$\Delta \vec{x}_1 = \Delta t \vec{f}_C (\vec{x}_{k - 1}, \vec{u}_{k - 1}, t_{k - 1})$\\
+$\Delta \vec{x}_2 = \Delta t \vec{f}_C (\vec{x}_{k - 1} + \Delta \vec{x}_1, \vec{u}_{k}, t_{k})$, then:\\
+$\vec{x}_k = \vec{x}_{k - 1} + 0.5 \cdot (\Delta \vec{x}_1 + \Delta \vec{x}_2)$

electives/others/amr/parts/01_kinematics/04_rigid-body-imu-kinematics.tex

@@ -1 +1,50 @@
+\newpage
 \subsection{Rigid body \& IMU kinematics}
+\begin{wrapfigure}[5]{r}{0.3\columnwidth}
+    \includegraphics[width=0.3\columnwidth]{assets/rigid-body-6d.png}
+\end{wrapfigure}
+\bi{Velocity} ${_I}\vec{v}_{IB} = \diff{t} ({_I}\vec{t}_B)$
+
+\bi{Rot. Velocity} ${_I}\vec{\omega}_{IB} = \diff{t} (\alpha)\; {_I}\vec{t}$
+
+\bi{Velocity point $P$} ${_B}\vec{v}_{IP} = {_B}\vec{v}_{IB} + {_B}\vec{\omega}_{IB} \times {_B}\vec{t}_{P}$
+
+\bi{Rotation Matrices}
+\begin{itemize}
+    \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$
+    \item Constant angular velocity ($\exp{[\Delta \alpha]^\times} = \delta \mat{R}(\Delta \alpha)$)\\
+          $\mat{R}_{IB}(t + \Delta t) = \exp{[\Delta \alpha]^\times} \mat{R}_{IB}(t)$
+\end{itemize}
+
+\bi{Quaternions}
+\begin{itemize}
+    \item For left pertubing
+          $\displaystyle \vec{\dot{q}}_{IB} = \frac{1}{2} \begin{bmatrix}
+                  {_I}\vec{\omega}_{IB} \\
+                  0
+              \end{bmatrix}
+              \otimes \vec{q}_{IB}$
+    \item For right pertubing
+          $\displaystyle \vec{\dot{q}}_{IB} = \frac{1}{2} \vec{q}_{IB} \otimes \begin{bmatrix}
+                  {_B}\vec{\omega}_{IB} \\
+                  0
+              \end{bmatrix}$
+\end{itemize}
+
+\bi{IMU} (Outputs {\color{blue} ${_S}\vec{\tilde{a}}$} (accel.), {\color{red} ${_S}\vec{\tilde{\omega}}$} (rot. accel.))\\
+${_W}\vec{\dot{t}}_S = {_W} \vec{v}$,
+$\displaystyle \vec{\dot{q}}_{WS} = \frac{1}{2} \vec{q}_{WS} \otimes
+    \begin{bmatrix}
+        {\color{red}{_S}\vec{\tilde{\omega}}} {\color{gray} + \vec{w}_g - \vec{b}_g} \\
+        0
+    \end{bmatrix}$
+
+    ${_W}\vec{\dot{v}} = \mat{R}_{WS}\; ({\color{blue}{_S}\vec{\tilde{a}}} {\color{gray} + \vec{w}_a - \vec{b}_a}) + {_W}\vec{g}$
+    where {\color{gray} gray parts} only IRL (in theor. models, leave out), with $\vec{\dot{b}}_g = \vec{w}_{b_g}$ and $\vec{\dot{b}}_a = \vec{w}_{b_a}$
+
+\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 modled as time-varying state $\dot{b}(t) = \sigma_C n(t)$.
+$\vec{b}_C$ const. calib. and $\vec{n}$ the model.

electives/others/amr/parts/01_kinematics/05_rigid-body-dynamics.tex

@@ -1 +1,11 @@
 \subsection{Rigid Body Dynamics}
+\inlinedefinition[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), expressed in body frame:
+
+\rmvspace[1.5]
+\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}
+\end{align*}
+
+\rmvspace
+${_B} \vec{v}_{CoM}$ vel. of CoM, ${_B}\omega$ rot. speed; both w.r.t. world frame

electives/others/amr/parts/01_kinematics/06_wheeled-robot.tex

@@ -1 +1,57 @@
 \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$
+
+\begin{itemize}
+    \item \textit{Driving straight} all $\vec{v}$ equal
+    \item \textit{Turning} Wheel axis must intersect the \bi{Instant Centre of Rotation} (ICR),
+          speeds: $v_i \div R_i = \Omega$ ($R_i$ dist. wheel-ICR, $\Omega$, vehicle body rotation rate)
+\end{itemize}
+
+\bi{Maneuverability}
+\begin{itemize}
+    \item Deg. of Mobility: $\delta_m = 3 - $\#constrained directions
+    \item Deg. of Steerability: $\delta_s = $\#steerable wheels
+    \item Deg. of Maneuverability: $\delta_M = \delta_m + \delta_s$
+\end{itemize}
+
+\bi{Wheel Configurations}
+
+\includegraphics[width=1\columnwidth]{assets/wheel-config.png}
+
+\begin{scriptsize}
+    \begin{tabular}{llllll}
+        Bicyle         & Tricycle       & Ackermann      & Diff. Drive    & Two-Steer      & Three-Steer    \\
+        $\delta_m = 1$ & $\delta_m = 1$ & $\delta_m = 0$ & $\delta_m = 2$ & $\delta_m = 1$ & $\delta_m = 0$ \\
+        $\delta_s = 1$ & $\delta_s = 1$ & $\delta_s = 2$ & $\delta_s = 0$ & $\delta_s = 2$ & $\delta_s = 3$ \\
+        $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 2$ & $\delta_M = 3$ & $\delta_M = 3$ \\
+    \end{tabular}
+\end{scriptsize}
+
+\bi{Differential Drive Kinematics}
+
+\bi{State vec} $\vec{x} = [x_1, x_2, \theta]^\top$,
+\bi{Inputs} $\vec{u} = [\omega_l, \omega_r]^\top$, $r_r$ radius of right wheel, $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}$
+
+\textit{Straight}: $v = \omega_l r_l = \omega_r r_r$, $\Omega = 0$, $D = v\Delta t$.\\
+$\vec{b}_s = \begin{bmatrix}
+        D \cos(\theta) \\
+        D \sin(\theta) \\
+        0
+    \end{bmatrix}$
+$\vec{b}_t = \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\}$. ($\int \ldots \dx \Delta t$)