diff --git a/07-Kalman-Filter-Math.ipynb b/07-Kalman-Filter-Math.ipynb index b236104..0342e40 100644 --- a/07-Kalman-Filter-Math.ipynb +++ b/07-Kalman-Filter-Math.ipynb @@ -1049,7 +1049,7 @@ "This allows us to define the covariance of our estimate, which is defined as the expected value of $\\mathbf{ee}^\\mathsf T$:\n", "\n", "$$\\begin{aligned}\n", - "P &= E[\\mathbf e, \\mathbf e^\\mathsf T] \\\\\n", + "P &= E[\\mathbf{ee}^\\mathsf T] \\\\\n", "&= E[(\\mathbf x - \\mathbf{\\hat x})(\\mathbf x - \\mathbf{\\hat x})^\\mathsf T]\n", "\\end{aligned}$$\n", "\n", @@ -1103,11 +1103,11 @@ "\n", "$$\\begin{aligned}\n", "&= (\\mathbf I - \\mathbf{KH})\\mathbf{\\bar P}(\\mathbf I - \\mathbf{KH})^\\mathsf T + \\mathbf{KRK}^\\mathsf T\\\\\n", - "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P} - \\mathbf{\\bar PH}\\mathbf{K}^\\mathsf T + \\mathbf K(\\mathbf{HPH}^\\mathsf T + \\mathbf R)\\mathbf K^\\mathsf T \\\\\n", - "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P} - \\mathbf{\\bar PH}\\mathbf{K}^\\mathsf T + \\mathbf{\\bar P H^\\mathsf T}(\\mathbf{H \\bar P H}^\\mathsf T + \\mathbf R)^{-1}(\\mathbf{HPH}^\\mathsf T + \\mathbf R)\\mathbf K^\\mathsf T\\\\\n", - "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P} - \\mathbf{\\bar PH}\\mathbf{K}^\\mathsf T + \\mathbf{\\bar P H^\\mathsf T}\\mathbf K^\\mathsf T\\\\\n", + "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P} - \\mathbf{\\bar PH}^\\mathsf T\\mathbf{K}^\\mathsf T + \\mathbf K(\\mathbf{H \\bar P H}^\\mathsf T + \\mathbf R)\\mathbf K^\\mathsf T \\\\\n", + "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P} - \\mathbf{\\bar PH}^\\mathsf T\\mathbf{K}^\\mathsf T + \\mathbf{\\bar P H^\\mathsf T}(\\mathbf{H \\bar P H}^\\mathsf T + \\mathbf R)^{-1}(\\mathbf{H \\bar P H}^\\mathsf T + \\mathbf R)\\mathbf K^\\mathsf T\\\\\n", + "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P} - \\mathbf{\\bar PH}^\\mathsf T\\mathbf{K}^\\mathsf T + \\mathbf{\\bar P H^\\mathsf T}\\mathbf K^\\mathsf T\\\\\n", "&= \\mathbf{\\bar P} - \\mathbf{KH}\\mathbf{\\bar P}\\\\\n", - "&= (\\mathbf I - \\mathbf{KH})\\mathbf P\n", + "&= (\\mathbf I - \\mathbf{KH})\\mathbf{\\bar P}\n", "\\end{aligned}$$\n", "\n", "Therefore $\\mathbf P = (\\mathbf I - \\mathbf{KH})\\mathbf{\\bar P}$ is mathematically correct when the gain is optimal, but so is $(\\mathbf I - \\mathbf{KH})\\mathbf{\\bar P}(\\mathbf I - \\mathbf{KH})^\\mathsf T + \\mathbf{KRK}^\\mathsf T$. As we already discussed the latter is also correct when the gain is suboptimal, and it is also more numerically stable. Therefore I use this computation in FilterPy.\n",