update jupyter notebook

Author: AtsushiSakai

PathTracking/cgmres_nmpc/cgmres_nmpc.ipynb

@@ -121,17 +121,47 @@
    "source": [
     "### Mathematical Formulation\n",
     "\n",
-    "TBD\n",
-    "\n",
-    "$\\begin{equation*}\n",
-    "F=\n",
-    "\\begin{bmatrix}\n",
-    "1 & 0 & 0 & 0\\\\\n",
-    "0 & 1 & 0 & 0\\\\\n",
-    "0 & 0 & 1 & 0 \\\\\n",
-    "0 & 0 & 0 & 0 \\\\\n",
-    "\\end{bmatrix}\n",
-    "\\end{equation*}$"
+    "Motion model is\n",
+    "\n",
+    "$\\dot{x}=vcos\\theta$\n",
+    "\n",
+    "$\\dot{y}=vsin\\theta$\n",
+    "\n",
+    "$\\dot{\\theta}=\\frac{v}{WB}sin(u_{\\delta})$ (tan is not good for optimization)\n",
+    "\n",
+    "$\\dot{v}=u_a$\n",
+    "\n",
+    "Cost function is \n",
+    "\n",
+    "$J=\\frac{1}{2}(u_a^2+u_{\\delta}^2)-\\phi_a d_a-\\phi_\\delta d_\\delta$\n",
+    "\n",
+    "Input constraints are\n",
+    "\n",
+    "$|u_a| \\leq u_{a,max}$\n",
+    "\n",
+    "$|u_\\delta| \\leq u_{\\delta,max}$\n",
+    "\n",
+    "So, Hamiltonian　is\n",
+    "\n",
+    "$J=\\frac{1}{2}(u_a^2+u_{\\delta}^2)-\\phi_a d_a-\\phi_\\delta d_\\delta\\\\ +\\lambda_1vcos\\theta+\\lambda_2vsin\\theta+\\lambda_3\\frac{v}{WB}sin(u_{\\delta})+\\lambda_4u_a\\\\ \n",
+    "+\\rho_1(u_a^2+d_a^2+u_{a,max}^2)+\\rho_2(u_\\delta^2+d_\\delta^2+u_{\\delta,max}^2)$\n",
+    "\n",
+    "Partial differential equations of the Hamiltonian are:\n",
+    "\n",
+    "$\\frac{\\partial H}{\\partial \\bf{x}}=\\\\ \n",
+    "[\\frac{\\partial H}{\\partial x}= 0,\\\\\n",
+    "\\frac{\\partial H}{\\partial y}= 0,\\\\\n",
+    "\\frac{\\partial H}{\\partial \\theta}= -\\lambda_1vsin\\theta+\\lambda_2vcos\\theta,\\\\\n",
+    "\\frac{\\partial H}{\\partial v}=-\\lambda_1cos\\theta+\\lambda_2sin\\theta+\\lambda_3\\frac{1}{WB}sin(u_{\\delta})]\\\\\n",
+    "$\n",
+    "\n",
+    "\n",
+    "$\\frac{\\partial H}{\\partial \\bf{u}}=\\\\ \n",
+    "[\\frac{\\partial H}{\\partial u_a}= u_a+\\lambda_4+2\\rho_1u_a,\\\\\n",
+    "\\frac{\\partial H}{\\partial u_\\delta}= u_\\delta+\\lambda_3\\frac{v}{WB}cos(u_{\\delta})+2\\rho_2u_\\delta\\\\\n",
+    "\\frac{\\partial H}{\\partial d_a}= -\\phi_a+2\\rho_1d_a,\\\\\n",
+    "\\frac{\\partial H}{\\partial d_\\delta}=-\\phi_\\delta+2\\rho_2d_\\delta]\\\\\n",
+    "$"
    ]
   },
   {