clean up doc

Author: AtsushiSakai

PathTracking/model_predictive_speed_and_steer_control/Model_predictive_speed_and_steering_control.ipynb

@@ -35,10 +35,16 @@
     "### MPC modeling\n",
     "\n",
     "State vector is:\n",
-    "$$ z = [x, y, v,\\phi]$$ x: x-position, y:y-position, v:velocity, φ: yaw angle\n",
+    "\n",
+    "$$ z = [x, y, v,\\phi]$$\n",
+    "\n",
+    "x: x-position, y:y-position, v:velocity, φ: yaw angle\n",
     "\n",
     "Input vector is:\n",
-    "$$ u = [a, \\delta]$$ a: accellation, δ: steering angle\n",
+    "\n",
+    "$$ u = [a, \\delta]$$\n",
+    "\n",
+    "a: accellation, δ: steering angle\n",
     "\n"
    ]
   },
@@ -103,9 +109,13 @@
     "\n",
     "\n",
     "Vehicle model is \n",
+    "\n",
     "$$ \\dot{x} = vcos(\\phi)$$\n",
+    "\n",
     "$$ \\dot{y} = vsin((\\phi)$$\n",
+    "\n",
     "$$ \\dot{v} = a$$\n",
+    "\n",
     "$$ \\dot{\\phi} = \\frac{vtan(\\delta)}{L}$$\n",
     "\n",
     "\n",
@@ -128,7 +138,7 @@
    "source": [
     "where\n",
     "\n",
-    "\\begin{equation*}\n",
+    "$\\begin{equation*}\n",
     "A' =\n",
     "\\begin{bmatrix}\n",
     "\\frac{\\partial }{\\partial x}vcos(\\phi) & \n",
@@ -156,15 +166,15 @@
     "0 & 0 & 0 & 0 \\\\\n",
     "0 & 0 &\\frac{tan(\\bar{\\delta})}{L} & 0 \\\\\n",
     "\\end{bmatrix}\n",
-    "\\end{equation*}\n",
+    "\\end{equation*}$\n",
     "\n"
    ]
   },
   {
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "\\begin{equation*}\n",
+    "$\\begin{equation*}\n",
     "B' =\n",
     "\\begin{bmatrix}\n",
     "\\frac{\\partial }{\\partial a}vcos(\\phi) &\n",
@@ -184,7 +194,7 @@
     "1 & 0 \\\\\n",
     "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
     "\\end{bmatrix}\n",
-    "\\end{equation*}\n",
+    "\\end{equation*}$\n",
     "\n"
    ]
   },
@@ -228,7 +238,7 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "\\begin{equation*}\n",
+    "$\\begin{equation*}\n",
     "B = dtB'\\\\\n",
     "=\n",
     "\\begin{bmatrix} \n",
@@ -237,14 +247,14 @@
     "dt & 0 \\\\\n",
     "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})}dt \\\\\n",
     "\\end{bmatrix}\n",
-    "\\end{equation*}"
+    "\\end{equation*}$"
    ]
   },
   {
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "\\begin{equation*}\n",
+    "$\\begin{equation*}\n",
     "C = (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt\\\\\n",
     "= dt(\n",
     "\\begin{bmatrix} \n",
@@ -275,7 +285,7 @@
     "0\\\\\n",
     "-\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
     "\\end{bmatrix}\n",
-    "\\end{equation*}"
+    "\\end{equation*}$"
    ]
   },
   {

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*}`
-
-: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*} 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*} 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*}`
-
-: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*} 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*} 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