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

Merge pull request #90 from Gluttton/master

Browse Source 
Unscented Kalman Filter: using "state sigmas" instead of "prior sigmas" in the cross-covariance equation at the predict step
This commit is contained in:
Roger Labbe 2016-03-15 17:00:54 -07:00
commit 38996b6d24
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6154,7 +6154,7 @@
"\n",
"To compute the Kalman gain we first compute the [cross covariance](https://en.wikipedia.org/wiki/Cross-covariance) of the state and the measurements, which is defined as: \n",
"\n",
"$$\\mathbf P_{xz} =\\sum_{i=0}^{2n} w^c_i(\\boldsymbol\\chi_i-\\mathbf{\\bar x})(\\boldsymbol{\\mathcal Z}_i-\\boldsymbol\\mu_z)^\\mathsf T$$\n",
"$$\\mathbf P_{xz} =\\sum_{i=0}^{2n} w^c_i(\\boldsymbol{\\mathcal Y}_i-\\mathbf{\\bar x})(\\boldsymbol{\\mathcal Z}_i-\\boldsymbol\\mu_z)^\\mathsf T$$\n",
"\n",
"And then the Kalman gain is defined as\n",
"\n",
@ -6193,7 +6193,7 @@
"\\mathbf S = \\mathbf{H\\bar PH}^\\mathsf{T} + \\mathbf R & \n",
"\\mathbf P_z = \\sum w^c{(\\boldsymbol{\\mathcal Z}-\\boldsymbol\\mu_z)(\\boldsymbol{\\mathcal{Z}}-\\boldsymbol\\mu_z)^\\mathsf{T}} + \\mathbf R \\\\ \n",
"\\mathbf K = \\mathbf{\\bar PH}^\\mathsf T \\mathbf S^{-1} &\n",
"\\mathbf K = \\left[\\sum w^c(\\boldsymbol\\chi-\\bar{\\mathbf x})(\\boldsymbol{\\mathcal{Z}}-\\boldsymbol\\mu_z)^\\mathsf{T}\\right] \\mathbf P_z^{-1} \\\\\n",
"\\mathbf K = \\left[\\sum w^c(\\boldsymbol{\\mathcal Y}-\\bar{\\mathbf x})(\\boldsymbol{\\mathcal{Z}}-\\boldsymbol\\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 = \\bar{\\mathbf P} - \\mathbf{KP_z}\\mathbf{K}^\\mathsf{T}\n",
"\\end{array}$$"
@ -20805,7 +20805,7 @@
"\n",
"The mean and covariance of those points is computed with the unscented transform. The residual and Kalman gain is then computed. The cross variance is computed as:\n",
"\n",
"$$\\mathbf P_{xz} =\\sum_{i=0}^{2n} w^c_i(\\boldsymbol{\\chi}_i-\\mu)(\\boldsymbol{\\mathcal Z}_i-\\mu_z)^\\mathsf T$$\n",
"$$\\mathbf P_{xz} =\\sum_{i=0}^{2n} w^c_i(\\boldsymbol{\\mathcal Y}_i-\\mu)(\\boldsymbol{\\mathcal Z}_i-\\mu_z)^\\mathsf T$$\n",
"\n",
"Finally, we compute the new state estimate using the residual and Kalman gain:\n",
"\n",