[AMR] Add gauss newton from NumCS summary

electives/amr/parts/02_Sensors-Actuators/04_depth-range.tex

@@ -7,8 +7,10 @@ Typ. derive D.M. with sensor dir param (as vec line eq)
 
 
 \subsubsection{Classic Stereo}
-Both images: same plane, focal length, centre, $x$-axis. Given corresponding pixels $[u_l, v]$ and $[u_r, v]$,
-$z = \frac{b \cdot f}{u_r - u_l}$ with $u_l = f\cdot \frac{x}{z} + c_u$ and $u_r = f \cdot \frac{x - b}{z} + c_u$
+Both images: same plane, focal length $f$, centre, $x$-axis. Given corresponding pixels $[u_l, v]$ and $[u_r, v]$: disparity (pixel offset) $d = u_r - u_l$;
+$z = \frac{b \cdot f}{d}$, then projected into left (right) cam, $u_l = f\cdot \frac{x}{z} + c_u$ or $u_r = f \cdot \frac{x - b}{z} + c_u$, $b$ distance between cameras.
+Then often apply pinhole camera projection (to e.g. get 3D point coords, comp ${_C}\vec{r}$, then point $z \cdot {_C}\vec{r}$).
+Uncertainty introduced with 1.-order error propag. $\Delta z = \pardiff{d}z \Delta d$, with $\Delta d$ error of $d$
 
 
 \subsubsection{Time of Flight, Projection}
@@ -31,6 +33,7 @@ $z = \frac{b \cdot f}{u_r - u_l}$ with $u_l = f\cdot \frac{x}{z} + c_u$ and $u_r
                 d \vec{x'} \\d
             \end{bmatrix}
         }$
+This $x'$ is obtained from pinhole cam projection.
 
 
 \subsubsection{Range Sensors}

electives/amr/parts/03_multi-sensor-estimation/02_nonlinear-least-squares.tex

@@ -1,9 +1,25 @@
 \subsection{Non-Linear Least Squares}
 Find $\vec{x}^* = \text{argmax}\; \P(\vec{x} | \vec{z}) = \argmin{}(-\log(\P(\vec{x}|\vec{z})))$
 
-\bi{Gauss-Newton} % TODO: Do we really need these? If so, use from NumCS (much simpler notation)
+\bi{Gauss-Newton} Need func $F$ and its Jacobian (or derivative) $D$
+\rmvspace
 % TODO: Error propagation laws
+\begin{minted}[
+    breaklines,
+    breakindentnchars=2,
+]{python}
+def gauss_newton(x: np.ndarray, F, DF, tol=1e-6):
+    s = np.linalg.lstsq(DF(x), F(x))[0] # least sq.
+    x = x-s; k = 1
+    while np.linalg.norm(s) > tol * np.linalg.norm(x):
+        s = np.linalg.lstsq(DF(x), F(x))[0]
+        x = x-s; k += 1 # k opt for max iter
+    return x, k
+\end{minted}
+\rmvspace
 
 \bi{Levenberg-Marquardt}
+Minimize $||F(x^{(k)}) + DF(x^{(k)})|| + \lambda||s||$.
+% TODO: Do we need these?
 
 \bi{Local Param.}

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}
     \begin{algorithmic}[1]