{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# STATE SPACE MODEL MATRICES\n",
    "\n",
    "### Diff drive"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0, -v*sin(theta),          0, 0],\n",
       "[0, 0,  v*cos(theta),          0, 0],\n",
       "[0, 0,             0,          0, 0],\n",
       "[0, 0,             0,          0, 0],\n",
       "[0, 0,             0, v*cos(psi), 0]])"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import sympy as sp\n",
    "\n",
    "x, y, theta, psi, cte, v, w = sp.symbols(\"x y theta psi cte v w\")\n",
    "\n",
    "gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [w], [-w], [v * sp.sin(psi)]])\n",
    "\n",
    "state = sp.Matrix([x, y, theta, psi, cte])\n",
    "\n",
    "# A\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(theta),  0],\n",
       "[sin(theta),  0],\n",
       "[         0,  1],\n",
       "[         0, -1],\n",
       "[  sin(psi),  0]])"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "state = sp.Matrix([v, w])\n",
    "\n",
    "# B\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, -dt*v*sin(theta)],\n",
       "[0, 1,  dt*v*cos(theta)],\n",
       "[0, 0,                1]])"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import sympy as sp\n",
    "\n",
    "x, y, theta, psi, cte, v, w, dt = sp.symbols(\"x y theta psi cte v w dt\")\n",
    "\n",
    "gs = sp.Matrix(\n",
    "    [[x + sp.cos(theta) * v * dt], [y + sp.sin(theta) * v * dt], [theta + w * dt]]\n",
    ")\n",
    "\n",
    "state = sp.Matrix([x, y, theta])\n",
    "\n",
    "# A\n",
    "gs.jacobian(state)  # .subs({x:0,y:0,theta:0})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}dt \\cos{\\left(\\theta \\right)} & 0\\\\dt \\sin{\\left(\\theta \\right)} & 0\\\\0 & dt\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[dt*cos(theta),  0],\n",
       "[dt*sin(theta),  0],\n",
       "[            0, dt]])"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "state = sp.Matrix([v, w])\n",
    "\n",
    "# B\n",
    "gs.jacobian(state)  # .subs({x:0,y:0,theta:0})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Ackermann Kinematics model\n",
    "\n",
    "### Jacobians"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0,   cos(theta), -v*sin(theta)],\n",
       "[0, 0,   sin(theta),  v*cos(theta)],\n",
       "[0, 0,            0,             0],\n",
       "[0, 0, tan(delta)/L,             0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0,                       0],\n",
       "[0,                       0],\n",
       "[1,                       0],\n",
       "[0, v*(tan(delta)**2 + 1)/L]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "x, y, theta, v, delta, L, a = sp.symbols(\"x y theta v delta L a\")\n",
    "\n",
    "gs = sp.Matrix([[sp.cos(theta) * v], [sp.sin(theta) * v], [a], [v * sp.tan(delta) / L]])\n",
    "\n",
    "X = sp.Matrix([x, y, v, theta])\n",
    "\n",
    "# A\n",
    "A = gs.jacobian(X)\n",
    "\n",
    "U = sp.Matrix([a, delta])\n",
    "\n",
    "# B\n",
    "B = gs.jacobian(U)\n",
    "display(A)\n",
    "display(B)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Discretized and Linearized model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & dt \\cos{\\left(\\theta \\right)} & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt \\sin{\\left(\\theta \\right)} & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & \\frac{dt \\tan{\\left(\\delta \\right)}}{L} & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0,   dt*cos(theta), -dt*v*sin(theta)],\n",
       "[0, 1,   dt*sin(theta),  dt*v*cos(theta)],\n",
       "[0, 0,               1,                0],\n",
       "[0, 0, dt*tan(delta)/L,                1]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\dt & 0\\\\0 & \\frac{dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0,                          0],\n",
       "[ 0,                          0],\n",
       "[dt,                          0],\n",
       "[ 0, dt*v*(tan(delta)**2 + 1)/L]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}dt \\theta v \\sin{\\left(\\theta \\right)}\\\\- dt \\theta v \\cos{\\left(\\theta \\right)}\\\\0\\\\- \\frac{\\delta dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[            dt*theta*v*sin(theta)],\n",
       "[           -dt*theta*v*cos(theta)],\n",
       "[                                0],\n",
       "[-delta*dt*v*(tan(delta)**2 + 1)/L]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "DT = sp.symbols(\"dt\")\n",
    "\n",
    "display(sp.eye(4) + A * DT)\n",
    "display(B * DT)\n",
    "display(DT * (gs - A * X - B * U))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ADD DELAY (for real time implementation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is necessary to take *actuation latency* into account: so instead of using the actual state as estimated, the delay factored in using the kinematic model\n",
    "\n",
    "Starting State is :\n",
    "\n",
    "* $x_{delay} = 0.0 + v * dt$\n",
    "* $y_{delay} = 0.0$\n",
    "* $psi_{delay} = 0.0 + w * dt$\n",
    "* $cte_{delay} = cte + v * sin(epsi) * dt$\n",
    "* $epsi_{delay} = epsi - w * dt$\n",
    "\n",
    "Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}