12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixing notebook theory

master
mcarfagno 2020-05-04 12:07:01 +01:00
parent 3866ca7ca9
commit e404fbf84a
2 changed files with 154 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
notebook/MPC_cte_cvxpy.ipynb
View File

File diff suppressed because one or more lines are too long

91
notebook/equations.ipynb
View File

@ -9,7 +9,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 9, "execution_count": 1,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -26,7 +26,7 @@
"[0, 0, 0, v*cos(psi), 0]])" "[0, 0, 0, v*cos(psi), 0]])"
] ]
}, },
"execution_count": 9, "execution_count": 1,
"metadata": {}, "metadata": {},
"output_type": "execute_result" "output_type": "execute_result"
} }
@ -50,7 +50,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 10, "execution_count": 2,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -67,7 +67,7 @@
"[ sin(psi), 0]])" "[ sin(psi), 0]])"
] ]
}, },
"execution_count": 10, "execution_count": 2,
"metadata": {}, "metadata": {},
"output_type": "execute_result" "output_type": "execute_result"
} }
@ -81,10 +81,69 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [
"source": [] {
"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", "cell_type": "markdown",
@ -95,7 +154,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 11, "execution_count": 3,
"metadata": {}, "metadata": {},
"outputs": [], "outputs": [],
"source": [ "source": [
@ -148,7 +207,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 12, "execution_count": 11,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -191,7 +250,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 13, "execution_count": 12,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -201,7 +260,7 @@
" -0.58083993])" " -0.58083993])"
] ]
}, },
"execution_count": 13, "execution_count": 12,
"metadata": {}, "metadata": {},
"output_type": "execute_result" "output_type": "execute_result"
} }
@ -212,7 +271,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 20, "execution_count": 13,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
@ -224,7 +283,7 @@
" 4.2088127135203335)" " 4.2088127135203335)"
] ]
}, },
"execution_count": 20, "execution_count": 13,
"metadata": {}, "metadata": {},
"output_type": "execute_result" "output_type": "execute_result"
}, },
@ -303,15 +362,15 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 15, "execution_count": 7,
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
{ {
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"0.03551042807053904\n", "3.377314001132912\n",
"12.271049217491628\n" "22.461271020363764\n"
] ]
} }
], ],