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

changed from \bar to \overbar for prediction.

Browse Source 
More visible, and works better with things like matrices.
This commit is contained in:
Roger Labbe 2016-01-02 18:10:15 -08:00
parent ab33584a0c
commit 122fe27680
1 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
10-Unscented-Kalman-Filter.ipynb
View File

@ -518,7 +518,7 @@
"\n",
"The first two equations are the constraint that the weights must sum to one. The third equation is how you compute a weight mean. The forth equation may be less familiar, but recall that the equation for a covariance is:\n",
"\n",
"$$COV(x,y) = \\frac{\\sum(x-\\bar{x})(y-\\bar{y})}{n}$$\n",
"$$COV(x,y) = \\frac{\\sum(x-\\overline x)(y-\\bar{y})}{n}$$\n",
"\n",
"and you should see where it came from.\n",
"\n",
@ -750,8 +750,8 @@
"$$\\begin{array}{l|l}\n",
"\\mathrm{Kalman} & \\mathrm{Unscented} \\\\\n",
"\\hline \n",
"\\mathbf{\\bar{x}} = \\mathbf{Fx} & \\mathbf{\\bar{\\mu}} = \\sum w^m\\boldsymbol{\\mathcal{Y}} \\\\\n",
"\\mathbf{\\bar{P}} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q & \\sum w^c({\\boldsymbol{\\mathcal{Y}}-\\bf{\\bar{\\mu}})(\\boldsymbol{\\mathcal{Y}}-\\bf{\\bar{\\mu}})^\\mathsf{T}}+\\mathbf Q\n",
"\\mathbf{\\overline x} = \\mathbf{Fx} & \\mathbf{\\bar{\\mu}} = \\sum w^m\\boldsymbol{\\mathcal{Y}} \\\\\n",
"\\mathbf{\\overline P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q & \\sum w^c({\\boldsymbol{\\mathcal{Y}}-\\bf{\\bar{\\mu}})(\\boldsymbol{\\mathcal{Y}}-\\bf{\\bar{\\mu}})^\\mathsf{T}}+\\mathbf Q\n",
"\\end{array}$$\n",
"\n"
]
@ -800,7 +800,7 @@
"\n",
"Finally, we compute the new state estimate using the residual and Kalman gain:\n",
"\n",
"$$\\mathbf x = \\mathbf{\\bar{x}} + \\mathbf{Ky}$$\n",
"$$\\mathbf x = \\mathbf{\\overline x} + \\mathbf{Ky}$$\n",
"\n",
"and the new covariance is computed as:\n",
"\n",
@ -818,17 +818,17 @@
"$$\\begin{array}{l|l}\n",
"\\textrm{Kalman Filter} & \\textrm{Unscented Kalman Filter} \\\\\n",
"\\hline \n",
"\\mathbf{\\bar{x}} = \\mathbf{Fx} & \\mathbf{\\bar{\\mu}} = \\sum w^m\\boldsymbol{\\mathcal{Y}} \\\\\n",
"\\mathbf{\\bar{P}} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q & \\mathbf{\\bar{P}} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q \\\\\n",
"\\mathbf{\\overline x} = \\mathbf{Fx} & \\mathbf{\\bar{\\mu}} = \\sum w^m\\boldsymbol{\\mathcal{Y}} \\\\\n",
"\\mathbf{\\overline P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q & \\mathbf{\\overline P} = \\mathbf{FPF}^\\mathsf{T}+\\mathbf Q \\\\\n",
"\\hline \n",
"\\mathbf{y} = \\boldsymbol{\\mathbf{z}} - \\mathbf{H\\bar{x}} &\n",
"\\mathbf{y} = \\boldsymbol{\\mathbf{z}} - \\mathbf{H\\overline x} &\n",
"\\mathbf{y} = \\mathbf{z} - \\sum w^m h(\\boldsymbol{\\mathcal{Y}})\\\\\n",
"\\mathbf{S} = \\mathbf{H\\bar{P}H}^\\mathsf{T} + \\mathbf R & \n",
"\\mathbf{S} = \\mathbf{H\\overline PH}^\\mathsf{T} + \\mathbf R & \n",
"\\mathbf P_z = \\sum w^c{(\\boldsymbol{\\mathcal{Z}}-\\bar{\\mu})(\\boldsymbol{\\mathcal{Z}}-\\bar{\\mu})^\\mathsf{T}} + \\mathbf R \\\\ \n",
"\\mathbf{K} = \\mathbf{\\bar{P}H}^\\mathsf{T} \\mathbf{S}^{-1} &\n",
"\\mathbf{K} = \\mathbf{\\overline PH}^\\mathsf{T} \\mathbf{S}^{-1} &\n",
"\\mathbf{K} = \\left[\\sum w^c(\\boldsymbol{\\chi}-\\mu)(\\boldsymbol{\\mathcal{Z}}-\\mathbf{\\mu}_z)^\\mathsf{T}\\right] \\mathbf P_z^{-1}\\\\\n",
"\\mathbf x = \\mathbf{\\bar{x}} + \\mathbf{Ky} & \\mathbf x = \\mathbf{\\bar{x}} + \\mathbf{Ky}\\\\\n",
"\\mathbf P = (\\mathbf{I}-\\mathbf{KH})\\mathbf{\\bar{P}} & \\mathbf P = \\mathbf{\\bar{\\Sigma}} - \\mathbf{KP_z}\\mathbf{K}^\\mathsf{T}\n",
"\\mathbf x = \\mathbf{\\overline x} + \\mathbf{Ky} & \\mathbf x = \\mathbf{\\overline x} + \\mathbf{Ky}\\\\\n",
"\\mathbf P = (\\mathbf{I}-\\mathbf{KH})\\mathbf{\\overline P} & \\mathbf P = \\mathbf{\\bar{\\Sigma}} - \\mathbf{KP_z}\\mathbf{K}^\\mathsf{T}\n",
"\\end{array}$$"
]
},
@ -1210,7 +1210,7 @@
"source": [
"Our state transition function is linear \n",
"\n",
"$$\\mathbf{\\bar{x}} = \\begin{bmatrix} 1 & \\Delta t & 0 \\\\ 0& 1& 0 \\\\ 0&0&1\\end{bmatrix}\n",
"$$\\mathbf{\\overline x} = \\begin{bmatrix} 1 & \\Delta t & 0 \\\\ 0& 1& 0 \\\\ 0&0&1\\end{bmatrix}\n",
"\\begin{bmatrix}x \\\\ \\dot x\\\\ y\\end{bmatrix}\n",
"$$\n",
"\n",
@ -1513,7 +1513,7 @@
"\n",
"This requires the following change to the state transition function, which is still linear.\n",
"\n",
"$$\\mathbf{\\bar{x}} = \\begin{bmatrix} 1 & \\Delta t & 0 &0 \\\\ 0& 1& 0 &0\\\\ 0&0&1&\\Delta t \\\\ 0&0&0&1\\end{bmatrix}\n",
"$$\\mathbf{\\overline x} = \\begin{bmatrix} 1 & \\Delta t & 0 &0 \\\\ 0& 1& 0 &0\\\\ 0&0&1&\\Delta t \\\\ 0&0&0&1\\end{bmatrix}\n",
"\\begin{bmatrix}x \\\\\\dot x\\\\ y\\\\ \\dot y\\end{bmatrix} \n",
"$$\n",
"\n",
@ -2400,11 +2400,11 @@
"\n",
"\n",
"$$K = \\mathbf P_{xz} \\mathbf P_z^{-1}\\\\\n",
"{\\mathbf x} = \\mathbf{\\bar{x}} + \\mathbf{Ky}$$\n",
"{\\mathbf x} = \\mathbf{\\overline x} + \\mathbf{Ky}$$\n",
"\n",
"and the new covariance is computed as:\n",
"\n",
"$$ \\mathbf P = \\mathbf{\\bar{P}} - \\mathbf{KP}_z\\mathbf{K}^\\mathsf{T}$$\n",
"$$ \\mathbf P = \\mathbf{\\overline P} - \\mathbf{KP}_z\\mathbf{K}^\\mathsf{T}$$\n",
"\n",
"This function can be implemented as follows, assuming it is a method of a class that stores the necessary matrices and data."
]
@ -2972,7 +2972,7 @@
"\n",
"In general we model our system as a nonlinear motion model plus noise.\n",
"\n",
"$$\\bar{x} = x + f(x, u) + \\mathcal{N}(0, Q)$$\n",
"$$\\overline x = x + f(x, u) + \\mathcal{N}(0, Q)$$\n",
"\n",
"Using the motion model for a robot that we created above, we can write:"
]