From a84cac57858c08f3b7b26842a1baa5cc4a89b8b5 Mon Sep 17 00:00:00 2001 From: Roger Labbe Date: Fri, 12 Sep 2014 23:58:53 -0700 Subject: [PATCH] Standardizing nomenclature. Got rid of the raised - for prediction; looks too much like subtraction for my taste. --- .../Multivariate_Kalman_Filters.ipynb | 101 +++++++++--------- 1 file changed, 51 insertions(+), 50 deletions(-) diff --git a/Chapter06_Multivariate_Kalman_Filter/Multivariate_Kalman_Filters.ipynb b/Chapter06_Multivariate_Kalman_Filter/Multivariate_Kalman_Filters.ipynb index 8782101..0b64c32 100644 --- a/Chapter06_Multivariate_Kalman_Filter/Multivariate_Kalman_Filters.ipynb +++ b/Chapter06_Multivariate_Kalman_Filter/Multivariate_Kalman_Filters.ipynb @@ -1,7 +1,7 @@ { "metadata": { "name": "", - "signature": "sha256:ab930a5e6755e41b88e1cd30e7e98fd569029d7ac664de4c489ca9f5d577e4a8" + "signature": "sha256:26bc5ef18b3883ff467385caf9e41a591415f9534c795760311bf2410b0e36c1" }, "nbformat": 3, "nbformat_minor": 0, @@ -259,7 +259,7 @@ "output_type": "pyout", "prompt_number": 1, "text": [ - "" + "" ] } ], @@ -846,18 +846,16 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "The chart shows a prior estimate of $\\hat{x}=2$ and a velocity $\\dot{x}=1$. \n", + "The chart shows a prior estimate of $x=2$ and a velocity $\\dot{x}=1$. \n", "\n", - "> We use the caret notation $\\hat{x}$ to denote an *estimate* - the output of the filter. We use the familar dot notation $\\dot{x}$ to denote the derivative of x.\n", + "> We use the familar dot notation $\\dot{x}$ to denote the derivative of x.\n", "\n", "\n", - "Therefore we predict $x^-= 2+1 = 3$.\n", + "Therefore we predict $x= 2+1 = 3$.\n", "\n", - "> We use the notation $x^-$ to denote a *prediction*.\n", + "However, the new measurement is $z=2.3$, giving a residual $r=0.7$. Finally, the Kalman filter gain $K$ gives us a new estimate of $x=2.8$.\n", "\n", - "However, the new measurement is $x^*=2.3$, giving a residual $r=0.7$. Finally, the Kalman filter gain $K$ gives us a new estimate of $\\hat{x}=2.8$.\n", - "\n", - "> We use the symbolology $x^*$ to denote a measurement. I will address symbology in more detail later. It is an unfortunate reality that nearly every text on Kalman filtering uses different symbology and variables - there is almost no agreement across texts. Be sure to read the introductory material very carefully to avoid being led astray.\n" + "> We use the notation $z$ to denote a measurement. I will address notation in more detail later. It is an unfortunate reality that nearly every text on Kalman filtering uses different notation and variables - there is almost no agreement across texts. Be sure to read the introductory material very carefully to avoid being led astray." ] }, { @@ -928,7 +926,7 @@ "\n", "Different texts use different notation and variable names for the Kalman filter. Later we will expose you to these different forms to prepare you for reading the original literature. In the equations above I have adopted the variable names used by the Wikipedia article[2] on Kalman filters. Each bold letter denotes a matrix or vector. The subscripts indicate which time step the data comes from; $k$ is now, $k+1$ is the next step. The caret (^) indicates that the value is an estimate. Finally, I particularly like how Brown [3] uses a raised $^-$ to denote a prediction, and so I have adopted that approach. For a matrix $\\mathbf{A}$, $\\mathbf{A}^T$ signifies its transpose, and $\\mathbf{A}^{-1}$ its inverse. So, taken together, $\\hat{\\mathbf{x}}^-_{k+1}$ represents the prediction for the estimate of $\\mathbf{x}$ at time step $k+1$, where $\\mathbf{x}$ is some vector in the form $\\mathbf{x} = \\begin{bmatrix}x_1&x_2&..&x_n\\end{bmatrix}^T.$ The notation does not specify that $\\mathbf{x}$ is a column vector - we will learn the shapes and sizes of all of the variables later in the chapter.\n", "\n", - "**author's note: do we really want to explain notation here?**\n" + "**author's note: do we really want to explain notation here?**" ] }, { @@ -943,19 +941,19 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "However, I still find the notation to be a bit dense, and unnecessarily complicated for writing code. The subscripts indicate the time step, but when we right code it is very clear what is being calculated at each time step. For most of this book I'm going to use the following simplified equations, which express an algorithm.\n", + "However, I still find the notation to be a bit dense, and unnecessarily complicated for writing code. The subscripts indicate the time step, but when we write code it is very clear what is being calculated at each time step. For most of this book I'm going to use the following simplified equations, which express an algorithm.\n", "\n", "$$\n", "\\begin{aligned}\n", "\\text{Predict Step}\\\\\n", - "\\mathbf{x}^- &:= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;&(1) \\\\\n", + "\\mathbf{x} &:= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;&(1) \\\\\n", "\\mathbf{P } &:= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;&(2) \\\\\n", "\\\\\n", "\\text{Update Step}\\\\\n", - "\\textbf{y} &:= \\mathbf{z} - \\mathbf{H x^-} \\;\\;\\;&(3)\\\\\n", + "\\textbf{y} &:= \\mathbf{z} - \\mathbf{H x} \\;\\;\\;&(3)\\\\\n", "\\textbf{S} &:= \\mathbf{HPH}^T + \\mathbf{R} \\;\\;\\;&(4)\\\\\n", "\\mathbf{K} &:= \\mathbf{PH}^T \\mathbf{S}^{-1}\\;\\;\\;&(5) \\\\\n", - "\\mathbf{x}&:=\\mathbf{x}^- +\\mathbf{K\\textbf{y}} \\;\\;\\;&(6)\\\\\n", + "\\mathbf{x}&:=\\mathbf{x} +\\mathbf{K\\textbf{y}} \\;\\;\\;&(6)\\\\\n", "\\mathbf{P}&:= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;&(7)\n", "\\end{aligned}\n", "$$" @@ -984,7 +982,7 @@ "source": [ "Before we go any further let's gain some familiarity with the equations by programming them in Python. I have written a production quality implementation of the Kalman filter equations in my `filterpy` library, and we will be using that later in the chapter and the remainder of the book. We could just look at that code, but it contains a significant amount of code to ensure that the computations are numerically stable, that you do not pass in bad data, and so on. Let's just try to program this.\n", "\n", - "The filter equations are *linear algebra* equations, so we will use the Python library that implements linear algebra - `numpy`. In the filter equations a **bold** variable denotes a matrix. Numpy provides two types to implement matrices: `numpy.array` and `numpy.matrix`. You might suspect that the latter is the one we want to use. As it turns out `numpy.matrix` does support linear algebra well, except for one problem - most of the rest of `numpy` uses `numpy.array`, not `numpy.matrix`. You can pass a `numpy.matrix` into a function, and get a `numpy.array` back as a result. Hence, the standard advice is that `numpy.matrix` is deprecated, and you should always use `numpy.array` even when `numpy.matrix` is more convienient. I ignored this advice in a early version of this code and ended up regretting that choice, and so now I use 'numpy.array' only.\n", + "The filter equations are *linear algebra* equations, so we will use the Python library that implements linear algebra - numpy. In the filter equations a **bold** variable denotes a matrix. Numpy provides two types to implement matrices: `numpy.array` and `numpy.matrix`. You might suspect that the latter is the one we want to use. As it turns out `numpy.matrix` does support linear algebra well, except for one problem - most of the rest of `numpy` uses `numpy.array`, not `numpy.matrix`. You can pass a `numpy.matrix` into a function, and get a `numpy.array` back as a result. Hence, the standard advice is that `numpy.matrix` is deprecated, and you should always use `numpy.array` even when `numpy.matrix` is more convienient. I ignored this advice in a early version of this code and ended up regretting that choice, and so now I use 'numpy.array' only.\n", "\n", "`numpy.array` implements a any-dimensional array. You can construct it with any list like object. The following constructs a 1-D array from a list:" ] @@ -1083,33 +1081,25 @@ "input": [ "x = np.array([[1,2],[3,4]], dtype=float)\n", "print('addition:\\n', x+x)\n", - "print('element-wise multiplication\\n', x*x)\n", - "print('multiplication\\n', np.dot(x,x))\n", - "print('dot is also a member\\n', x.dot(x))" + "print('\\nelement-wise multiplication\\n', x*x)\n", + "print('\\nmultiplication\\n', np.dot(x,x))\n", + "print('\\ndot is also a member\\n', x.dot(x))" ], "language": "python", "metadata": {}, "outputs": [ { - "output_type": "stream", - "stream": "stdout", - "text": [ - "addition:\n", - " [[ 2. 4.]\n", - " [ 6. 8.]]\n", - "element-wise multiplication\n", - " [[ 1. 4.]\n", - " [ 9. 16.]]\n", - "multiplication\n", - " [[ 7. 10.]\n", - " [ 15. 22.]]\n", - "dot is also a member\n", - " [[ 7. 10.]\n", - " [ 15. 22.]]\n" + "ename": "NameError", + "evalue": "name 'np' is not defined", + "output_type": "pyerr", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m\n\u001b[1;31mNameError\u001b[0m Traceback (most recent call last)", + "\u001b[1;32m\u001b[0m in \u001b[0;36m\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[0mx\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0marray\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m1\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m3\u001b[0m\u001b[1;33m,\u001b[0m\u001b[1;36m4\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mdtype\u001b[0m\u001b[1;33m=\u001b[0m\u001b[0mfloat\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m 2\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'addition:\\n'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mx\u001b[0m\u001b[1;33m+\u001b[0m\u001b[0mx\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'\\nelement-wise multiplication\\n'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mx\u001b[0m\u001b[1;33m*\u001b[0m\u001b[0mx\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 4\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'\\nmultiplication\\n'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mnp\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mdot\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mx\u001b[0m\u001b[1;33m,\u001b[0m\u001b[0mx\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 5\u001b[0m \u001b[0mprint\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'\\ndot is also a member\\n'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mx\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mdot\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mx\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n", + "\u001b[1;31mNameError\u001b[0m: name 'np' is not defined" ] } ], - "prompt_number": 19 + "prompt_number": 2 }, { "cell_type": "markdown", @@ -1123,7 +1113,7 @@ "collapsed": false, "input": [ "print('transpose\\n', x.T)\n", - "print('inverse\\n', np.linalg.inv(x))" + "print('\\ninverse\\n', np.linalg.inv(x))" ], "language": "python", "metadata": {}, @@ -1186,16 +1176,15 @@ "\n", "$$\n", "\\begin{aligned}\n", - "\\mathbf{x}^- &:= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;&(1) \\\\\n", - "\\mathbf{P } &:= \\mathbf{FP{F}}^T + \\mathbf{Q}\\;\\;\\;&(2) \\\\\n", + "\\mathbf{x} &:= \\mathbf{F x} + \\mathbf{B u}\\;\\;\\;&(1) \\\\\n", + "\\mathbf{P} &:= \\mathbf{FPF}^T + \\mathbf{Q}\\;\\;\\;&(2) \\\\\n", "\\end{aligned}\n", "$$\n", "\n", - "Those are linear algebra equations using matrix multiplication and addition. Assuming each variable is already defined somewhere, we could write:\n", + "Those are linear algebra equations using matrix multiplication and addition. Assuming each variable is already defined somewhere, we can implement these equations in Python and numpy with:\n", "\n", - "\n", - " x = dot(F,x) + dot(B,u)\n", - " P = F.dot(P).dot(F.T) + Q\n", + " x = dot(F, x) + dot(B, u)\n", + " P = dot(F, P).dot(F.T) + Q\n", "\n", "That is all there is to it! Okay, we need to put these in a function or a class somehow, but that is the 'hard' code, which is actually pretty easy.\n", "\n", @@ -1203,10 +1192,10 @@ "\n", "$$\n", "\\begin{aligned}\n", - "\\textbf{y} &:= \\mathbf{z} - \\mathbf{H x^-} \\;\\;\\;&(3)\\\\\n", + "\\textbf{y} &:= \\mathbf{z} - \\mathbf{H x} \\;\\;\\;&(3)\\\\\n", "\\textbf{S} &:= \\mathbf{HPH}^T + \\mathbf{R} \\;\\;\\;&(4)\\\\\n", "\\mathbf{K} &:= \\mathbf{PH}^T \\mathbf{S}^{-1}\\;\\;\\;&(5) \\\\\n", - "\\mathbf{x}&:=\\mathbf{x}^- +\\mathbf{K\\textbf{y}} \\;\\;\\;&(6)\\\\\n", + "\\mathbf{x}&:=\\mathbf{x} +\\mathbf{K\\textbf{y}} \\;\\;\\;&(6)\\\\\n", "\\mathbf{P}&:= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;&(7)\n", "\\end{aligned}\n", "$$\n", @@ -1215,8 +1204,8 @@ "In Python we would write:\n", "\n", " y = z - dot(H, x)\n", - " S = H.dot(P).dot(H.T) + R\n", - " K = P.dot(H.T).dot(np.linalg.inv(S))\n", + " S = dot(H, P).dot(H.T) + R\n", + " K = dot(P, H.T).dot(np.linalg.inv(S))\n", " x = x + dot(K,y)\n", " P = (I - dot(K, H)).dot(P)\n", " \n", @@ -1303,7 +1292,7 @@ " y = Z - dot(H, x)\n", " \n", " # project system uncertainty into measurement space \n", - " S = dot(H,P).dot(H.T) + R\n", + " S = dot(H, P).dot(H.T) + R\n", "\n", " # map system uncertainty into kalman gain\n", " K = dot(P, H.T).dot(linalg.inv(S))\n", @@ -1314,6 +1303,7 @@ " I_KH = self._I - dot (K, H)\n", " self.P = dot(I_KH).dot(P).dot(I_KH.T) + dot(K, R).dot(K.T)\n", "\n", + "\n", " def predict(self, u=0):\n", " \"\"\" Predict next position.\n", " Parameters\n", @@ -1323,7 +1313,7 @@ " to create the control input into the system.\n", " \"\"\"\n", " \n", - " self.x = np.dot(self.F, self.x) + np.dot(self.B, u)\n", + " self.x = dot(self.F, self.x) + dot(self.B, u)\n", " self.P = self.F.dot(self.P).dot(self.F.T) + self.Q" ], "language": "python", @@ -1341,7 +1331,7 @@ " \n", "and the class will recognize that $R$ is actually supposed to be a matrix and convert the $3$ into an appropriate matrix (we don't yet know what an 'appropriate' matrix for $R=3$ would be, but we will learn that soon).\n", "\n", - "You can import the class using the following Python:" + "You can import the class from `filterpy` using the following import statement, and that is what we will do in the rest of this chapter and book:" ] }, { @@ -1352,8 +1342,19 @@ ], "language": "python", "metadata": {}, - "outputs": [], - "prompt_number": 23 + "outputs": [ + { + "ename": "ImportError", + "evalue": "No module named 'filterpy'", + "output_type": "pyerr", + "traceback": [ + "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m\n\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)", + "\u001b[1;32m\u001b[0m in \u001b[0;36m\u001b[1;34m()\u001b[0m\n\u001b[1;32m----> 1\u001b[1;33m \u001b[1;32mfrom\u001b[0m \u001b[0mfilterpy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mkalman\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mKalmanFilter\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m", + "\u001b[1;31mImportError\u001b[0m: No module named 'filterpy'" + ] + } + ], + "prompt_number": 1 }, { "cell_type": "heading",