[AMR] Fix errors pointed out to me

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)}

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}$
 }

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}.
 

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
 % \[

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})$
 

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

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.
 

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$

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)

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

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),
+\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$, $\lambda^{k + 1} = \lambda^k v$, go to step 3;
+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

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]$

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}$.

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}$)

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)$.
+\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 L}{\partial y^2} \right)$
+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)$
 

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$

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.

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)

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}
 

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}

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}$.

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} \\
+        \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}
+        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}

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})$.