release notebook

Author: AtsushiSakai

PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb

@@ -6,6 +6,8 @@
    "source": [
     "# Model predictive speed and steering control\n",
     "\n",
+    "code:\n",
+    "\n",
     "https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py\n",
     "\n",
     "This is a path tracking simulation using model predictive control (MPC)\n",
@@ -17,7 +19,52 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "# MPC modeling"
+    "# 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",
+    "Input vector is:\n",
+    "$$ u = [a, \\delta]$$ a: accellation, δ: steering angle\n",
+    "\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "The MPC cotroller minimize this cost function for path tracking:\n",
+    "\n",
+    "$$min\\ Q_f(z_{T,ref}-z_{T})^2+Q\\Sigma({z_{t,ref}-z_{t}})^2+R\\Sigma{u_t}^2+R_d\\Sigma({u_{t+1}-u_{t}})^2$$\n",
+    "\n",
+    "z_ref come from target path and speed."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "subject to:\n",
+    "- Linearlied vehicle model\n",
+    "$$z_{t+1}=Az_t+Bu+C$$\n",
+    "- Maximum steering speed\n",
+    "$$|u_{t+1}-u_{t}|<du_{max}$$\n",
+    "- Maximum steering angle\n",
+    "$$|u_{t}|<u_{max}$$\n",
+    "- Initial state\n",
+    "$$z_0 = z_{0,ob}$$\n",
+    "- Maximum and minimum speed\n",
+    "$$v_{min} < v_t < v_{max}$$\n",
+    "- Maximum and minimum input\n",
+    "$$u_{min} < u_t < u_{max}$$\n"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "This is implemented at \n",
+    "https://github.com/AtsushiSakai/PythonRobotics/blob/f51a73f47cb922a12659f8ce2d544c347a2a8156/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L247-L301"
    ]
   },
   {
@@ -26,11 +73,7 @@
    "source": [
     "# Vehicle model linearization\n",
     "\n",
-    "State vector is:\n",
-    "$$ z = [x, y, v,\\phi]$$ 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",
     "Vehicle model is \n",
     "$$ \\dot{x} = vcos(\\phi)$$\n",