reorganizing my jupyter notebooks
mcarfagno
parent
804bb08fd3
commit
1ed9537fdc
File diff suppressed because one or more lines are too long
|
|
@ -0,0 +1,344 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# STATE SPACE MODEL MATRICES\n",
|
||||
"\n",
|
||||
"### Diff drive"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[0, 0, -v*sin(theta), 0, 0],\n",
|
||||
"[0, 0, v*cos(theta), 0, 0],\n",
|
||||
"[0, 0, 0, 0, 0],\n",
|
||||
"[0, 0, 0, 0, 0],\n",
|
||||
"[0, 0, 0, v*cos(psi), 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",
|
||||
" [-w],\n",
|
||||
" [ v*sp.sin(psi)]])\n",
|
||||
"\n",
|
||||
"state = sp.Matrix([x,y,theta,psi,cte])\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\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[cos(theta), 0],\n",
|
||||
"[sin(theta), 0],\n",
|
||||
"[ 0, 1],\n",
|
||||
"[ 0, -1],\n",
|
||||
"[ sin(psi), 0]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"state = sp.Matrix([v,w])\n",
|
||||
"\n",
|
||||
"#B\n",
|
||||
"gs.jacobian(state)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[1, 0, -dt*v*sin(theta)],\n",
|
||||
"[0, 1, dt*v*cos(theta)],\n",
|
||||
"[0, 0, 1]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import sympy as sp\n",
|
||||
"\n",
|
||||
"x,y,theta,psi,cte,v,w ,dt= sp.symbols(\"x y theta psi cte v w dt\")\n",
|
||||
"\n",
|
||||
"gs = sp.Matrix([[x + sp.cos(theta)*v*dt],\n",
|
||||
" [y+ sp.sin(theta)*v*dt],\n",
|
||||
" [theta + w*dt]])\n",
|
||||
"\n",
|
||||
"state = sp.Matrix([x,y,theta])\n",
|
||||
"\n",
|
||||
"#A\n",
|
||||
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}dt \\cos{\\left(\\theta \\right)} & 0\\\\dt \\sin{\\left(\\theta \\right)} & 0\\\\0 & dt\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[dt*cos(theta), 0],\n",
|
||||
"[dt*sin(theta), 0],\n",
|
||||
"[ 0, dt]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"state = sp.Matrix([v,w])\n",
|
||||
"\n",
|
||||
"#B\n",
|
||||
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Ackermann"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 5,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"x,y,theta,v,delta,L,a = sp.symbols(\"x y theta v delta L a\")\n",
|
||||
"\n",
|
||||
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
|
||||
" [ sp.sin(theta)*v],\n",
|
||||
" [a],\n",
|
||||
" [ v*sp.tan(delta)/L]])\n",
|
||||
"\n",
|
||||
"X = sp.Matrix([x,y,v,theta])\n",
|
||||
"\n",
|
||||
"#A\n",
|
||||
"A=gs.jacobian(X)\n",
|
||||
"\n",
|
||||
"U = sp.Matrix([a,delta])\n",
|
||||
"\n",
|
||||
"#B\n",
|
||||
"B=gs.jacobian(U)#.subs({x:0,y:0,theta:0})B="
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & dt \\cos{\\left(\\theta \\right)} & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt \\sin{\\left(\\theta \\right)} & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & \\frac{dt \\tan{\\left(\\delta \\right)}}{L} & 1\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[1, 0, dt*cos(theta), -dt*v*sin(theta)],\n",
|
||||
"[0, 1, dt*sin(theta), dt*v*cos(theta)],\n",
|
||||
"[0, 0, 1, 0],\n",
|
||||
"[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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 8,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\dt & 0\\\\0 & \\frac{dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[ 0, 0],\n",
|
||||
"[ 0, 0],\n",
|
||||
"[dt, 0],\n",
|
||||
"[ 0, dt*v*(tan(delta)**2 + 1)/L]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 8,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"B*DT"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 9,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}dt \\theta v \\sin{\\left(\\theta \\right)}\\\\- dt \\theta v \\cos{\\left(\\theta \\right)}\\\\0\\\\- \\frac{\\delta dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[ dt*theta*v*sin(theta)],\n",
|
||||
"[ -dt*theta*v*cos(theta)],\n",
|
||||
"[ 0],\n",
|
||||
"[-delta*dt*v*(tan(delta)**2 + 1)/L]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 9,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"DT*(gs - A*X - B*U)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# ADD DELAY (for real time implementation)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"It is necessary to take *actuation latency* into account: so instead of using the actual state as estimated, the delay factored in using the kinematic model\n",
|
||||
"\n",
|
||||
"Starting State is :\n",
|
||||
"\n",
|
||||
"* $x_{delay} = 0.0 + v * dt$\n",
|
||||
"* $y_{delay} = 0.0$\n",
|
||||
"* $psi_{delay} = 0.0 + w * dt$\n",
|
||||
"* $cte_{delay} = cte + v * sin(epsi) * dt$\n",
|
||||
"* $epsi_{delay} = epsi - w * dt$\n",
|
||||
"\n",
|
||||
"Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
"name": "ipython",
|
||||
"version": 3
|
||||
},
|
||||
"file_extension": ".py",
|
||||
"mimetype": "text/x-python",
|
||||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.8.5"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
"nbformat_minor": 4
|
||||
}
|
||||
|
|
@ -1,293 +1,5 @@
|
|||
{
|
||||
"cells": [
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# STATE SPACE MODEL MATRICES\n",
|
||||
"\n",
|
||||
"### Diff drive"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 1,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[0, 0, -v*sin(theta), 0, 0],\n",
|
||||
"[0, 0, v*cos(theta), 0, 0],\n",
|
||||
"[0, 0, 0, 0, 0],\n",
|
||||
"[0, 0, 0, 0, 0],\n",
|
||||
"[0, 0, 0, v*cos(psi), 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",
|
||||
" [-w],\n",
|
||||
" [ v*sp.sin(psi)]])\n",
|
||||
"\n",
|
||||
"state = sp.Matrix([x,y,theta,psi,cte])\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\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[cos(theta), 0],\n",
|
||||
"[sin(theta), 0],\n",
|
||||
"[ 0, 1],\n",
|
||||
"[ 0, -1],\n",
|
||||
"[ sin(psi), 0]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 2,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"state = sp.Matrix([v,w])\n",
|
||||
"\n",
|
||||
"#B\n",
|
||||
"gs.jacobian(state)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[1, 0, -dt*v*sin(theta)],\n",
|
||||
"[0, 1, dt*v*cos(theta)],\n",
|
||||
"[0, 0, 1]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 3,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"import sympy as sp\n",
|
||||
"\n",
|
||||
"x,y,theta,psi,cte,v,w ,dt= sp.symbols(\"x y theta psi cte v w dt\")\n",
|
||||
"\n",
|
||||
"gs = sp.Matrix([[x + sp.cos(theta)*v*dt],\n",
|
||||
" [y+ sp.sin(theta)*v*dt],\n",
|
||||
" [theta + w*dt]])\n",
|
||||
"\n",
|
||||
"state = sp.Matrix([x,y,theta])\n",
|
||||
"\n",
|
||||
"#A\n",
|
||||
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}dt \\cos{\\left(\\theta \\right)} & 0\\\\dt \\sin{\\left(\\theta \\right)} & 0\\\\0 & dt\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[dt*cos(theta), 0],\n",
|
||||
"[dt*sin(theta), 0],\n",
|
||||
"[ 0, dt]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 4,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"state = sp.Matrix([v,w])\n",
|
||||
"\n",
|
||||
"#B\n",
|
||||
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"### Ackermann"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 5,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"x,y,theta,v,delta,L,a = sp.symbols(\"x y theta v delta L a\")\n",
|
||||
"\n",
|
||||
"gs = sp.Matrix([[ sp.cos(theta)*v],\n",
|
||||
" [ sp.sin(theta)*v],\n",
|
||||
" [a],\n",
|
||||
" [ v*sp.tan(delta)/L]])\n",
|
||||
"\n",
|
||||
"X = sp.Matrix([x,y,v,theta])\n",
|
||||
"\n",
|
||||
"#A\n",
|
||||
"A=gs.jacobian(X)\n",
|
||||
"\n",
|
||||
"U = sp.Matrix([a,delta])\n",
|
||||
"\n",
|
||||
"#B\n",
|
||||
"B=gs.jacobian(U)#.subs({x:0,y:0,theta:0})B="
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 6,
|
||||
"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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 7,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}1 & 0 & dt \\cos{\\left(\\theta \\right)} & - dt v \\sin{\\left(\\theta \\right)}\\\\0 & 1 & dt \\sin{\\left(\\theta \\right)} & dt v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & \\frac{dt \\tan{\\left(\\delta \\right)}}{L} & 1\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[1, 0, dt*cos(theta), -dt*v*sin(theta)],\n",
|
||||
"[0, 1, dt*sin(theta), dt*v*cos(theta)],\n",
|
||||
"[0, 0, 1, 0],\n",
|
||||
"[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"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 8,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\dt & 0\\\\0 & \\frac{dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[ 0, 0],\n",
|
||||
"[ 0, 0],\n",
|
||||
"[dt, 0],\n",
|
||||
"[ 0, dt*v*(tan(delta)**2 + 1)/L]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 8,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"B*DT"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 9,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"data": {
|
||||
"text/latex": [
|
||||
"$\\displaystyle \\left[\\begin{matrix}dt \\theta v \\sin{\\left(\\theta \\right)}\\\\- dt \\theta v \\cos{\\left(\\theta \\right)}\\\\0\\\\- \\frac{\\delta dt v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]$"
|
||||
],
|
||||
"text/plain": [
|
||||
"Matrix([\n",
|
||||
"[ dt*theta*v*sin(theta)],\n",
|
||||
"[ -dt*theta*v*cos(theta)],\n",
|
||||
"[ 0],\n",
|
||||
"[-delta*dt*v*(tan(delta)**2 + 1)/L]])"
|
||||
]
|
||||
},
|
||||
"execution_count": 9,
|
||||
"metadata": {},
|
||||
"output_type": "execute_result"
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"DT*(gs - A*X - B*U)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
|
|
@ -471,70 +183,6 @@
|
|||
"#plt.savefig(\"fitted_poly\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"## With SPLINES"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 17,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"(array([-0.39433757, -0.39433757, -0.39433757, -0.39433757, 0.56791288,\n",
|
||||
" 1.04903811, 1.67104657, 1.67104657, 1.67104657, 1.67104657]), array([-0.34967937, 0.15467936, -2.19173016, 1.11089663, -8. ,\n",
|
||||
" -0.7723291 , 0. , 0. , 0. , 0. ]), 3)\n",
|
||||
"[[ 4.64353595 4.64353595 4.64353595 4.64353595 -23.21767974\n",
|
||||
" 65.74806776 65.74806776 65.74806776 65.74806776]\n",
|
||||
" [ -6.70236682 -6.70236682 -6.70236682 -6.70236682 6.70236682\n",
|
||||
" -26.8094673 95.8780974 95.8780974 95.8780974 ]\n",
|
||||
" [ 1.57243489 1.57243489 1.57243489 1.57243489 1.57243489\n",
|
||||
" -8.10159833 34.85967446 34.85967446 34.85967446]\n",
|
||||
" [ -0.34967937 -0.34967937 -0.34967937 -0.34967937 -0.90523492\n",
|
||||
" -1.1830127 -0.7723291 -0.7723291 -0.7723291 ]]\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"#define track\n",
|
||||
"wp=np.array([0,5,6,10,11,15, 0,0,2,2,0,4]).reshape(2,-1)\n",
|
||||
"track = compute_path_from_wp(wp[0,:],wp[1,:],step=0.5)\n",
|
||||
"\n",
|
||||
"#vehicle state\n",
|
||||
"state=[3.5,0.5,np.radians(30)]\n",
|
||||
"\n",
|
||||
"#given vehicle pos find lookahead waypoints\n",
|
||||
"nn_idx=get_nn_idx(state,track)-1 #index ox closest wp, take the previous to have a straighter line\n",
|
||||
"LOOKAHED=6\n",
|
||||
"lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
|
||||
"\n",
|
||||
"#trasform lookahead waypoints to vehicle ref frame\n",
|
||||
"dx = lk_wp[0,:] - state[0]\n",
|
||||
"dy = lk_wp[1,:] - state[1]\n",
|
||||
"\n",
|
||||
"wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
|
||||
" dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
|
||||
"\n",
|
||||
"#fit poly\n",
|
||||
"import scipy\n",
|
||||
"from scipy.interpolate import CubicSpline\n",
|
||||
"from scipy.interpolate import PPoly,splrep\n",
|
||||
"spl=splrep(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:])\n",
|
||||
"print( spl)\n",
|
||||
"print(PPoly.from_spline(spl).c)\n",
|
||||
"#coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
|
||||
"\n",
|
||||
"#def f(x,coeff):\n",
|
||||
"# return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
|
||||
"\n"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
|
|
@ -558,108 +206,13 @@
|
|||
" a = np.linalg.lstsq(C,bc)[0]\n",
|
||||
" return a"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# COMPUTE ERRORS"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"* **crosstrack error** cte -> desired y-position - y-position of vehicle: this is the value of the fitted polynomial (road curve)\n",
|
||||
" \n",
|
||||
"$\n",
|
||||
"f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
|
||||
"$\n",
|
||||
"\n",
|
||||
"Then for the origin cte = K_3\n",
|
||||
" \n",
|
||||
"* **heading error** epsi -> desired heading - heading of vehicle : is the inclination of tangent to the fitted polynomial (road curve)\n",
|
||||
"\n",
|
||||
"The derivative of the fitted 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 for the origin the equation of the tangent in the origin is $y=k2$ \n",
|
||||
"\n",
|
||||
"epsi = -atan(K_2)\n",
|
||||
"\n",
|
||||
"in general:\n",
|
||||
"\n",
|
||||
"$\n",
|
||||
"y_{desired} = f(px) \\\\\n",
|
||||
"heading_{desired} = -atan(f`(px))\n",
|
||||
"$"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 15,
|
||||
"metadata": {},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"-0.5808399313875324\n",
|
||||
"28.307545725691345\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"#for 0\n",
|
||||
"\n",
|
||||
"# cte=coeff[3]\n",
|
||||
"# epsi=-np.arctan(coeff[2])\n",
|
||||
"cte=f(0,coeff)\n",
|
||||
"epsi=-np.arctan(df(0,coeff))\n",
|
||||
"print(cte)\n",
|
||||
"print(np.degrees(epsi))"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# ADD DELAY (for real time implementation)"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"It is necessary to take *actuation latency* into account: so instead of using the actual state as estimated, the delay factored in using the kinematic model\n",
|
||||
"\n",
|
||||
"Starting State is :\n",
|
||||
"\n",
|
||||
"* $x_{delay} = 0.0 + v * dt$\n",
|
||||
"* $y_{delay} = 0.0$\n",
|
||||
"* $psi_{delay} = 0.0 + w * dt$\n",
|
||||
"* $cte_{delay} = cte + v * sin(epsi) * dt$\n",
|
||||
"* $epsi_{delay} = epsi - w * dt$\n",
|
||||
"\n",
|
||||
"Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {},
|
||||
"outputs": [],
|
||||
"source": []
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
"kernelspec": {
|
||||
"display_name": "Python 3",
|
||||
"display_name": "Python [conda env:.conda-jupyter] *",
|
||||
"language": "python",
|
||||
"name": "python3"
|
||||
"name": "conda-env-.conda-jupyter-py"
|
||||
},
|
||||
"language_info": {
|
||||
"codemirror_mode": {
|
||||
|
|
@ -671,7 +224,7 @@
|
|||
"name": "python",
|
||||
"nbconvert_exporter": "python",
|
||||
"pygments_lexer": "ipython3",
|
||||
"version": "3.7.6"
|
||||
"version": "3.8.5"
|
||||
}
|
||||
},
|
||||
"nbformat": 4,
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
|
|
@ -564,15 +564,6 @@
|
|||
"The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"\n",
|
||||
"\n",
|
||||
""
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {},
|
||||
Loading…
Reference in New Issue