From 754aec51a86deb953ca577a3018b97379fb3c56e Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Tue, 17 Jul 2018 15:00:48 +0900
Subject: [PATCH] finish vehicle model linealization documentation

---
 .../notebook.ipynb                            | 102 +++++++++++++++++-
 1 file changed, 98 insertions(+), 4 deletions(-)

diff --git a/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb b/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
index 43c9c5e7..6226afdd 100644
--- a/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
+++ b/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
@@ -42,7 +42,7 @@
    "source": [
     "ODE is\n",
     "\n",
-    "$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = A'z+B'u$$\n",
+    "$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = f(z, u) = A'z+B'u$$\n",
     "\n"
    ]
   },
@@ -76,7 +76,7 @@
     "　=\n",
     "\\begin{bmatrix}\n",
     "0 & 0 & cos(\\bar{\\phi}) & -\\bar{v}sin(\\bar{\\phi})\\\\\n",
-    "0 & 1 & sin(\\bar{\\phi}) & \\bar{v}cos(\\bar{\\phi}) \\\\\n",
+    "0 & 0 & sin(\\bar{\\phi}) & \\bar{v}cos(\\bar{\\phi}) \\\\\n",
     "0 & 0 & 0 & 0 \\\\\n",
     "0 & 0 &\\frac{tan(\\bar{\\delta})}{L} & 0 \\\\\n",
     "\\end{bmatrix}\n",
@@ -109,10 +109,104 @@
     "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
     "\\end{bmatrix}\n",
     "\\end{equation*}\n",
-    "\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
     "You can get a discrete-time mode with Forward Euler Discretization with sampling time dt.\n",
     "\n",
-    "$$z_{k+1}=z_k+f(z_k,u_k)dt$$"
+    "$$z_{k+1}=z_k+f(z_k,u_k)dt$$\n",
+    "\n",
+    "Using first degree Tayer expantion around zbar and ubar\n",
+    "$$z_{k+1}=z_k+(f(\\bar{z},\\bar{u})+A'z_k+B'u_k)dt$$\n",
+    "\n",
+    "$$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\\bar{z},\\bar{u})-A'\\bar{z}+B'\\bar{u})dt$$\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "So, \n",
+    "\n",
+    "$$z_{k+1}=Az_k+Bu_k +C$$\n",
+    "\n",
+    "where,\n",
+    "\n",
+    "\\begin{equation*}\n",
+    "A = (I + dtA')\\\\\n",
+    "=\n",
+    "\\begin{bmatrix} \n",
+    "1 & 0 & cos(\\bar{\\phi})dt & -\\bar{v}sin(\\bar{\\phi})dt\\\\\n",
+    "0 & 1 & sin(\\bar{\\phi})dt & \\bar{v}cos(\\bar{\\phi})dt \\\\\n",
+    "0 & 0 & 1 & 0 \\\\\n",
+    "0 & 0 &\\frac{tan(\\bar{\\delta})}{L}dt & 1 \\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "\\begin{equation*}\n",
+    "B = dtB'\\\\\n",
+    "=\n",
+    "\\begin{bmatrix} \n",
+    "0 & 0 \\\\\n",
+    "0 & 0 \\\\\n",
+    "dt & 0 \\\\\n",
+    "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})}dt \\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "\\begin{equation*}\n",
+    "C = (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt\\\\\n",
+    "= dt(\n",
+    "\\begin{bmatrix} \n",
+    "\\bar{v}cos(\\bar{\\phi})\\\\\n",
+    "\\bar{v}sin(\\bar{\\phi}) \\\\\n",
+    "\\bar{a}\\\\\n",
+    "\\frac{\\bar{v}tan(\\bar{\\delta})}{L}\\\\\n",
+    "\\end{bmatrix}\n",
+    "-\n",
+    "\\begin{bmatrix} \n",
+    "\\bar{v}cos(\\bar{\\phi})-\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}\\\\\n",
+    "\\bar{v}sin(\\bar{\\phi})+\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}\\\\\n",
+    "0\\\\\n",
+    "\\frac{\\bar{v}tan(\\bar{\\delta})}{L}\\\\\n",
+    "\\end{bmatrix}\n",
+    "-\n",
+    "\\begin{bmatrix} \n",
+    "0\\\\\n",
+    "0 \\\\\n",
+    "\\bar{a}\\\\\n",
+    "\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}\\\\\n",
+    "\\end{bmatrix}\n",
+    ")\\\\\n",
+    "=\n",
+    "\\begin{bmatrix} \n",
+    "\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
+    "-\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
+    "0\\\\\n",
+    "\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
+    "\\end{bmatrix}\n",
+    "\\end{equation*}"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "This equation is implemented at https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L80-L102"
    ]
   },
   {