clean up doc

docs/modules/Model_predictive_speed_and_steering_control.rst

@@ -29,13 +29,13 @@ State vector is:
 
 .. math::  z = [x, y, v,\phi]
 
-\ x: x-position, y:y-position, v:velocity, φ: yaw angle
+x: x-position, y:y-position, v:velocity, φ: yaw angle
 
 Input vector is:
 
 .. math::  u = [a, \delta]
 
-\ a: accellation, δ: steering angle
+a: accellation, δ: steering angle
 
 The MPC cotroller minimize this cost function for path tracking:
 
@@ -94,57 +94,9 @@ ODE is
 
 where
 
-:raw-latex:`\begin{equation*}
-A' =
-\begin{bmatrix}
-\frac{\partial }{\partial x}vcos(\phi) & 
-\frac{\partial }{\partial y}vcos(\phi) & 
-\frac{\partial }{\partial v}vcos(\phi) &
-\frac{\partial }{\partial \phi}vcos(\phi)\\
-\frac{\partial }{\partial x}vsin(\phi) & 
-\frac{\partial }{\partial y}vsin(\phi) & 
-\frac{\partial }{\partial v}vsin(\phi) &
-\frac{\partial }{\partial \phi}vsin(\phi)\\
-\frac{\partial }{\partial x}a& 
-\frac{\partial }{\partial y}a& 
-\frac{\partial }{\partial v}a&
-\frac{\partial }{\partial \phi}a\\
-\frac{\partial }{\partial x}\frac{vtan(\delta)}{L}& 
-\frac{\partial }{\partial y}\frac{vtan(\delta)}{L}& 
-\frac{\partial }{\partial v}\frac{vtan(\delta)}{L}&
-\frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\
-\end{bmatrix}
-\\
-　=
-\begin{bmatrix}
-0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\
-0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\
-0 & 0 & 0 & 0 \\
-0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\
-\end{bmatrix}
-\end{equation*}`
+:math:`\begin{equation*} A' = \begin{bmatrix} \frac{\partial }{\partial x}vcos(\phi) & \frac{\partial }{\partial y}vcos(\phi) & \frac{\partial }{\partial v}vcos(\phi) & \frac{\partial }{\partial \phi}vcos(\phi)\\ \frac{\partial }{\partial x}vsin(\phi) & \frac{\partial }{\partial y}vsin(\phi) & \frac{\partial }{\partial v}vsin(\phi) & \frac{\partial }{\partial \phi}vsin(\phi)\\ \frac{\partial }{\partial x}a& \frac{\partial }{\partial y}a& \frac{\partial }{\partial v}a& \frac{\partial }{\partial \phi}a\\ \frac{\partial }{\partial x}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial y}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial v}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ 　= \begin{bmatrix} 0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\ 0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\ 0 & 0 & 0 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\ \end{bmatrix} \end{equation*}`
 
-:raw-latex:`\begin{equation*}
-B' =
-\begin{bmatrix}
-\frac{\partial }{\partial a}vcos(\phi) &
-\frac{\partial }{\partial \delta}vcos(\phi)\\
-\frac{\partial }{\partial a}vsin(\phi) &
-\frac{\partial }{\partial \delta}vsin(\phi)\\
-\frac{\partial }{\partial a}a &
-\frac{\partial }{\partial \delta}a\\
-\frac{\partial }{\partial a}\frac{vtan(\delta)}{L} &
-\frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\
-\end{bmatrix}
-\\
-　=
-\begin{bmatrix}
-0 & 0 \\
-0 & 0 \\
-1 & 0 \\
-0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\
-\end{bmatrix}
-\end{equation*}`
+:math:`\begin{equation*} B' = \begin{bmatrix} \frac{\partial }{\partial a}vcos(\phi) & \frac{\partial }{\partial \delta}vcos(\phi)\\ \frac{\partial }{\partial a}vsin(\phi) & \frac{\partial }{\partial \delta}vsin(\phi)\\ \frac{\partial }{\partial a}a & \frac{\partial }{\partial \delta}a\\ \frac{\partial }{\partial a}\frac{vtan(\delta)}{L} & \frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ 　= \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\ \end{bmatrix} \end{equation*}`
 
 You can get a discrete-time mode with Forward Euler Discretization with
 sampling time dt.
@@ -165,49 +117,9 @@ where,
 
 :math:`\begin{equation*} A = (I + dtA')\\ = \begin{bmatrix} 1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\ 0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\ \end{bmatrix} \end{equation*}`
 
-:raw-latex:`\begin{equation*}
-B = dtB'\\
-=
-\begin{bmatrix} 
-0 & 0 \\
-0 & 0 \\
-dt & 0 \\
-0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\
-\end{bmatrix}
-\end{equation*}`
+:math:`\begin{equation*} B = dtB'\\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\ \end{bmatrix} \end{equation*}`
 
-:raw-latex:`\begin{equation*}
-C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\
-= dt(
-\begin{bmatrix} 
-\bar{v}cos(\bar{\phi})\\
-\bar{v}sin(\bar{\phi}) \\
-\bar{a}\\
-\frac{\bar{v}tan(\bar{\delta})}{L}\\
-\end{bmatrix}
--
-\begin{bmatrix} 
-\bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\
-\bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\
-0\\
-\frac{\bar{v}tan(\bar{\delta})}{L}\\
-\end{bmatrix}
--
-\begin{bmatrix} 
-0\\
-0 \\
-\bar{a}\\
-\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\
-\end{bmatrix}
-)\\
-=
-\begin{bmatrix} 
-\bar{v}sin(\bar{\phi})\bar{\phi}dt\\
--\bar{v}cos(\bar{\phi})\bar{\phi}dt\\
-0\\
--\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\
-\end{bmatrix}
-\end{equation*}`
+:math:`\begin{equation*} C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\ = dt( \begin{bmatrix} \bar{v}cos(\bar{\phi})\\ \bar{v}sin(\bar{\phi}) \\ \bar{a}\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} \bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\ \bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\ 0\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} 0\\ 0 \\ \bar{a}\\ \frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\ \end{bmatrix} )\\ = \begin{bmatrix} \bar{v}sin(\bar{\phi})\bar{\phi}dt\\ -\bar{v}cos(\bar{\phi})\bar{\phi}dt\\ 0\\ -\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\ \end{bmatrix} \end{equation*}`
 
 This equation is implemented at