{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Nonlinear Model Predictive Control with C-GMRES"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 25440 characters]\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 10,
     "metadata": {
      "image/png": {
       "width": 600
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from IPython.display import Image\n",
    "Image(filename=\"Figure_4.png\",width=600)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 45100 characters]\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 9,
     "metadata": {
      "image/png": {
       "width": 600
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Image(filename=\"Figure_1.png\",width=600)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 41256 characters]\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 8,
     "metadata": {
      "image/png": {
       "width": 600
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Image(filename=\"Figure_2.png\",width=600)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 33696 characters]\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 7,
     "metadata": {
      "image/png": {
       "width": 600
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Image(filename=\"Figure_3.png\",width=600)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![gif](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Mathematical Formulation\n",
    "\n",
    "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",
    "$\\begin{equation*}\n",
    "\\frac{\\partial H}{\\partial \\bf{x}}=\\\\ \n",
    "\\begin{bmatrix}\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",
    "\\end{bmatrix}\n",
    "\\\\\n",
    "\\end{equation*}$\n",
    "\n",
    "\n",
    "$\\begin{equation*}\n",
    "\\frac{\\partial H}{\\partial \\bf{u}}=\\\\ \n",
    "\\begin{bmatrix}\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",
    "\\end{bmatrix}\n",
    "\\\\\n",
    "\\end{equation*}$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Ref\n",
    "\n",
    "- [Shunichi09/nonlinear\\_control: Implementing the nonlinear model predictive control, sliding mode control](https://github.com/Shunichi09/nonlinear_control)\n",
    "\n",
    "- [非線形モデル予測制御におけるCGMRES法をpythonで実装する \\- Qiita](https://qiita.com/MENDY/items/4108190a579395053924)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
