12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

correted function for numerical jacobian

master
mcarfagno 2020-12-12 18:15:08 +00:00
parent c208eaeaee
commit 741f2c158d
1 changed files with 60 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
notebooks/numericalJacobian.ipynb
View File

@ -15,13 +15,38 @@
},
{
"cell_type": "code",
"execution_count": 12,
"execution_count": 31,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\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",
" :param x:\n",
" :param u:\n",
@ -38,59 +63,69 @@
" \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",
" xx + np.cos(theta)*v*dt,\n",
" xy + np.sin(theta)*v*dt,\n",
" v + a*dt,\n",
" theta + v*np.arctan(delta)/L*dt\n",
" ])\n",
"\n",
"def J(f,x,u,epsilon=1e-6):\n",
"def J(f,x,u,epsilon=1e-4):\n",
" \"\"\"\n",
" :param f:\n",
" :param x:\n",
" :param u:\n",
" \"\"\"\n",
" \n",
" jac_x = np.zeros((4,4))\n",
" \n",
" x_minus = x-epsilon\n",
" u_minus = u-epsilon\n",
" for i in range(x.shape[0]):\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",
" x_plus = x+epsilon\n",
" u_plus = u+epsilon\n",
" \n",
" # compute finite differences\n",
" f_plus = f(x_plus, u_plus)\n",
" f_minus = f(x_minus, u_minus)\n",
" jac_u = np.zeros((4,2))\n",
" \n",
" jac = (f_plus - f_minus)/2*epsilon\n",
" \n",
" return jac\n",
" for i in range(u.shape[0]):\n",
" perturb = np.zeros(u.shape[0])\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",
"execution_count": 13,
"execution_count": 32,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"array([[1.e-12],\n",
" [0.e+00],\n",
" [1.e-12],\n",
" [0.e+00]])"
"(array([[1.00000000e-08, 0.00000000e+00, 1.00000000e-09, 0.00000000e+00],\n",
" [0.00000000e+00, 1.00000000e-08, 0.00000000e+00, 9.99999998e-10],\n",
" [0.00000000e+00, 0.00000000e+00, 1.00000000e-08, 0.00000000e+00],\n",
" [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": {},
"output_type": "execute_result"
}
],
"source": [
"#starting condition\n",
"x=np.array([0,0,0,0])\n",
"u=np.array([1,0])\n",
"x=np.array([0,0,1,0])\n",
"u=np.array([1,0.2])\n",
"\n",
"J(f,x,u)"
]