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

fixed derivation of EKF propagation equations

Browse Source
This commit is contained in:
Peter Schneider 2017-07-29 01:27:21 -07:00
parent f77e2bf19b
commit 8d5215ffc8
1 changed files with 3 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -382,29 +382,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",