tidy up MPC class

master
mcarfagno 2021-04-19 16:18:08 +01:00
parent 4716cbd108
commit caf78afa34
2 changed files with 95 additions and 32 deletions

View File

@ -7,6 +7,13 @@
"# PATH WAYPOINTS AS PARAMETRIZED CURVE" "# 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", "cell_type": "code",
"execution_count": 10, "execution_count": 10,
@ -183,6 +190,58 @@
"#plt.savefig(\"fitted_poly\")" "#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", "cell_type": "code",
"execution_count": null, "execution_count": null,

File diff suppressed because one or more lines are too long