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

Merge pull request #166 from peteryschneider/ekf_math

Browse Source 
EKF Math Corrections
This commit is contained in:
Roger Labbe 2018-01-01 11:51:03 -08:00 committed by GitHub
commit 7041feb145
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 18 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -347,29 +347,22 @@
"\n",
"$$\\begin{aligned}m &= f'(x=1.5) \\\\&= 2(1.5) - 2 \\\\&= 1\\end{aligned}$$ \n",
"\n",
"Linearizing systems of differential equations is similar. We linearize $f(\\mathbf x, \\mathbf u)$, and $h(\\mathbf x)$ by taking the partial derivatives of each to evaluate $\\mathbf A$ and $\\mathbf H$ at the point $\\mathbf x_t$ and $\\mathbf u_t$. We call the partial derivative of a matrix the [*Jacobian*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant). This gives us the the system dynamics matrix and measurement model matrix:\n",
"Linearizing systems of differential equations is similar. We linearize $f(\\mathbf x, \\mathbf u)$, and $h(\\mathbf x)$ by taking the partial derivatives of each to evaluate $\\mathbf F$ and $\\mathbf H$ at the point $\\mathbf x_t$ and $\\mathbf u_t$. We call the partial derivative of a matrix the [*Jacobian*](https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant). This gives us the the discrete state transition matrix and measurement model matrix:\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf A \n",
"\\mathbf F \n",
"&= {\\frac{\\partial{f(\\mathbf x_t, \\mathbf u_t)}}{\\partial{\\mathbf x}}}\\biggr|_{{\\mathbf x_t},{\\mathbf u_t}} \\\\\n",
"\\mathbf H &= \\frac{\\partial{h(\\bar{\\mathbf x}_t)}}{\\partial{\\bar{\\mathbf x}}}\\biggr|_{\\bar{\\mathbf x}_t} \n",
"\\end{aligned}\n",
"$$\n",
"\n",
"Finally, we find the discrete state transition matrix $\\mathbf F$ by using the Taylor series expansion of $e^{\\mathbf A \\Delta t}$:\n",
"\n",
"$$\\mathbf F = e^{\\mathbf A\\Delta t} = \\mathbf{I} + \\mathbf A\\Delta t + \\frac{(\\mathbf A\\Delta t)^2}{2!} + \\frac{(\\mathbf A\\Delta t)^3}{3!} + ... $$\n",
"\n",
"Alternatively, you can use one of the other techniques we learned in the **Kalman Math** chapter. \n",
"\n",
"This leads to the following equations for the EKF. I put boxes around the differences from the linear filter:\n",
"\n",
"$$\\begin{array}{l|l}\n",
"\\text{linear Kalman filter} & \\text{EKF} \\\\\n",
"\\hline \n",
"& \\boxed{\\mathbf A = {\\frac{\\partial{f(\\mathbf x_t, \\mathbf u_t)}}{\\partial{\\mathbf x}}}\\biggr|_{{\\mathbf x_t},{\\mathbf u_t}}} \\\\\n",
"& \\boxed{\\mathbf F = e^{\\mathbf A \\Delta t}} \\\\\n",
"& \\boxed{\\mathbf F = {\\frac{\\partial{f(\\mathbf x_t, \\mathbf u_t)}}{\\partial{\\mathbf x}}}\\biggr|_{{\\mathbf x_t},{\\mathbf u_t}}} \\\\\n",
"\\mathbf{\\bar x} = \\mathbf{Fx} + \\mathbf{Bu} & \\boxed{\\mathbf{\\bar x} = f(\\mathbf x, \\mathbf u)} \\\\\n",
"\\mathbf{\\bar P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q & \\mathbf{\\bar P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q \\\\\n",
"\\hline\n",
@ -427,7 +420,7 @@
"This gives us the equalities:\n",
"\n",
"$$\\begin{aligned}\n",
"\\theta &= \\tan^{-1} \\frac y x\\\\\n",
"\\epsilon &= \\tan^{-1} \\frac y x\\\\\n",
"r^2 &= x^2 + y^2\n",
"\\end{aligned}$$ "
]
@ -457,7 +450,7 @@
"\n",
"I've partioned the matrix into blocks to show the upper left block is a constant velocity model for $x$, and the lower right block is a constant position model for $y$.\n",
"\n",
"However, let's practice finding these matrix for a nonlinear system. We model nonlinear systems with a set of differential equations. We need an equation in the form \n",
"However, let's practice finding these matrices. We model systems with a set of differential equations. We need an equation in the form \n",
"\n",
"$$\\dot{\\mathbf x} = \\mathbf{Ax} + \\mathbf{w}$$\n",
"where $\\mathbf{w}$ is the system noise. \n",
@ -928,7 +921,7 @@
"\n",
"We model our system as a nonlinear motion model plus noise.\n",
"\n",
"$$\\bar x = x + f(x, u) + \\mathcal{N}(0, Q)$$\n",
"$$\\bar x = f(x, u) + \\mathcal{N}(0, Q)$$\n",
"\n",
"\n",
"\n",
@ -942,15 +935,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",
@ -1065,9 +1058,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. "