checking out different implementation

master
mcarfagno 2021-04-15 14:46:12 +01:00
parent 5d1bb69cc3
commit 4716cbd108
2 changed files with 550 additions and 57 deletions

View File

@ -151,14 +151,49 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"### Ackermann"
"## Ackermann Kinematics model\n",
"\n",
"### Jacobians"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [],
"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",
@ -175,39 +210,21 @@
"U = sp.Matrix([a,delta])\n",
"\n",
"#B\n",
"B=gs.jacobian(U)#.subs({x:0,y:0,theta:0})B="
"B=gs.jacobian(U)\n",
"display(A)\n",
"display(B)"
]
},
{
"cell_type": "code",
"execution_count": 6,
"cell_type": "markdown",
"metadata": {},
"outputs": [
{
"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]])"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"B"
"### Discretized and Linearized model"
]
},
{
"cell_type": "code",
"execution_count": 7,
"execution_count": 9,
"metadata": {},
"outputs": [
{
@ -223,22 +240,9 @@
"[0, 0, dt*tan(delta)/L, 1]])"
]
},
"execution_count": 7,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"#A LIN\n",
"DT = sp.symbols(\"dt\")\n",
"sp.eye(4)+A*DT"
]
"output_type": "display_data"
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
@ -252,20 +256,9 @@
"[ 0, dt*v*(tan(delta)**2 + 1)/L]])"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"B*DT"
]
"output_type": "display_data"
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"text/latex": [
@ -279,13 +272,16 @@
"[-delta*dt*v*(tan(delta)**2 + 1)/L]])"
]
},
"execution_count": 9,
"metadata": {},
"output_type": "execute_result"
"output_type": "display_data"
}
],
"source": [
"DT*(gs - A*X - B*U)"
"DT = sp.symbols(\"dt\")\n",
"\n",
"display(sp.eye(4)+A*DT)\n",
"display(B*DT)\n",
"display(DT*(gs - A*X - B*U))"
]
},
{

File diff suppressed because one or more lines are too long