345 lines
8.7 KiB
Plaintext
345 lines
8.7 KiB
Plaintext
|
{
|
||
|
"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
|
||
|
}
|