reorganizing my jupyter notebooks

notebooks/1.1-symbolic-system-equations.ipynb

@@ -0,0 +1,344 @@
+{
+ "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],\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": 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([[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": 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"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 5,
+   "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": 6,
+   "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": 6,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "B"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 7,
+   "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": 7,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "#A LIN\n",
+    "DT = sp.symbols(\"dt\")\n",
+    "sp.eye(4)+A*DT"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 8,
+   "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": 8,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "B*DT"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 9,
+   "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": 9,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "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
+}

notebooks/1.2-parametrized-path-curves.ipynb

@@ -1,293 +1,5 @@
 {
  "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],\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": 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([[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": 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"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 5,
-   "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": 6,
-   "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": 6,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "B"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 7,
-   "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": 7,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "#A LIN\n",
-    "DT = sp.symbols(\"dt\")\n",
-    "sp.eye(4)+A*DT"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 8,
-   "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": 8,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "B*DT"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 9,
-   "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": 9,
-     "metadata": {},
-     "output_type": "execute_result"
-    }
-   ],
-   "source": [
-    "DT*(gs - A*X - B*U)"
-   ]
-  },
   {
    "cell_type": "markdown",
    "metadata": {},
@@ -471,70 +183,6 @@
     "#plt.savefig(\"fitted_poly\")"
    ]
   },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "## With SPLINES"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": 17,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "(array([-0.39433757, -0.39433757, -0.39433757, -0.39433757,  0.56791288,\n",
-      "        1.04903811,  1.67104657,  1.67104657,  1.67104657,  1.67104657]), array([-0.34967937,  0.15467936, -2.19173016,  1.11089663, -8.        ,\n",
-      "       -0.7723291 ,  0.        ,  0.        ,  0.        ,  0.        ]), 3)\n",
-      "[[  4.64353595   4.64353595   4.64353595   4.64353595 -23.21767974\n",
-      "   65.74806776  65.74806776  65.74806776  65.74806776]\n",
-      " [ -6.70236682  -6.70236682  -6.70236682  -6.70236682   6.70236682\n",
-      "  -26.8094673   95.8780974   95.8780974   95.8780974 ]\n",
-      " [  1.57243489   1.57243489   1.57243489   1.57243489   1.57243489\n",
-      "   -8.10159833  34.85967446  34.85967446  34.85967446]\n",
-      " [ -0.34967937  -0.34967937  -0.34967937  -0.34967937  -0.90523492\n",
-      "   -1.1830127   -0.7723291   -0.7723291   -0.7723291 ]]\n"
-     ]
-    }
-   ],
-   "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",
-    "import scipy\n",
-    "from scipy.interpolate import CubicSpline\n",
-    "from scipy.interpolate import PPoly,splrep\n",
-    "spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
-    "print( spl)\n",
-    "print(PPoly.from_spline(spl).c)\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",
-    "\n"
-   ]
-  },
   {
    "cell_type": "code",
    "execution_count": null,
@@ -558,108 +206,13 @@
     "    a = np.linalg.lstsq(C,bc)[0]\n",
     "    return a"
    ]
-  },
-  {
-   "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": 15,
-   "metadata": {},
-   "outputs": [
-    {
-     "name": "stdout",
-     "output_type": "stream",
-     "text": [
-      "-0.5808399313875324\n",
-      "28.307545725691345\n"
-     ]
-    }
-   ],
-   "source": [
-    "#for 0\n",
-    "\n",
-    "# cte=coeff[3]\n",
-    "# epsi=-np.arctan(coeff[2])\n",
-    "cte=f(0,coeff)\n",
-    "epsi=-np.arctan(df(0,coeff))\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",
+   "display_name": "Python [conda env:.conda-jupyter] *",
    "language": "python",
-   "name": "python3"
+   "name": "conda-env-.conda-jupyter-py"
   },
   "language_info": {
    "codemirror_mode": {
@@ -671,7 +224,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.7.6"
+   "version": "3.8.5"
   }
  },
  "nbformat": 4,

notebooks/cte_based_formulation/MPC_racecar_crosstrack.ipynb

@@ -564,15 +564,6 @@
     "The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
    ]
   },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "![mpc](img/mpc_block_diagram.png)\n",
-    "\n",
-    "![mpc](img/mpc_t.png)"
-   ]
-  },
   {
    "cell_type": "markdown",
    "metadata": {},