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

Fixed ^T to use non serif font.

Browse Source
This commit is contained in:
Roger Labbe 2014-11-24 18:01:59 -08:00
parent 7a343857f0
commit a9537504c6
1 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
07_Kalman_Filter_Math/Kalman_Filter_Math.ipynb
View File

@ -543,13 +543,13 @@
"\\begin{aligned}\n",
"\\text{Predict Step}\\\\\n",
"\\mathbf{x} &= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;\\;&(1) \\\\\n",
"\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;\\;&(2) \\\\\n",
"\\mathbf{P} &= \\mathbf{FP{F}}^\\mathsf{T} + \\mathbf{Q}\\;\\;\\;\\;&(2) \\\\\n",
"\\\\\n",
"\\text{Update Step}\\\\\n",
"\\textbf{y} &= \\mathbf{z} - \\mathbf{H}\\mathbf{x}\\;\\;\\;&(3) \\\\\n",
"\\mathbf{S} &= \\mathbf{HPH}^T + \\mathbf{R} \\;\\;\\;&(4) \\\\\n",
"\\mathbf{K} &= \\mathbf{PH}^T\\mathbf{S}^{-1}\\;\\;\\;&(5) \\\\\n",
"\\mathbf{x} &= \\mathbf{x} +\\mathbf{K}\\mathbf{y} \\;\\;\\;&(6)\\\\\n",
"\\mathbf{S} &= \\mathbf{HPH}^\\mathsf{T} + \\mathbf{R} \\;\\;\\;&(4) \\\\\n",
"\\mathbf{K} &= \\mathbf{PH}^\\mathsf{T}\\mathbf{S}^{-1}\\;\\;\\;&(5) \\\\\n",
"\\mathbf{x} &= \\mathbf{x} +\\mathbf{K}\\mathbf{y} \\;\\;\\;&(6)\\\\\n",
"\\mathbf{P} &= (\\mathbf{I}-\\mathbf{K}\\mathbf{H})\\mathbf{P}\\;\\;\\;&(7)\n",
"\\end{aligned}\n",
"$$\n",
@ -594,8 +594,8 @@
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\mathbf{S} &= \\textbf{HPH}^T + \\textbf{R} \\;\\;\\;&(4) \\\\\n",
"\\textbf{K} &= \\textbf{PH}^T\\mathbf{S}^{-1}\\;\\;\\;&(5) \\\\\n",
"\\mathbf{S} &= \\textbf{HPH}^\\mathsf{T} + \\textbf{R} \\;\\;\\;&(4) \\\\\n",
"\\textbf{K} &= \\textbf{PH}^\\mathsf{T}\\mathbf{S}^{-1}\\;\\;\\;&(5) \\\\\n",
"\\end{aligned}\n",
"$$\n",
"Unfortunately it is a fair amount of linear algebra to derive this. The derivation can be quite elegant, and I urge you to look it up if you have the mathematical education to follow it. But $\\mathbf{K}$ is just the *Kalman gain* - the ratio of how much measurement vs prediction we should use to create the new estimate. $\\mathbf{R}$ is the *measurement noise*, and $\\mathbf{P}$ is our *uncertainty covariance matrix* from the prediction step.\n",
@ -608,7 +608,7 @@
"\n",
"> For example, for the equation of $\\mathbf{S}$ above, Wikipedia uses\n",
"\n",
"> $$\\textbf{S}_k = \\textbf{H}_k \\textbf{P}_{k\\mid k-1} \\textbf{H}_k^\\text{T} + \\textbf{R}_k\n",
"> $$\\textbf{S}_k = \\textbf{H}_k \\textbf{P}_{k\\mid k-1} \\textbf{H}_k^\\mathsf{T} + \\textbf{R}_k\n",
"$$\n",
"\n",
"> Is that more exact? Absolutely. Is it easier or harder to read? You'll need to answer that for yourself.\n",
@ -616,13 +616,13 @@
"> For reference, the Appendix **Symbols and Notations** lists the symbology used by the major authors in the field.\n",
"\n",
"\n",
"So let's work through this expression by expression. Start with $\\mathbf{HPH}^T$. The linear equation $\\mathbf{ABA}^T$ can be thought of as changing the basis of $\\mathbf{B}$ to $\\mathbf{A}$. So $\\mathbf{HPH}^T$ is taking the covariance $\\mathbf{P}$ and putting it in measurement ($\\mathbf{H}$) space. \n",
"So let's work through this expression by expression. Start with $\\mathbf{HPH}^\\mathsf{T}$. The linear equation $\\mathbf{ABA}^T$ can be thought of as changing the basis of $\\mathbf{B}$ to $\\mathbf{A}$. So $\\mathbf{HPH}^\\mathsf{T}$ is taking the covariance $\\mathbf{P}$ and putting it in measurement ($\\mathbf{H}$) space. \n",
"\n",
"In English, consider the problem of reading a temperature with a thermometer that provices readings in volts. Our state is in terms of temperature, but we are now doing calculations in *measurement space* - volts. So we need to convert $\\mathbf{P}$ from applying to temperatures to volts. The linear algebra form $\\textbf{H}\\textbf{P}\\textbf{H}$ takes $\\mathbf{P}$ to the basis used by $\\mathbf{H}$, namely volts. \n",
"\n",
"Then, once in measurement space, we can add the measurement noise $\\mathbf{R}$ to it. Hence, the expression for the uncertainty once we include the measurement is:\n",
"\n",
"$$\\mathbf{S} = \\mathbf{HP}\\mathbf{H}^T + \\mathbf{R}$$"
"$$\\mathbf{S} = \\mathbf{HP}\\mathbf{H}^\\mathsf{T} + \\mathbf{R}$$"
]
},
{
@ -637,7 +637,7 @@
"\n",
"We can think of the inverse of a matrix as linear algebra's way ofcomputing $\\frac{1}{x}$. So we can read the equation for $\\textbf{K}$ as\n",
"\n",
"$$ \\textbf{K} = \\frac{\\textbf{P}\\textbf{H}^T}{\\mathbf{S}} $$\n",
"$$ \\textbf{K} = \\frac{\\textbf{P}\\textbf{H}^\\mathsf{T}}{\\mathbf{S}} $$\n",
"\n",
"\n",
"$$\n",
@ -682,9 +682,9 @@
"This is just our state transition equation which we have already discussed. $\\mathbf{Fx}$ multiplies $\\mathbf{x}$ with the state transition matrix to compute the next state. $B$ and $u$ add in the contribution of the control input $\\mathbf{u}$, if any.\n",
"\n",
"The final equation is:\n",
"$$\\mathbf{P} = \\mathbf{FPF}^T + \\mathbf{Q}\\tag{2}$$\n",
"$$\\mathbf{P} = \\mathbf{FPF}^\\mathsf{T} + \\mathbf{Q}\\tag{2}$$\n",
"\n",
"$\\mathbf{FPF}^T$ is the way we put $\\mathbf{P}$ into the process space using linear algebra so that we can add in the process noise $\\mathbf{Q}$ to it."
"$\\mathbf{FPF}^\\mathsf{T}$ is the way we put $\\mathbf{P}$ into the process space using linear algebra so that we can add in the process noise $\\mathbf{Q}$ to it."
]
},
{
@ -724,7 +724,7 @@
"\n",
"Since the noise is changing continuously we will need to integrate to get the discrete noise for the discretization interval that we have chosen. We will not prove it here, but the equation for discretizing the noise is\n",
"\n",
"$$\\mathbf{Q} = \\int_0^{\\Delta t} \\Phi(t)\\mathbf{Q_c}\\Phi^T(t) dt$$\n",
"$$\\mathbf{Q} = \\int_0^{\\Delta t} \\Phi(t)\\mathbf{Q_c}\\Phi^\\mathsf{T}(t) dt$$\n",
"\n",
"where $\\mathbf{Q_c}$ is the continuous noise. This gives us\n",
"\n",