correted function for numerical jacobian
parent
c208eaeaee
commit
741f2c158d
|
@ -15,13 +15,38 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 12,
|
"execution_count": 31,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [],
|
"outputs": [],
|
||||||
"source": [
|
"source": [
|
||||||
"import numpy as np\n",
|
"import numpy as np\n",
|
||||||
"\n",
|
"\n",
|
||||||
"def f(x,u):\n",
|
"# #CONTINUOUS\n",
|
||||||
|
"# def f(x,u):\n",
|
||||||
|
"# \"\"\"\n",
|
||||||
|
"# :param x:\n",
|
||||||
|
"# :param u:\n",
|
||||||
|
"# \"\"\"\n",
|
||||||
|
"# xx = x[0]\n",
|
||||||
|
"# xy = x[1]\n",
|
||||||
|
"# v = x[2]\n",
|
||||||
|
"# theta =x[3]\n",
|
||||||
|
" \n",
|
||||||
|
"# a = u[0]\n",
|
||||||
|
"# delta = u[1]\n",
|
||||||
|
" \n",
|
||||||
|
"# L=0.3\n",
|
||||||
|
" \n",
|
||||||
|
"# #vector of ackerman equations\n",
|
||||||
|
"# return np.array([\n",
|
||||||
|
"# np.cos(theta)*v,\n",
|
||||||
|
"# np.sin(theta)*v,\n",
|
||||||
|
"# a,\n",
|
||||||
|
"# v*np.arctan(delta)/L\n",
|
||||||
|
"# ])\n",
|
||||||
|
"\n",
|
||||||
|
"#DISCRETE\n",
|
||||||
|
"def f(x, u, dt=0.1):\n",
|
||||||
" \"\"\"\n",
|
" \"\"\"\n",
|
||||||
" :param x:\n",
|
" :param x:\n",
|
||||||
" :param u:\n",
|
" :param u:\n",
|
||||||
|
@ -38,59 +63,69 @@
|
||||||
" \n",
|
" \n",
|
||||||
" #vector of ackerman equations\n",
|
" #vector of ackerman equations\n",
|
||||||
" return np.array([\n",
|
" return np.array([\n",
|
||||||
" [np.cos(theta)*v],\n",
|
" xx + np.cos(theta)*v*dt,\n",
|
||||||
" [np.sin(theta)*v],\n",
|
" xy + np.sin(theta)*v*dt,\n",
|
||||||
" [a],\n",
|
" v + a*dt,\n",
|
||||||
" [v*np.arctan(delta)/L]\n",
|
" theta + v*np.arctan(delta)/L*dt\n",
|
||||||
" ])\n",
|
" ])\n",
|
||||||
"\n",
|
"\n",
|
||||||
"def J(f,x,u,epsilon=1e-6):\n",
|
"def J(f,x,u,epsilon=1e-4):\n",
|
||||||
" \"\"\"\n",
|
" \"\"\"\n",
|
||||||
" :param f:\n",
|
" :param f:\n",
|
||||||
" :param x:\n",
|
" :param x:\n",
|
||||||
" :param u:\n",
|
" :param u:\n",
|
||||||
" \"\"\"\n",
|
" \"\"\"\n",
|
||||||
" \n",
|
" \n",
|
||||||
|
" jac_x = np.zeros((4,4))\n",
|
||||||
" \n",
|
" \n",
|
||||||
" x_minus = x-epsilon\n",
|
" for i in range(x.shape[0]):\n",
|
||||||
" u_minus = u-epsilon\n",
|
" perturb = np.zeros(x.shape[0])\n",
|
||||||
|
" perturb[i] = epsilon\n",
|
||||||
|
" \n",
|
||||||
|
" #each loumn of the jac is given by perturbing a variable\n",
|
||||||
|
" jac_x[:,i]= (f(x+perturb, u)-f(x-perturb, u))/2*epsilon\n",
|
||||||
" \n",
|
" \n",
|
||||||
" x_plus = x+epsilon\n",
|
|
||||||
" u_plus = u+epsilon\n",
|
|
||||||
" \n",
|
" \n",
|
||||||
" # compute finite differences\n",
|
" jac_u = np.zeros((4,2))\n",
|
||||||
" f_plus = f(x_plus, u_plus)\n",
|
|
||||||
" f_minus = f(x_minus, u_minus)\n",
|
|
||||||
" \n",
|
" \n",
|
||||||
" jac = (f_plus - f_minus)/2*epsilon\n",
|
" for i in range(u.shape[0]):\n",
|
||||||
" \n",
|
" perturb = np.zeros(u.shape[0])\n",
|
||||||
" return jac\n",
|
" perturb[i] = epsilon\n",
|
||||||
|
" \n",
|
||||||
|
" #each loumn of the jac is given by perturbing a variable\n",
|
||||||
|
" jac_u[:,i]= (f(x, u+perturb)-f(x, u-perturb))/2*epsilon\n",
|
||||||
|
" \n",
|
||||||
|
" return jac_x, jac_u\n",
|
||||||
" "
|
" "
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 13,
|
"execution_count": 32,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
"data": {
|
"data": {
|
||||||
"text/plain": [
|
"text/plain": [
|
||||||
"array([[1.e-12],\n",
|
"(array([[1.00000000e-08, 0.00000000e+00, 1.00000000e-09, 0.00000000e+00],\n",
|
||||||
" [0.e+00],\n",
|
" [0.00000000e+00, 1.00000000e-08, 0.00000000e+00, 9.99999998e-10],\n",
|
||||||
" [1.e-12],\n",
|
" [0.00000000e+00, 0.00000000e+00, 1.00000000e-08, 0.00000000e+00],\n",
|
||||||
" [0.e+00]])"
|
" [0.00000000e+00, 0.00000000e+00, 6.57985199e-10, 1.00000000e-08]]),\n",
|
||||||
|
" array([[0.0000000e+00, 0.0000000e+00],\n",
|
||||||
|
" [0.0000000e+00, 0.0000000e+00],\n",
|
||||||
|
" [1.0000000e-09, 0.0000000e+00],\n",
|
||||||
|
" [0.0000000e+00, 3.2051282e-09]]))"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
"execution_count": 13,
|
"execution_count": 32,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"output_type": "execute_result"
|
"output_type": "execute_result"
|
||||||
}
|
}
|
||||||
],
|
],
|
||||||
"source": [
|
"source": [
|
||||||
"#starting condition\n",
|
"#starting condition\n",
|
||||||
"x=np.array([0,0,0,0])\n",
|
"x=np.array([0,0,1,0])\n",
|
||||||
"u=np.array([1,0])\n",
|
"u=np.array([1,0.2])\n",
|
||||||
"\n",
|
"\n",
|
||||||
"J(f,x,u)"
|
"J(f,x,u)"
|
||||||
]
|
]
|
||||||
|
|
Loading…
Reference in New Issue