daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix Jacobian matrices

Browse Source
This commit is contained in:
Peter Schneider 2017-07-29 01:41:14 -07:00
parent e6510ac579
commit 6dd31f822c
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
11-Extended-Kalman-Filters.ipynb
View File

@ -964,15 +964,15 @@
"We find The $\\mathbf F$ by taking the Jacobian of $f(x,u)$.\n",
"\n",
"$$\\mathbf F = \\frac{\\partial f(x, u)}{\\partial x} =\\begin{bmatrix}\n",
"\\frac{\\partial \\dot x}{\\partial x} & \n",
"\\frac{\\partial \\dot x}{\\partial y} &\n",
"\\frac{\\partial \\dot x}{\\partial \\theta}\\\\\n",
"\\frac{\\partial \\dot y}{\\partial x} & \n",
"\\frac{\\partial \\dot y}{\\partial y} &\n",
"\\frac{\\partial \\dot y}{\\partial \\theta} \\\\\n",
"\\frac{\\partial \\dot{\\theta}}{\\partial x} & \n",
"\\frac{\\partial \\dot{\\theta}}{\\partial y} &\n",
"\\frac{\\partial \\dot{\\theta}}{\\partial \\theta}\n",
"\\frac{\\partial f_1}{\\partial x} & \n",
"\\frac{\\partial f_1}{\\partial y} &\n",
"\\frac{\\partial f_1}{\\partial \\theta}\\\\\n",
"\\frac{\\partial f_2}{\\partial x} & \n",
"\\frac{\\partial f_2}{\\partial y} &\n",
"\\frac{\\partial f_2}{\\partial \\theta} \\\\\n",
"\\frac{\\partial f_3}{\\partial x} & \n",
"\\frac{\\partial f_3}{\\partial y} &\n",
"\\frac{\\partial f_3}{\\partial \\theta}\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
@ -1087,9 +1087,9 @@
"If this was a linear problem we would convert from control space to state space using the by now familiar $\\mathbf{FMF}^\\mathsf T$ form. Since our motion model is nonlinear we do not try to find a closed form solution to this, but instead linearize it with a Jacobian which we will name $\\mathbf{V}$. \n",
"\n",
"$$\\mathbf{V} = \\frac{\\partial f(x, u)}{\\partial u} \\begin{bmatrix}\n",
"\\frac{\\partial \\dot x}{\\partial v} & \\frac{\\partial \\dot x}{\\partial \\alpha} \\\\\n",
"\\frac{\\partial \\dot y}{\\partial v} & \\frac{\\partial \\dot y}{\\partial \\alpha} \\\\\n",
"\\frac{\\partial \\dot{\\theta}}{\\partial v} & \\frac{\\partial \\dot{\\theta}}{\\partial \\alpha}\n",
"\\frac{\\partial f_1}{\\partial v} & \\frac{\\partial f_1}{\\partial \\alpha} \\\\\n",
"\\frac{\\partial f_2}{\\partial v} & \\frac{\\partial f_2}{\\partial \\alpha} \\\\\n",
"\\frac{\\partial f_3}{\\partial v} & \\frac{\\partial f_3}{\\partial \\alpha}\n",
"\\end{bmatrix}$$\n",
"\n",
"These partial derivatives become very difficult to work with. Let's compute them with SymPy. "