tidy up MPC class

notebooks/1.2-parametrized-path-curves.ipynb

@@ -7,6 +7,13 @@
     "# PATH WAYPOINTS AS PARAMETRIZED CURVE"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "In this notebook I try to reproduce the parmetrization of the track via curve-fitting like its done in Udacity MPC Course. "
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": 10,
@@ -183,6 +190,58 @@
     "#plt.savefig(\"fitted_poly\")"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Error Formulation\n",
+    "\n",
+    "So, the track can be represented by fitting a curve trough its waypoints, using the vehicle position as reference!\n",
+    "\n",
+    "<!-- ![mpc](img/fitted_poly.png) -->\n",
+    "\n",
+    "Recall A fitted cubic poly has the form:\n",
+    "\n",
+    "$\n",
+    "f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
+    "$\n",
+    "\n",
+    "The derivative of a fitted cubic 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 we can formulate\n",
+    "\n",
+    "* **crosstrack error** cte: desired y-position - y-position of vehicle -> this is the value of the fitted polynomial\n",
+    "\n",
+    "* **heading error** epsi:  desired heading - heading of vehicle -> is the inclination of  tangent to the  fitted polynomial\n",
+    "\n",
+    "Becouse the reference is centered on vehicle the eqation are simplified!\n",
+    "Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n",
+    "\n",
+    "$\n",
+    "cte = f(px) \\\\\n",
+    "\\psi = -atan(f`(px)) \\\\\n",
+    "$"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### In Practice:\n",
+    "I use a **convex** mpc so non-linearities are not allowed (in Udacity they use a general-purpose nonlinear solver) -> so this solution does not really work well for my case..."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Extras"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,