[AMR] Finish first rework

electives/amr/parts/05_planning-control/00_feedback-control/02_lqr.tex

@@ -7,7 +7,7 @@ We try to minimise cost functional (for $\vec{u}(t) = -\mat{K}\vec{x}(t)$)
 \]
 Solution: $\mat{K} = \mat{R}^{-1} (\mat{G}_c^\top \mat{P})$, with $\mat{P}$ found from
 \[
-    \mat{F}_c^\top \mat{P} + \mat{PF}_c - (\mat{PG}_c) \mat{R}^{-1} + \mat{Q} = \mat{0}
+    \mat{F}_c^\top \mat{P} + \mat{PF}_c - (\mat{PG}_c) \mat{R}^{-1} (\mat{G}_c^\top \mat{P}) + \mat{Q} = \mat{0}
 \]
 Finding $\mat{K}$ is expensive, but \textit{offline}, at runtime only $\vec{u}(t)$.
 

electives/amr/parts/05_planning-control/00_feedback-control/03_mpc.tex

@@ -1,7 +1,7 @@
 \subsection{MPC}
 \bi{Cost function} ($p(\vec{x}_N)$ \textit{terminal cost}, sum the \textit{stage cost})
 \[
-    J_{0 \rightarrow N}(\vec{x}_0, \vec{u}_0, \ldots, \vec{u_{N - 1}}) = p(\vec{x}_N) + \sum_{k = 0}^{N - 1} q(\vec{x}_k, \vec{u}_k)
+    J_{0 \rightarrow N}(\vec{x}_0, \vec{u}_0, \ldots, \vec{u}_{N - 1}) = p(\vec{x}_N) + \sum_{k = 0}^{N - 1} q(\vec{x}_k, \vec{u}_k)
 \]
 We minimize the above s.t. for $k \in \{ 0, \ldots, N - 1 \}$ we have $\vec{x}_{k + 1} = \vec{f}(\vec{x}_k, \vec{u}_k)$, $\vec{g}(\vec{x}_k, \vec{u}_k) \leq \vec{0}$
 and $\vec{x}_N \in \cX_f$ and $\vec{x}_0 = \vec{x}(0)$
@@ -13,4 +13,4 @@ Quad. Cost:
     \vec{x}_N^\top \vec{P} \vec{x}_N + \sum_{k = 0}^{N - 1} \vec{x}_k^\top \mat{Q} \vec{x}_k + \vec{u}_k^\top \mat{R} \vec{u}_k
 \]
 without constraints, \bi{State Feedback Law} $\vec{u}^*_0 = -\mat{K}\vec{x}(0)$.
-With constraints, minimize as above.
+With constraints, minimize as above, $\vec{x}_{k + 1} = \mat{F} \vec{x}_k + \mat{G} \vec{u}_k$