2020-04-21 23:33:00 +08:00
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
2020-07-01 00:21:27 +08:00
"# STATE SPACE MODEL MATRICES\n",
"\n",
"### Diff drive"
2020-04-21 23:33:00 +08:00
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 1,
2020-04-21 23:33:00 +08:00
"metadata": {},
"outputs": [
{
2020-07-01 00:21:27 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 1,
2020-07-01 00:21:27 +08:00
"metadata": {},
"output_type": "execute_result"
2020-04-21 23:33:00 +08:00
}
],
"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",
2020-04-22 22:32:10 +08:00
" [ sp.sin(theta)*v],\n",
" [w],\n",
" [-w],\n",
" [ v*sp.sin(psi)]])\n",
2020-04-21 23:33:00 +08:00
"\n",
"state = sp.Matrix([x,y,theta,psi,cte])\n",
"\n",
"#A\n",
"gs.jacobian(state)"
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 2,
2020-04-21 23:33:00 +08:00
"metadata": {},
2020-07-01 00:21:27 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 2,
2020-07-01 00:21:27 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
2020-04-21 23:33:00 +08:00
"source": [
"state = sp.Matrix([v,w])\n",
"\n",
"#B\n",
"gs.jacobian(state)"
]
},
2020-04-22 22:32:10 +08:00
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 3,
2020-04-22 22:32:10 +08:00
"metadata": {},
2020-07-01 00:21:27 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 3,
2020-07-01 00:21:27 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
2020-05-04 19:07:01 +08:00
"source": [
"import sympy as sp\n",
"\n",
2020-05-04 22:16:29 +08:00
"x,y,theta,psi,cte,v,w ,dt= sp.symbols(\"x y theta psi cte v w dt\")\n",
2020-05-04 19:07:01 +08:00
"\n",
2020-05-04 22:16:29 +08:00
"gs = sp.Matrix([[x + sp.cos(theta)*v*dt],\n",
" [y+ sp.sin(theta)*v*dt],\n",
" [theta + w*dt]])\n",
2020-05-04 19:07:01 +08:00
"\n",
"state = sp.Matrix([x,y,theta])\n",
"\n",
"#A\n",
2020-05-04 22:16:29 +08:00
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
2020-05-04 19:07:01 +08:00
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 4,
2020-05-04 19:07:01 +08:00
"metadata": {},
2020-07-01 00:21:27 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 4,
2020-07-01 00:21:27 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
2020-05-04 19:07:01 +08:00
"source": [
"state = sp.Matrix([v,w])\n",
"\n",
"#B\n",
2020-05-04 22:16:29 +08:00
"gs.jacobian(state)#.subs({x:0,y:0,theta:0})"
2020-05-04 19:07:01 +08:00
]
2020-04-22 22:32:10 +08:00
},
2020-07-01 00:21:27 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Ackermann"
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 5,
2020-07-01 00:21:27 +08:00
"metadata": {},
2020-07-01 21:33:16 +08:00
"outputs": [],
2020-07-01 00:21:27 +08:00
"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",
2020-07-01 21:33:16 +08:00
"X = sp.Matrix([x,y,v,theta])\n",
2020-07-01 00:21:27 +08:00
"\n",
"#A\n",
2020-07-01 21:33:16 +08:00
"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="
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 6,
2020-07-01 00:21:27 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 6,
2020-07-01 00:21:27 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
2020-07-01 21:33:16 +08:00
"B"
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 7,
2020-07-01 21:33:16 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 7,
2020-07-01 21:33:16 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"#A LIN\n",
"DT = sp.symbols(\"dt\")\n",
"sp.eye(4)+A*DT"
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 8,
2020-07-01 21:33:16 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 8,
2020-07-01 21:33:16 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"B*DT"
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 9,
2020-07-01 21:33:16 +08:00
"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]])"
]
},
2020-07-06 23:54:22 +08:00
"execution_count": 9,
2020-07-01 21:33:16 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"DT*(gs - A*X - B*U)"
2020-07-01 00:21:27 +08:00
]
},
2020-04-21 23:33:00 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PATH WAYPOINTS AS PARAMETRIZED CURVE"
]
},
{
"cell_type": "code",
2020-07-06 23:54:22 +08:00
"execution_count": 10,
2020-04-21 23:33:00 +08:00
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from scipy.interpolate import interp1d\n",
"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
"\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" final_xp=np.append(final_xp,fx(interp_range))\n",
" final_yp=np.append(final_yp,fy(interp_range))\n",
"\n",
" return np.vstack((final_xp,final_yp))\n",
"\n",
"def get_nn_idx(state,path):\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.sqrt(dx**2 + dy**2)\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx"
]
},
{
"cell_type": "code",
2020-08-06 17:21:47 +08:00
"execution_count": 11,
2020-04-21 23:33:00 +08:00
"metadata": {},
2020-07-06 23:54:22 +08:00
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/miniconda3/envs/jupyter/lib/python3.8/site-packages/IPython/core/interactiveshell.py:3331: RankWarning: Polyfit may be poorly conditioned\n",
" exec(code_obj, self.user_global_ns, self.user_ns)\n"
]
}
],
2020-04-21 23:33:00 +08:00
"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",
2020-05-01 23:40:00 +08:00
"nn_idx=get_nn_idx(state,track)-1 #index ox closest wp, take the previous to have a straighter line\n",
2020-04-21 23:33:00 +08:00
"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",
2020-05-01 23:40:00 +08:00
"coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
2020-04-21 23:33:00 +08:00
"\n",
2020-05-01 23:40:00 +08:00
"#def f(x,coeff):\n",
"# return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
2020-04-21 23:33:00 +08:00
"def f(x,coeff):\n",
2020-07-06 23:54:22 +08:00
" return coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0\n",
"\n",
"def f(x,coeff):\n",
" y=0\n",
" j=len(coeff)\n",
" for k in range(j):\n",
" y += coeff[k]*x**(j-k-1)\n",
" return y\n",
"\n",
"# def df(x,coeff):\n",
"# return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
"def df(x,coeff):\n",
" y=0\n",
" j=len(coeff)\n",
" for k in range(j-1):\n",
" y += (j-k-1)*coeff[k]*x**(j-k-2)\n",
" return y"
2020-04-21 23:33:00 +08:00
]
},
{
"cell_type": "code",
2020-08-06 17:21:47 +08:00
"execution_count": 12,
2020-04-21 23:33:00 +08:00
"metadata": {},
2020-07-06 23:54:22 +08:00
"outputs": [
{
"data": {
"text/plain": [
"array([ 0.10275887, 0.03660033, -0.21750601, 0.03551043, -0.53861442,\n",
" -0.58083993])"
]
},
2020-08-06 17:21:47 +08:00
"execution_count": 12,
2020-07-06 23:54:22 +08:00
"metadata": {},
"output_type": "execute_result"
}
],
2020-04-21 23:33:00 +08:00
"source": [
"coeff"
]
},
{
"cell_type": "code",
2020-08-06 17:21:47 +08:00
"execution_count": 13,
2020-04-21 23:33:00 +08:00
"metadata": {},
2020-07-06 23:54:22 +08:00
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 432x288 with 2 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
2020-04-21 23:33:00 +08:00
"source": [
"import matplotlib.pyplot as plt\n",
"plt.style.use(\"ggplot\")\n",
"\n",
2020-06-29 22:31:43 +08:00
"x=np.arange(-1,2,0.001) #interp range of curve \n",
2020-04-21 23:33:00 +08:00
"\n",
"# VEHICLE REF FRAME\n",
2020-06-29 22:31:43 +08:00
"plt.subplot(2,1,1)\n",
"plt.title('parametrized curve, vehicle ref frame')\n",
2020-04-21 23:33:00 +08:00
"plt.scatter(0,0)\n",
"plt.scatter(wp_vehicle_frame[0,:],wp_vehicle_frame[1,:])\n",
"plt.plot(x,[f(xs,coeff) for xs in x])\n",
"plt.axis('equal')\n",
"\n",
"# MAP REF FRAME\n",
2020-06-29 22:31:43 +08:00
"plt.subplot(2,1,2)\n",
"plt.title('waypoints, map ref frame')\n",
2020-04-21 23:33:00 +08:00
"plt.scatter(state[0],state[1])\n",
"plt.scatter(track[0,:],track[1,:])\n",
"plt.scatter(track[0,nn_idx:nn_idx+LOOKAHED],track[1,nn_idx:nn_idx+LOOKAHED])\n",
2020-06-29 22:31:43 +08:00
"plt.axis('equal')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()\n",
"#plt.savefig(\"fitted_poly\")"
2020-04-21 23:33:00 +08:00
]
},
2020-07-06 23:54:22 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
2020-08-06 17:21:47 +08:00
"## With SPLINES"
2020-07-06 23:54:22 +08:00
]
},
{
"cell_type": "code",
2020-08-06 17:21:47 +08:00
"execution_count": 17,
2020-07-06 23:54:22 +08:00
"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",
2020-08-06 17:21:47 +08:00
"from scipy.interpolate import CubicSpline\n",
2020-07-06 23:54:22 +08:00
"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"
]
},
2020-08-06 17:21:47 +08:00
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
" \n",
" bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T \n",
" \n",
" C = np.array([[1, xs, xs**2, xs**3, xs**4, xs**5], #f(xs)=ys\n",
" [0, 1, 2*xs**1, 3*xs**2, 4*xs**3, 5**xs^4], #df(xs)=dys\n",
" [0, 0, 1, 6*xs**1, 12*xs**2, 20**xs^3], #ddf(xs)=ddys\n",
" [1, xf, xf**2, xf**3, xf**4, xf**5], #f(xf)=yf\n",
" [0, 1, 2*xf**1, 3*xf**2, 4*xf**3, 5**xf^4], #df(xf)=dyf\n",
" [0, 0, 1, 6*xf**1, 12*xf**2, 20**xf^3]]) #ddf(xf)=ddyf\n",
" \n",
" #To compute the polynomial coefficients we solve:\n",
" #Ax = B. \n",
" #Matrices A and B must have the same number of rows\n",
" a = np.linalg.lstsq(C,bc)[0]\n",
" return a"
]
},
2020-04-21 23:33:00 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# COMPUTE ERRORS"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2020-05-01 23:40:00 +08:00
"* **crosstrack error** cte -> desired y-position - y-position of vehicle: this is the value of the fitted polynomial (road curve)\n",
2020-04-21 23:33:00 +08:00
" \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",
2020-08-06 17:21:47 +08:00
"execution_count": 15,
2020-04-21 23:33:00 +08:00
"metadata": {},
2020-07-06 23:54:22 +08:00
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"-0.5808399313875324\n",
"28.307545725691345\n"
]
}
],
2020-04-21 23:33:00 +08:00
"source": [
"#for 0\n",
"\n",
2020-07-06 23:54:22 +08:00
"# cte=coeff[3]\n",
"# epsi=-np.arctan(coeff[2])\n",
"cte=f(0,coeff)\n",
"epsi=-np.arctan(df(0,coeff))\n",
2020-04-21 23:33:00 +08:00
"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",
2020-07-01 21:33:16 +08:00
"Starting State is :\n",
"\n",
2020-04-21 23:33:00 +08:00
"* $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",
2020-05-01 23:40:00 +08:00
"Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
2020-04-21 23:33:00 +08:00
]
},
{
"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",
2020-12-12 22:45:43 +08:00
"version": "3.7.6"
2020-04-21 23:33:00 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 4
}
