{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# STATE SPACE MODEL MATRICES\n",
    "\n",
    "### Diff drive"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "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": 2,
     "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],\n",
    "               [ sp.sin(theta)*v],\n",
    "               [w],\n",
    "               [-w],\n",
    "               [ 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": 3,
   "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": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "state = sp.Matrix([v,w])\n",
    "\n",
    "#B\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "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": 4,
     "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([[x + sp.cos(theta)*v*dt],\n",
    "               [y+ sp.sin(theta)*v*dt],\n",
    "               [theta + w*dt]])\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": 5,
   "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": 5,
     "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"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "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],\n",
    "               [ sp.sin(theta)*v],\n",
    "               [a],\n",
    "               [ 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)#.subs({x:0,y:0,theta:0})B="
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "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]])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "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]])"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#A LIN\n",
    "DT = sp.symbols(\"dt\")\n",
    "sp.eye(4)+A*DT"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "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]])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "B*DT"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "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]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "DT*(gs - A*X - B*U)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PATH WAYPOINTS AS PARAMETRIZED CURVE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.interpolate import interp1d\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
    "    final_xp=[]\n",
    "    final_yp=[]\n",
    "    delta = step #[m]\n",
    "\n",
    "    for idx in range(len(start_xp)-1):\n",
    "        section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
    "\n",
    "        interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
    "        \n",
    "        fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
    "        fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
    "        \n",
    "        final_xp=np.append(final_xp,fx(interp_range))\n",
    "        final_yp=np.append(final_yp,fy(interp_range))\n",
    "\n",
    "    return np.vstack((final_xp,final_yp))\n",
    "\n",
    "def get_nn_idx(state,path):\n",
    "\n",
    "    dx = state[0]-path[0,:]\n",
    "    dy = state[1]-path[1,:]\n",
    "    dist = np.sqrt(dx**2 + dy**2)\n",
    "    nn_idx = np.argmin(dist)\n",
    "\n",
    "    try:\n",
    "        v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
    "             path[1,nn_idx+1] - path[1,nn_idx]]   \n",
    "        v /= np.linalg.norm(v)\n",
    "\n",
    "        d = [path[0,nn_idx] - state[0],\n",
    "             path[1,nn_idx] - state[1]]\n",
    "\n",
    "        if np.dot(d,v) > 0:\n",
    "            target_idx = nn_idx\n",
    "        else:\n",
    "            target_idx = nn_idx+1\n",
    "\n",
    "    except IndexError as e:\n",
    "        target_idx = nn_idx\n",
    "\n",
    "    return target_idx"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#define track\n",
    "wp=np.array([0,5,6,10,11,15, 0,0,2,2,0,4]).reshape(2,-1)\n",
    "track = compute_path_from_wp(wp[0,:],wp[1,:],step=0.5)\n",
    "\n",
    "#vehicle state\n",
    "state=[3.5,0.5,np.radians(30)]\n",
    "\n",
    "#given vehicle pos find lookahead waypoints\n",
    "nn_idx=get_nn_idx(state,track)-1 #index ox closest wp, take the previous to have a straighter line\n",
    "LOOKAHED=6\n",
    "lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
    "\n",
    "#trasform lookahead waypoints to vehicle ref frame\n",
    "dx = lk_wp[0,:] - state[0]\n",
    "dy = lk_wp[1,:] - state[1]\n",
    "\n",
    "wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
    "                               dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
    "\n",
    "#fit poly\n",
    "coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
    "\n",
    "#def f(x,coeff):\n",
    "#    return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
    "def f(x,coeff):\n",
    "    return  coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "coeff"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "x=np.arange(-1,2,0.001) #interp range of curve \n",
    "\n",
    "# VEHICLE REF FRAME\n",
    "plt.subplot(2,1,1)\n",
    "plt.title('parametrized curve, vehicle ref frame')\n",
    "plt.scatter(0,0)\n",
    "plt.scatter(wp_vehicle_frame[0,:],wp_vehicle_frame[1,:])\n",
    "plt.plot(x,[f(xs,coeff) for xs in x])\n",
    "plt.axis('equal')\n",
    "\n",
    "# MAP REF FRAME\n",
    "plt.subplot(2,1,2)\n",
    "plt.title('waypoints, map ref frame')\n",
    "plt.scatter(state[0],state[1])\n",
    "plt.scatter(track[0,:],track[1,:])\n",
    "plt.scatter(track[0,nn_idx:nn_idx+LOOKAHED],track[1,nn_idx:nn_idx+LOOKAHED])\n",
    "plt.axis('equal')\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()\n",
    "#plt.savefig(\"fitted_poly\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# COMPUTE ERRORS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **crosstrack error** cte -> desired y-position - y-position of vehicle: this is the value of the fitted polynomial (road curve)\n",
    " \n",
    "$\n",
    "f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
    "$\n",
    "\n",
    "Then for the origin cte = K_3\n",
    "  \n",
    "* **heading error** epsi ->  desired heading - heading of vehicle : is the inclination of  tangent to the  fitted polynomial (road curve)\n",
    "\n",
    "The derivative of the fitted poly has the form\n",
    "\n",
    "$\n",
    "f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
    "$\n",
    "\n",
    "Then for the origin the equation of the tangent in the origin is $y=k2$   \n",
    "\n",
    "epsi = -atan(K_2)\n",
    "\n",
    "in general:\n",
    "\n",
    "$\n",
    "y_{desired} = f(px) \\\\\n",
    "heading_{desired} = -atan(f`(px))\n",
    "$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#for 0\n",
    "\n",
    "cte=coeff[3]\n",
    "epsi=-np.arctan(coeff[2])\n",
    "print(cte)\n",
    "print(np.degrees(epsi))"
   ]
  },
  {
   "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.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}