diff --git a/10-Unscented-Kalman-Filter.ipynb b/10-Unscented-Kalman-Filter.ipynb index 0755b4e..a2c82a3 100644 --- a/10-Unscented-Kalman-Filter.ipynb +++ b/10-Unscented-Kalman-Filter.ipynb @@ -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:" ]