Fixing notebook theory

notebook/equations.ipynb

@@ -9,7 +9,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 9,
+   "execution_count": 1,
    "metadata": {},
    "outputs": [
     {
@@ -26,7 +26,7 @@
        "[0, 0,             0, v*cos(psi), 0]])"
       ]
      },
-     "execution_count": 9,
+     "execution_count": 1,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -50,7 +50,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 10,
+   "execution_count": 2,
    "metadata": {},
    "outputs": [
     {
@@ -67,7 +67,7 @@
        "[  sin(psi),  0]])"
       ]
      },
-     "execution_count": 10,
+     "execution_count": 2,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -81,10 +81,69 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 1,
    "metadata": {},
-   "outputs": [],
-   "source": []
+   "outputs": [
+    {
+     "data": {
+      "text/latex": [
+       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0\\end{matrix}\\right]$"
+      ],
+      "text/plain": [
+       "Matrix([\n",
+       "[0, 0, -v*sin(theta)],\n",
+       "[0, 0,  v*cos(theta)],\n",
+       "[0, 0,             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",
+    "\n",
+    "state = sp.Matrix([x,y,theta])\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\\end{matrix}\\right]$"
+      ],
+      "text/plain": [
+       "Matrix([\n",
+       "[cos(theta), 0],\n",
+       "[sin(theta), 0],\n",
+       "[         0, 1]])"
+      ]
+     },
+     "execution_count": 2,
+     "metadata": {},
+     "output_type": "execute_result"
+    }
+   ],
+   "source": [
+    "state = sp.Matrix([v,w])\n",
+    "\n",
+    "#B\n",
+    "gs.jacobian(state)"
+   ]
   },
   {
    "cell_type": "markdown",
@@ -95,7 +154,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 11,
+   "execution_count": 3,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -148,7 +207,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 12,
+   "execution_count": 11,
    "metadata": {},
    "outputs": [
     {
@@ -191,7 +250,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 13,
+   "execution_count": 12,
    "metadata": {},
    "outputs": [
     {
@@ -201,7 +260,7 @@
        "       -0.58083993])"
       ]
      },
-     "execution_count": 13,
+     "execution_count": 12,
      "metadata": {},
      "output_type": "execute_result"
     }
@@ -212,7 +271,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 20,
+   "execution_count": 13,
    "metadata": {},
    "outputs": [
     {
@@ -224,7 +283,7 @@
        " 4.2088127135203335)"
       ]
      },
-     "execution_count": 20,
+     "execution_count": 13,
      "metadata": {},
      "output_type": "execute_result"
     },
@@ -303,15 +362,15 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 15,
+   "execution_count": 7,
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "0.03551042807053904\n",
-      "12.271049217491628\n"
+      "3.377314001132912\n",
+      "22.461271020363764\n"
      ]
     }
    ],