128 lines
2.7 KiB
Plaintext
128 lines
2.7 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"metadata": {},
|
|
"source": [
|
|
"# Compute the jacobian numerically\n",
|
|
"\n",
|
|
"link: --> http://www.maths.lth.se/na/courses/FMN081/FMN081-06/lecture7.pdf\n",
|
|
"\n",
|
|
"Often the Jacobian is not **analytically** available and it has to be computed numerically.\n",
|
|
"It can be computed column wise by finite differences:\n",
|
|
"\n"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 12,
|
|
"metadata": {},
|
|
"outputs": [],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"\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",
|
|
"def J(f,x,u,epsilon=1e-6):\n",
|
|
" \"\"\"\n",
|
|
" :param f:\n",
|
|
" :param x:\n",
|
|
" :param u:\n",
|
|
" \"\"\"\n",
|
|
" \n",
|
|
" \n",
|
|
" x_minus = x-epsilon\n",
|
|
" u_minus = u-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",
|
|
" \n",
|
|
" jac = (f_plus - f_minus)/2*epsilon\n",
|
|
" \n",
|
|
" return jac\n",
|
|
" "
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"data": {
|
|
"text/plain": [
|
|
"array([[1.e-12],\n",
|
|
" [0.e+00],\n",
|
|
" [1.e-12],\n",
|
|
" [0.e+00]])"
|
|
]
|
|
},
|
|
"execution_count": 13,
|
|
"metadata": {},
|
|
"output_type": "execute_result"
|
|
}
|
|
],
|
|
"source": [
|
|
"#starting condition\n",
|
|
"x=np.array([0,0,0,0])\n",
|
|
"u=np.array([1,0])\n",
|
|
"\n",
|
|
"J(f,x,u)"
|
|
]
|
|
},
|
|
{
|
|
"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.7.6"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 4
|
|
}
|