cleaned up modelling notebooks, added notebook from autoware.ai

notebooks/models/ackerman_model.ipynb

@@ -0,0 +1,193 @@
+{
+ "cells": [
+  {
+   "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],\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)\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.6"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}

notebooks/models/numerical_jacobian.ipynb

@@ -152,7 +152,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.8.5"
+   "version": "3.8.6"
   }
  },
  "nbformat": 4,