{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Model predictive speed and steering control\n",
    "\n",
    "![Model predictive speed and steering control](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif?raw=true)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "\n",
    "code:\n",
    "\n",
    "[PythonRobotics/model\\_predictive\\_speed\\_and\\_steer\\_control\\.py at master · AtsushiSakai/PythonRobotics](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",
    "\n",
    "The MPC controller controls vehicle speed and steering base on linealized model.\n",
    "\n",
    "This code uses cvxpy as an optimization modeling tool.\n",
    "\n",
    "- [Welcome to CVXPY 1\\.0 — CVXPY 1\\.0\\.6 documentation](http://www.cvxpy.org/)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### MPC modeling\n",
    "\n",
    "State vector is:\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",
    "\n",
    "$$ u = [a, \\delta]$$\n",
    "\n",
    "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",
    "\n",
    "- Linearlied vehicle model\n",
    "\n",
    "$$z_{t+1}=Az_t+Bu+C$$\n",
    "\n",
    "- Maximum steering speed\n",
    "\n",
    "$$|u_{t+1}-u_{t}|<du_{max}$$\n",
    "\n",
    "- Maximum steering angle\n",
    "\n",
    "$$|u_{t}|<u_{max}$$\n",
    "\n",
    "- Initial state\n",
    "\n",
    "$$z_0 = z_{0,ob}$$\n",
    "\n",
    "- Maximum and minimum speed\n",
    "\n",
    "$$v_{min} < v_t < v_{max}$$\n",
    "\n",
    "- Maximum and minimum input\n",
    "\n",
    "$$u_{min} < u_t < u_{max}$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This is implemented at \n",
    "\n",
    "[PythonRobotics/model\\_predictive\\_speed\\_and\\_steer\\_control\\.py at f51a73f47cb922a12659f8ce2d544c347a2a8156 · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/f51a73f47cb922a12659f8ce2d544c347a2a8156/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L247-L301)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Vehicle model linearization\n",
    "\n",
    "\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",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "ODE is\n",
    "\n",
    "$$ \\dot{z} =\\frac{\\partial }{\\partial z} z = f(z, u) = A'z+B'u$$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where\n",
    "\n",
    "$\\begin{equation*}\n",
    "A' =\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial }{\\partial x}vcos(\\phi) & \n",
    "\\frac{\\partial }{\\partial y}vcos(\\phi) & \n",
    "\\frac{\\partial }{\\partial v}vcos(\\phi) &\n",
    "\\frac{\\partial }{\\partial \\phi}vcos(\\phi)\\\\\n",
    "\\frac{\\partial }{\\partial x}vsin(\\phi) & \n",
    "\\frac{\\partial }{\\partial y}vsin(\\phi) & \n",
    "\\frac{\\partial }{\\partial v}vsin(\\phi) &\n",
    "\\frac{\\partial }{\\partial \\phi}vsin(\\phi)\\\\\n",
    "\\frac{\\partial }{\\partial x}a& \n",
    "\\frac{\\partial }{\\partial y}a& \n",
    "\\frac{\\partial }{\\partial v}a&\n",
    "\\frac{\\partial }{\\partial \\phi}a\\\\\n",
    "\\frac{\\partial }{\\partial x}\\frac{vtan(\\delta)}{L}& \n",
    "\\frac{\\partial }{\\partial y}\\frac{vtan(\\delta)}{L}& \n",
    "\\frac{\\partial }{\\partial v}\\frac{vtan(\\delta)}{L}&\n",
    "\\frac{\\partial }{\\partial \\phi}\\frac{vtan(\\delta)}{L}\\\\\n",
    "\\end{bmatrix}\n",
    "\\\\\n",
    "　=\n",
    "\\begin{bmatrix}\n",
    "0 & 0 & cos(\\bar{\\phi}) & -\\bar{v}sin(\\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",
    "\\end{equation*}$\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$\\begin{equation*}\n",
    "B' =\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial }{\\partial a}vcos(\\phi) &\n",
    "\\frac{\\partial }{\\partial \\delta}vcos(\\phi)\\\\\n",
    "\\frac{\\partial }{\\partial a}vsin(\\phi) &\n",
    "\\frac{\\partial }{\\partial \\delta}vsin(\\phi)\\\\\n",
    "\\frac{\\partial }{\\partial a}a &\n",
    "\\frac{\\partial }{\\partial \\delta}a\\\\\n",
    "\\frac{\\partial }{\\partial a}\\frac{vtan(\\delta)}{L} &\n",
    "\\frac{\\partial }{\\partial \\delta}\\frac{vtan(\\delta)}{L}\\\\\n",
    "\\end{bmatrix}\n",
    "\\\\\n",
    "　=\n",
    "\\begin{bmatrix}\n",
    "0 & 0 \\\\\n",
    "0 & 0 \\\\\n",
    "1 & 0 \\\\\n",
    "0 & \\frac{\\bar{v}}{Lcos^2(\\bar{\\delta})} \\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\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$$\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-A'\\bar{z}-B'\\bar{u})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 \n",
    "\n",
    "[PythonRobotics/model\\_predictive\\_speed\\_and\\_steer\\_control\\.py at eb6d1cbe6fc90c7be9210bf153b3a04f177cc138 · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py#L80-L102)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Reference\n",
    "\n",
    "- [Vehicle Dynamics and Control \\| Rajesh Rajamani \\| Springer](http://www.springer.com/us/book/9781461414322)\n",
    "\n",
    "- [MPC Course Material \\- MPC Lab @ UC\\-Berkeley](http://www.mpc.berkeley.edu/mpc-course-material)\n"
   ]
  }
 ],
 "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
}
