diff --git a/Multidimensional_Kalman_Filters.ipynb b/Multidimensional_Kalman_Filters.ipynb index ff215d6..f33cca4 100644 --- a/Multidimensional_Kalman_Filters.ipynb +++ b/Multidimensional_Kalman_Filters.ipynb @@ -1,7 +1,7 @@ { "metadata": { "name": "", - "signature": "sha256:b02cf3016d12a2b3dbf885298a5c67f74ad1fe1b799eebfb3d479a95d4f2362e" + "signature": "sha256:d8ea908d5b59d9a8c4f1a078ade573323053af449dbaae8931bf4ed5b0f2ed5e" }, "nbformat": 3, "nbformat_minor": 0, @@ -248,13 +248,13 @@ ], "metadata": {}, "output_type": "pyout", - "prompt_number": 1, + "prompt_number": 10, "text": [ - "" + "" ] } ], - "prompt_number": 1 + "prompt_number": 10 }, { "cell_type": "heading", @@ -918,9 +918,10 @@ "\\\\\n", "\\text{Update Step}\\\\\n", "\\textbf{y} &:= \\mathbf{z} - \\mathbf{H x^-} \\;\\;\\;&(3)\\\\\n", - "\\mathbf{K}&:= \\mathbf{PH}^T (\\mathbf{HPH}^T + \\mathbf{R})^{-1}\\;\\;\\;&(4) \\\\\n", - "\\mathbf{x}&:=\\mathbf{x}^- +\\mathbf{K\\textbf{y}} \\;\\;\\;&(5)\\\\\n", - "\\mathbf{P}&:= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;&(6)\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{P}&:= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;&(7)\n", "\\end{aligned}\n", "$$" ] @@ -931,19 +932,394 @@ "source": [ "This is an algorithm, so $:=$ denotes assignment, not equality. For example, equation (6) has P on both sides of the $:=$. This equation updates the value of P by the computation on the right hand side; it does not imply that the two sides of the equation are equal (they are not). \n", "\n", - "What do all of the variables mean? What is $\\mathbf{P}$, for example? Don't worry right now. Instead, I am just going to design a Kalman filter, and introduce the names as we go. Then we will just pass them into Python function that implement the equations above, and we will have our solution. Later sections will then delve into more detail about each step and equation. I think learning by example and practice is far easier than trying to memorize a dozen abstract facts at once. \n", - "\n", - "Look at the code below for the predict step (which we will present a bit later). \n", - "\n", - " def predict():\n", - " x = F*x + B*u # equation (1)\n", - " P = F*P*F.T + Q # equation (2)\n", - " \n", - "Notice how simple it really is. It really isn't much different from the predict step in the previous chapter, and it is a nearly exact transliteration of the equations above. As you become familiar with this notation you will find yourself able to read textbooks and paper and implement the equations without much difficulty. \n", - " \n", - "> Later, if you become interested in the details of numerical computation you may change the implementation to be faster or more numerically stable than this written form, but for most of this book our code will follow the Kalman filter equations almost exactly. " + "What do all of the variables mean? What is $\\mathbf{P}$, for example? Don't worry right now. Instead, I am just going to design a Kalman filter, and introduce the names as we go. Then we will just pass them into Python function that implement the equations above, and we will have our solution. Later sections will then delve into more detail about each step and equation. I think learning by example and practice is far easier than trying to memorize a dozen abstract facts at once. " ] }, + { + "cell_type": "heading", + "level": 2, + "metadata": {}, + "source": [ + "Implementation in Python" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Before we go any further let's gain some familarity 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 convienent. 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:" + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "import numpy as np\n", + "x = np.array([1,2,3])\n", + "print(x)\n", + "print(type(x))" + ], + "language": "python", + "metadata": {}, + "outputs": [ + { + "output_type": "stream", + "stream": "stdout", + "text": [ + "[1 2 3]\n", + "\n" + ] + } + ], + "prompt_number": 11 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You can create a 2D array with nested lists:" + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "x = np.array([[1,2,3],\n", + " [4,5,6]])\n", + "print(x)" + ], + "language": "python", + "metadata": {}, + "outputs": [ + { + "output_type": "stream", + "stream": "stdout", + "text": [ + "[[1 2 3]\n", + " [4 5 6]]\n" + ] + } + ], + "prompt_number": 12 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You can create arrays of 3 or more dimensions, but we have no need for that here, and so I will not elaborate.\n", + "\n", + "By default the arrays use the data type of the values in the list; if there are multiple types than it will choose the type that most accurately represents all the values. So, for example, if your list contains a mix of `int` and `float` the data type of the array would be of type `float`. You can override this with the `dtype` parameter." + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "x = np.array([1,2,3],dtype=float)\n", + "print(x)" + ], + "language": "python", + "metadata": {}, + "outputs": [ + { + "output_type": "stream", + "stream": "stdout", + "text": [ + "[ 1. 2. 3.]\n" + ] + } + ], + "prompt_number": 13 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You can perform matrix addition with the `+` operator, but matrix multiplication requires the `dot` method or function. The `*` operator performs elementwise multiplication, which is **not** what you want for linear algebra." + ] + }, + { + "cell_type": "code", + "collapsed": false, + "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))" + ], + "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" + ] + } + ], + "prompt_number": 18 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "You can get the transpose with `.T`, and the inverse with `numpy.linalg.inv`." + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "print('transpose\\n', x.T)\n", + "print('inverse\\n', np.linalg.inv(x))" + ], + "language": "python", + "metadata": {}, + "outputs": [ + { + "output_type": "stream", + "stream": "stdout", + "text": [ + "transpose\n", + " [[ 1. 3.]\n", + " [ 2. 4.]]\n", + "inverse\n", + " [[-2. 1. ]\n", + " [ 1.5 -0.5]]\n" + ] + } + ], + "prompt_number": 19 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, there are helper functions like `zeros` to create a matrix of all zeros, `ones` to get all ones, and `eye` to get the identity matrix." + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "print('zeros\\n', np.zeros((3,2)))\n", + "print('\\neye\\n', np.eye(3))" + ], + "language": "python", + "metadata": {}, + "outputs": [ + { + "output_type": "stream", + "stream": "stdout", + "text": [ + "zeros\n", + " [[ 0. 0.]\n", + " [ 0. 0.]\n", + " [ 0. 0.]]\n", + "\n", + "eye\n", + " [[ 1. 0. 0.]\n", + " [ 0. 1. 0.]\n", + " [ 0. 0. 1.]]\n" + ] + } + ], + "prompt_number": 24 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "There is a lot of useful functionality in `numpy`, but let's move on to implementing the Kalman filter. Let's start with the prediction equations.\n", + "\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", + "\\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", + "\n", + "\n", + " x = dot(F,x) + dot(B,u)\n", + " P = F.dot(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", + "Now let's do the update step. Again, they consist of matrix multiplication and addition.\n", + "\n", + "$$\n", + "\\begin{aligned}\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{P}&:= (\\mathbf{I}-\\mathbf{KH})\\mathbf{P}\\;\\;\\;&(7)\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "\n", + "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", + " x = x + dot(K,y)\n", + " P = (I - dot(K, H)).dot(P)\n", + " \n", + "And that is it, we have implemented a Kalman filter!\n", + "\n", + "Well, you probably do not want to cut and paste that code into every project that uses a Kalman filter. So let's put this into a class. I don't intend to teach you how to program here, so I will instead point you to my `KalmanFilter` class from my filterpy module, available on github [4]. However, I have written a simplified version of that class below for your inspection." + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "import numpy as np\n", + "import scipy.linalg as linalg\n", + "import matplotlib.pyplot as plt\n", + "import numpy.random as random\n", + "from numpy import dot\n", + "\n", + "class KalmanFilter:\n", + "\n", + " def __init__(self, dim_x, dim_z, dim_u=0):\n", + " \"\"\" Create a Kalman filter. You are responsible for setting the\n", + " various state variables to reasonable values; the defaults below will\n", + " not give you a functional filter.\n", + "\n", + " Parameters\n", + " ----------\n", + " dim_x : int\n", + " Number of state variables for the Kalman filter. For example, if\n", + " you are tracking the position and velocity of an object in two\n", + " dimensions, dim_x would be 4.\n", + "\n", + " This is used to set the default size of P, Q, and u\n", + "\n", + " dim_z : int\n", + " Number of of measurement inputs. For example, if the sensor\n", + " provides you with position in (x,y), dim_z would be 2.\n", + "\n", + " dim_u : int (optional)\n", + " size of the control input, if it is being used.\n", + " Default value of 0 indicates it is not used.\n", + " \"\"\"\n", + "\n", + " self.x = np.zeros((dim_x,1)) # state\n", + " self.P = np.eye(dim_x) # uncertainty covariance\n", + " self.Q = np.eye(dim_x) # process uncertainty\n", + " self.u = np.zeros((dim_x,1)) # motion vector\n", + " self.B = 0 # control transition matrix\n", + " self.F = 0 # state transition matrix\n", + " self.H = 0 # Measurement function\n", + " self.R = np.eye(dim_z) # state uncertainty\n", + " \n", + " # identity matrix. Do not alter this. \n", + " self._I = np.eye(dim_x)\n", + " \n", + " if use_short_form:\n", + " self.update = self.update_short_form\n", + "\n", + "\n", + " def update(self, Z, R=None):\n", + " \"\"\"\n", + " Add a new measurement (Z) to the kalman filter. If Z is None, nothing\n", + " is changed.\n", + "\n", + " Parameters\n", + " ----------\n", + " Z : np.array\n", + " measurement for this update.\n", + "\n", + " R : np.array, scalar, or None\n", + " Optionally provide R to override the measurement noise for this\n", + " one call, otherwise self.R will be used.\n", + " \"\"\"\n", + "\n", + " if Z is None:\n", + " return\n", + " \n", + " if R is None:\n", + " R = self.R\n", + " elif np.isscalar(R):\n", + " R = np.eye(self.dim_z) * R\n", + "\n", + " # error (residual) between measurement and prediction\n", + " y = Z - dot(H, x)\n", + " \n", + " # project system uncertainty into measurement space \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", + "\n", + " # predict new x with residual scaled by the kalman gain\n", + " self.x = self.x + dot(K, y)\n", + "\n", + " 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", + " def predict(self, u=0):\n", + " \"\"\" Predict next position.\n", + " Parameters\n", + " ----------\n", + " u : np.array\n", + " Optional control vector. If non-zero, it is multiplied by B\n", + " 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.P = self.F.dot(self.P).dot(self.F.T) + self.Q" + ], + "language": "python", + "metadata": {}, + "outputs": [], + "prompt_number": 27 + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "We will see how to use this class in the rest of the chapter, so I will not belabor its use here. There are several additions to the version in filterpy that make it more usable. For example, instead of using variables for the $R$, $P$, and so on, the `filterpy` version uses properties. This allows you to write something like:\n", + "\n", + " dog_filter.R = 3\n", + " \n", + "and the class will recognize that $R$ is actually supposed to be a matrix and conver 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:" + ] + }, + { + "cell_type": "code", + "collapsed": false, + "input": [ + "from filterpy.kalman import KalmanFilter" + ], + "language": "python", + "metadata": {}, + "outputs": [], + "prompt_number": 25 + }, { "cell_type": "heading", "level": 2, @@ -1231,7 +1607,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "As promised, the Kalman filter equations are already programmed for you. In many circumstances you will never have to write your own Kalman filter equations. We will look at the code later, but for now we will just import the code and use it. It lives in `filterpy.kalman` so let's start by importing it and creating a filter." + "As we already explained, the Kalman filter equations are already implemented for you in the `filterpy` library, so let's start by importing it and creating a filter." ] }, { @@ -1239,13 +1615,14 @@ "collapsed": false, "input": [ "import numpy as np\n", - "import filterpy.kalman as kf\n", - "dog_filter = kf.KalmanFilter (dim_x=2, dim_z=1)" + "from filterpy.kalman import KalmanFilter\n", + "\n", + "dog_filter = KalmanFilter (dim_x=2, dim_z=1)" ], "language": "python", "metadata": {}, "outputs": [], - "prompt_number": 16 + "prompt_number": 31 }, { "cell_type": "markdown", @@ -1270,7 +1647,7 @@ "language": "python", "metadata": {}, "outputs": [], - "prompt_number": 17 + "prompt_number": 32 }, { "cell_type": "markdown", @@ -1312,10 +1689,10 @@ "input": [ "import numpy as np\n", "from DogSensor import DogSensor\n", - "import filterpy.kalman as kf\n", + "from filterpy.kalman import KalmanFilter\n", "\n", "def dog_tracking_filter(R,Q=0,cov=1.):\n", - " dog_filter = kf.KalmanFilter (dim_x=2, dim_z=1)\n", + " dog_filter = KalmanFilter (dim_x=2, dim_z=1)\n", " dog_filter.x = np.array([[0], \n", " [0]]) # initial state (location and velocity)\n", " dog_filter.F = np.array([[1,1],\n", @@ -1366,7 +1743,7 @@ "language": "python", "metadata": {}, "outputs": [], - "prompt_number": 18 + "prompt_number": 33 }, { "cell_type": "markdown", @@ -1436,7 +1813,7 @@ "language": "python", "metadata": {}, "outputs": [], - "prompt_number": 19 + "prompt_number": 34 }, { "cell_type": "markdown", @@ -1491,306 +1868,31 @@ "level": 2, "metadata": {}, "source": [ - "Walking Through the KalmanFilter Code (Optional)" + "Compare to Univariate Kalman Filter" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ - "The kalman filter code that we are using is implemented in my Python library `filterpy`. If you are interested in the full implmentation of the filter you should look in `filterpy\\kalman\\kalman_filter.py`. In the following I will present a simplified implementation of the same code. The code in the library handles issues that are beyond the scope of this chapter, such as numerical stability and support for the extended Kalman filter, subject of a later chapter. \n", - "\n", - "The code is implemented as the class `KalmanFilter`. Some Python programmers are not a fan of object oriented (OO) Python, and eschew classes. I do not intend to enter into that battle other than to say that I have often seen OO abused. Here I use the class to encapsulate the data that is pertinent to the filter so that you do not have to store and pass around a half dozen variables everywhere.\n", - "\n", - "The method `__init__()` is used by Python to create the object. Here is the method \n", - "\n", - " def __init__(self, dim_x, dim_z):\n", - " \"\"\" Create a Kalman filter. You are responsible for setting the \n", - " various state variables to reasonable values; the defaults below will\n", - " not give you a functional filter.\n", - " \n", - " Parameters\n", - " ----------\n", - " dim_x : int\n", - " Number of state variables for the Kalman filter. For example, if\n", - " you are tracking the position and velocity of an object in two\n", - " dimensions, dim_x would be 4.\n", - " \n", - " This is used to set the default size of P, Q, and u\n", - " \n", - " dim_z : int\n", - " Number of of measurement inputs. For example, if the sensor\n", - " provides you with position in (x,y), dim_z would be 2. \n", - " \"\"\"\n", - " \n", - " self.dim_x = dim_x\n", - " self.dim_z = dim_z\n", - "\n", - " self.x = np.zeros((dim_x,1)) # state\n", - " self.P = np.eye(dim_x) # uncertainty covariance\n", - " self.Q = np.eye(dim_x) # process uncertainty\n", - " self.u = 0 # control input vector\n", - " self.B = np.zeros((dim_x,1))\n", - " self.F = 0 # state transition matrix\n", - " self.H = 0 # Measurement function\n", - " self.R = np.eye(dim_z) # state uncertainty\n", - "\n", - " # identity matrix. Do not alter this.\n", - " self._I = np.eye(dim_x)\n", - "\n", - "More than anything this method exists to document for you what the variable names are in the filter. To do anything useful with this filter you will have to modify most of these values. Some are set to useful values. For example, `R` is set to an identity matrix; if you want the diagonals of `R` to be 10. you may write (as we did earlier in this chapter) `my_filter.R += 10.`.\n", - "\n", - "The names used for each variable matches the math symbology used in this chapter. Thus, `self.P` is the covariance matrix, `self.x` is the state, and so on." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The predict function implements the predict step of the Kalman equations, which are \n", + "The equations in this chapter look very different from the equations in the last chapter, yet I claimed the last chapter implemented a full 1-D (univariate) Kalman filter. \n", "\n", + "Recall that the univariate equations for the update step are:\n", "$$\n", "\\begin{aligned}\n", - "\\mathbf{\\hat{x}} &= \\mathbf{F x} + \\mathbf{B u} \\\\\n", - "\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q} \n", + "\\mu &=\\frac{\\sigma_1^2 \\mu_2 + \\sigma_2^2 \\mu_1} {\\sigma_1^2 + \\sigma_2^2}, \\\\\n", + "\\sigma^2 &= \\frac{1}{\\frac{1}{\\sigma_1^2} + \\frac{1}{\\sigma_2^2}}\n", "\\end{aligned}\n", "$$\n", "\n", - "The corresponding code is\n", - "\n", - " def predict(self): \n", - " self.x = self.F.dot(self.x) + self.B.dot(self.u)\n", - " self.P = self.F.dot(self.P).dot(self.F.T) + self.Q\n", - "\n", - "I haven't discussed the use of numpy much until now, but this method illustrates the power of that package. We use numpy's `array` class to store our data and perform the linear algebra for our filters. `array` implements matrix multication using the `.dot()` method; if you use `*` you will get element-wise multiplication. As a heavy user of linear algebra this design is somewhat distressing as I use matrix multiplation far more often than element-wise multiplaction. However, this design is due to historical developments in the library and we must live with it. The Python community has recognized this problem, and in Python 3.5 we will have the `@` operator to implement matrix multiplication. \n", - "\n", - "With that in mind, the Python code `self.F.dot(self.x)` implements the math expression $\\mathbf{F x}$.\n", - "\n", - "Numpy's `array` implements matrix transposition by using the `.T` property. Therefore, `F.T` is the python implementation of $\\mathbf{F}^T$." - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The `update()` method implements the update equations of the Kalman filter, which are\n", - "\n", - "$$\n", - "\\begin{aligned}\n", - "\\mathbf{\\tilde{y}} &= \\mathbf{z} - \\mathbf{H}\\hat{\\mathbf{x}} \\\\\n", - "\\mathbf{K} &= \\mathbf{P} \\mathbf{H}^T (\\mathbf{H} \\mathbf{P} \\mathbf{H}^T +\\mathbf{R})^{-1} \\\\\n", - "\\hat{\\mathbf{x}} &= \\hat{\\mathbf{x}} + \\mathbf{K} \\mathbf{\\tilde{y}} \\\\\n", - "\\mathbf{P} &= (\\mathbf{I} - \\mathbf{K} \\mathbf{H})\\mathbf{P}\n", - "\\end{aligned}\n", - "$$\n", - "\n" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The corresponding code is:\n", - "\n", - " def update(self, Z, R=None):\n", - " \"\"\"\n", - " Add a new measurement (Z) to the kalman filter. If Z is None, nothing\n", - " is changed.\n", - "\n", - " Optionally provide R to override the measurement noise for this\n", - " one call, otherwise self.R will be used.\n", - "\n", - " self.residual, self.S, and self.K are stored in case you want to\n", - " inspect these variables. Strictly speaking they are not part of the\n", - " output of the Kalman filter, however, it is often useful to know\n", - " what these values are in various scenarios.\n", - " \"\"\"\n", - "\n", - " if Z is None:\n", - " return\n", - "\n", - " if R is None:\n", - " R = self.R\n", - " elif np.isscalar(R):\n", - " R = np.eye(self.dim_z) * R\n", - "\n", - " # error (residual) between measurement and prediction\n", - " self.residual = Z - self.H.dot(self.x)\n", - "\n", - " # project system uncertainty into measurement space\n", - " self.S = self.H.dot(self.P).dot(self.H.T) + R\n", - "\n", - " # map system uncertainty into kalman gain\n", - " self.K = self.P.dot(self.H.T).dot(linalg.inv(self.S))\n", - "\n", - " # predict new x with residual scaled by the kalman gain\n", - " self.x = self.x + self.K.dot(self.residual)\n", - "\n", - " KH = self.K.dot(self.H)\n", - " I_KH = self._I - KH\n", - " self.P = (I_KH.dot(self.P.dot(I_KH.T)) +\n", - " self.K.dot(self.R.dot(self.K.T)))" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "There are a few more complications in this piece of code compared to `predict()` but it should still be quite clear. \n", - "\n", - "The first complication are the lines:\n", - "\n", - " if Z is None:\n", - " return\n", - " \n", - "This just lets you deal with missing data in a natural way. It is typical to use `None` to indicate the absense of data. If there is no data for an update we skip the update equations. This bit of code means you can write something like:\n", - "\n", - " z = read_sensor() # may return None if no data\n", - " my_kf.update(z)\n", - " \n", - "instead of:\n", - " z = read_sensor()\n", - " if z is not None:\n", - " my_kf.update(z)\n", - " \n", - "Reasonable people will argue whether my choice is cleaner, or obscures the fact that we do not update if the measurement is `None`. Having written a lot of avionics code my proclivity is always to do the safe thing. If we pass 'None' into the function I do not want an exception to occur; instead, I want the reasonable thing to happen, which is to just return without doing anything. If you feel that my choice obscures that fact, go ahead and write the explicit `if` statement prior to calling `update()` and get the best of both worlds.\n", - "\n", - "The next bit of code lets you optionally pass in a value to override `R`. It is common for the sensor noise to vary over time; if it does you can pass in the value as the optional parameter `R`.\n", - "\n", - " if R is None:\n", - " R = self.R\n", - " elif np.isscalar(R):\n", - " R = np.eye(self.dim_z) * R\n", - " \n", - "This code will use self.R if you do not provide a value for `R`. If you did provide a value, it will check if you provided a scalar (number); if so it constructs a matrix of the correct dimension for you. Otherwise it assumes that you passed in a matrix of the correct dimension.\n", - "\n", - "The rest of the code implements the Kalman filter equations, with one exception. Instead of implementing \n", - "\n", - "$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}$$\n", - "\n", - "it implements the somewhat more complicated form \n", - "\n", - "$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}(\\mathbf{I} - \\mathbf{KRK})^T + \\mathbf{KRK}^T$$.\n", - "\n", - "The reason for this altered equation is that it is more numerically stable than the former equation, at the cost of being a bit more expensive to compute. It is not always possible to find the optimal value for $\\text{K}$, in which case the former equation will not produce good results because it assumes optimality. The longer reformulation used in the code is derived from more general math that does not assume optimality, and hence provides good results for nonoptimal filters (such as when we can not correctly model our measurement error).\n", - "\n", - "Various texts differ as to whether this form of the equation should always be used, or only used when you know you need it. I choose to expend a bit more processing power to ensure stability; if your hardware is very constrained and you are able to prove that the simpler equation is correct for your problem then you might choose to use it instead. Personally, I find that a risky approach and do not recommend it to non-experts. Brown's *Introduction to Random Signals and Applied Kalman Filtering* [3] discusses this issue in some detail, if you are interested." - ] - }, - { - "cell_type": "code", - "collapsed": false, - "input": [ - "# -*- coding: utf-8 -*-\n", - "\"\"\"\n", - "Created on Fri Oct 18 18:02:07 2013\n", - "\n", - "@author: rlabbe\n", - "\"\"\"\n", - "\n", - "import numpy as np\n", - "import scipy.linalg as linalg\n", - "import matplotlib.pyplot as plt\n", - "import numpy.random as random\n", - "\n", - "class KalmanFilter:\n", - "\n", - " def __init__(self, dim_x, dim_z, use_short_form=False):\n", - " \"\"\" Create a Kalman filter with 'dim_x' state variables, and\n", - " 'dim_z' measurements. You are responsible for setting the various\n", - " state variables to reasonable values; the defaults below will\n", - " not give you a functional filter.\n", - " \"\"\"\n", - "\n", - " self.x = 0 # state\n", - " self.P = np.eye(dim_x) # uncertainty covariance\n", - " self.Q = np.eye(dim_x) # process uncertainty\n", - " self.u = np.zeros((dim_x,1)) # motion vector\n", - " self.B = 0\n", - " self.F = 0 # state transition matrix\n", - " self.H = 0 # Measurement function (maps state to measurements)\n", - " self.R = np.eye(dim_z) # state uncertainty\n", - " \n", - " # identity matrix. Do not alter this. \n", - " self._I = np.eye(dim_x)\n", - " \n", - " if use_short_form:\n", - " self.update = self.update_short_form\n", - "\n", - "\n", - " def update(self, Z, R=None):\n", - " \"\"\"\n", - " Add a new measurement (Z) to the kalman filter. \n", - " \n", - " Optionally provide R to override the measurement noise for this \n", - " one call, otherwise self.R will be used.\n", - " \n", - " self.residual, self.S, and self.K are stored in case you want to\n", - " inspect these variables. Strictly speaking they are not part of the\n", - " output of the Kalman filter, however, it is often useful to know\n", - " what these values are in various scenarios.\n", - " \"\"\"\n", - " \n", - " if R is None:\n", - " R = self.R\n", - " elif np.isscalar(R):\n", - " R = np.eye(self.dim_z) * R\n", - "\n", - " # error (residual) between measurement and prediction\n", - " self.residual = Z - self.H.dot(self.x)\n", - " \n", - " # project system uncertainty into measurement space \n", - " self.S = self.H.dot(self.P).dot(self.H.T) + R \n", - "\n", - " # map system uncertainty into kalman gain\n", - " self.K = self.P.dot(self.H.T).dot(linalg.inv(self.S)) \n", - "\n", - " # predict new x with residual scaled by the kalman gain\n", - " self.x = self.x + self.K.dot(self.residual) \n", - "\n", - " KH = self.K.dot(self.H)\n", - " I_KH = self._I - KH\n", - " self.P = (I_KH.dot(self.P.dot(I_KH.T)) + \n", - " self.K.dot(self.R.dot(self.K.T)))\n", - "\n", - "\n", - " def predict(self):\n", - " \"\"\" predict next position \"\"\"\n", - " \n", - " self.x = self.F.dot(self.x)\n", - " if self.G != 0:\n", - " self.x += self.G.dot(self.u)\n", - " self.P = self.F.dot(self.P).dot(self.F.T) + self.Q" - ], - "language": "python", - "metadata": {}, - "outputs": [], - "prompt_number": 21 - }, - { - "cell_type": "heading", - "level": 2, - "metadata": {}, - "source": [ - "Compare to Single Dimensional Filter" - ] - }, - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "The equations in this chapter look very different from the equations in the last chapter, yet I claimed the last chapter implemented a full 1-D Kalman filter. \n", - "\n", - "Recall that the 1-D equations for the update step are:\n", - "$$\n", - "\\mu =\\frac{\\sigma_1^2 \\mu_2 + \\sigma_2^2 \\mu_1} {\\sigma_1^2 + \\sigma_2^2}, \n", - "\\sigma^2 = \\frac{1}{\\frac{1}{\\sigma_1^2} + \\frac{1}{\\sigma_2^2}}\n", - "$$\n", - "\n", "and that the 1-D equations for the predict step are:\n", - "$$ \\mu = \\mu_1+\\mu_2, \\sigma^2 = \\sigma_1^2 + \\sigma_2^2$$\n", + "$$\n", + "\\begin{aligned}\n", + "\\mu &= \\mu_1+\\mu_2, \\\\ \\sigma^2 &= \\sigma_1^2 + \\sigma_2^2\n", + "\\end{aligned}\n", + "$$\n", "\n", - "Let's implement a simple 1-D kalman filter using the Kalman filter from this chapter, and compare it's output to the kalman filter from the previous chapter by plotting it. We will use a simple model of tracking an object that starts at x=0 and moves by 1 at each step. We will assume the arbitrary value 5 for the measurement noise and .02 for the process noise.\n", + "Let's implement a simple 1-D kalman filter using the Kalman filter from this chapter, and compare its output to the kalman filter from the previous chapter by plotting it. We will use a simple model of tracking an object that starts at x=0 and moves by 1 at each step. We will assume the arbitrary value 5 for the measurement noise and .02 for the process noise.\n", "\n", "First, let's implement the filter from the last chapter:" ] @@ -2649,6 +2751,180 @@ "> **sidebar**: when plotting covariance ellipses, make sure to always use *plt.axis('equal')* in your code. If the axis use different scales the ellipses will be drawn distorted. For example, the ellipse may be drawn as being taller than it is wide, but it may actually be wider than tall." ] }, + { + "cell_type": "heading", + "level": 2, + "metadata": {}, + "source": [ + "Walking Through the KalmanFilter Code (Optional)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "** author's note: this code is somewhat old. This section needs to be edited **\n", + "\n", + "The kalman filter code that we are using is implemented in my Python library `filterpy`. If you are interested in the full implmentation of the filter you should look in `filterpy\\kalman\\kalman_filter.py`. In the following I will present a simplified implementation of the same code. The code in the library handles issues that are beyond the scope of this chapter, such as numerical stability and support for the extended Kalman filter, subject of a later chapter. \n", + "\n", + "The code is implemented as the class `KalmanFilter`. Some Python programmers are not a fan of object oriented (OO) Python, and eschew classes. I do not intend to enter into that battle other than to say that I have often seen OO abused. Here I use the class to encapsulate the data that is pertinent to the filter so that you do not have to store and pass around a half dozen variables everywhere.\n", + "\n", + "The method `__init__()` is used by Python to create the object. Here is the method \n", + "\n", + " def __init__(self, dim_x, dim_z):\n", + " \"\"\" Create a Kalman filter. You are responsible for setting the \n", + " various state variables to reasonable values; the defaults below will\n", + " not give you a functional filter.\n", + " \n", + " Parameters\n", + " ----------\n", + " dim_x : int\n", + " Number of state variables for the Kalman filter. For example, if\n", + " you are tracking the position and velocity of an object in two\n", + " dimensions, dim_x would be 4.\n", + " \n", + " This is used to set the default size of P, Q, and u\n", + " \n", + " dim_z : int\n", + " Number of of measurement inputs. For example, if the sensor\n", + " provides you with position in (x,y), dim_z would be 2. \n", + " \"\"\"\n", + " \n", + " self.dim_x = dim_x\n", + " self.dim_z = dim_z\n", + "\n", + " self.x = np.zeros((dim_x,1)) # state\n", + " self.P = np.eye(dim_x) # uncertainty covariance\n", + " self.Q = np.eye(dim_x) # process uncertainty\n", + " self.u = 0 # control input vector\n", + " self.B = np.zeros((dim_x,1))\n", + " self.F = 0 # state transition matrix\n", + " self.H = 0 # Measurement function\n", + " self.R = np.eye(dim_z) # state uncertainty\n", + "\n", + " # identity matrix. Do not alter this.\n", + " self._I = np.eye(dim_x)\n", + "\n", + "More than anything this method exists to document for you what the variable names are in the filter. To do anything useful with this filter you will have to modify most of these values. Some are set to useful values. For example, `R` is set to an identity matrix; if you want the diagonals of `R` to be 10. you may write (as we did earlier in this chapter) `my_filter.R += 10.`.\n", + "\n", + "The names used for each variable matches the math symbology used in this chapter. Thus, `self.P` is the covariance matrix, `self.x` is the state, and so on.\n", + "\n", + "The predict function implements the predict step of the Kalman equations, which are \n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\mathbf{\\hat{x}} &= \\mathbf{F x} + \\mathbf{B u} \\\\\n", + "\\mathbf{P} &= \\mathbf{FP{F}}^T + \\mathbf{Q} \n", + "\\end{aligned}\n", + "$$\n", + "\n", + "The corresponding code is\n", + "\n", + " def predict(self): \n", + " self.x = self.F.dot(self.x) + self.B.dot(self.u)\n", + " self.P = self.F.dot(self.P).dot(self.F.T) + self.Q\n", + "\n", + "I haven't discussed the use of numpy much until now, but this method illustrates the power of that package. We use numpy's `array` class to store our data and perform the linear algebra for our filters. `array` implements matrix multication using the `.dot()` method; if you use `*` you will get element-wise multiplication. As a heavy user of linear algebra this design is somewhat distressing as I use matrix multiplation far more often than element-wise multiplaction. However, this design is due to historical developments in the library and we must live with it. The Python community has recognized this problem, and in Python 3.5 we will have the `@` operator to implement matrix multiplication. \n", + "\n", + "With that in mind, the Python code `self.F.dot(self.x)` implements the math expression $\\mathbf{F x}$.\n", + "\n", + "Numpy's `array` implements matrix transposition by using the `.T` property. Therefore, `F.T` is the python implementation of $\\mathbf{F}^T$.\n", + "\n", + "The `update()` method implements the update equations of the Kalman filter, which are\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\mathbf{\\tilde{y}} &= \\mathbf{z} - \\mathbf{H}\\hat{\\mathbf{x}} \\\\\n", + "\\mathbf{K} &= \\mathbf{P} \\mathbf{H}^T (\\mathbf{H} \\mathbf{P} \\mathbf{H}^T +\\mathbf{R})^{-1} \\\\\n", + "\\hat{\\mathbf{x}} &= \\hat{\\mathbf{x}} + \\mathbf{K} \\mathbf{\\tilde{y}} \\\\\n", + "\\mathbf{P} &= (\\mathbf{I} - \\mathbf{K} \\mathbf{H})\\mathbf{P}\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "\n", + "\n", + "The corresponding code is:\n", + "\n", + " def update(self, Z, R=None):\n", + " \"\"\"\n", + " Add a new measurement (Z) to the kalman filter. If Z is None, nothing\n", + " is changed.\n", + "\n", + " Optionally provide R to override the measurement noise for this\n", + " one call, otherwise self.R will be used.\n", + "\n", + " self.residual, self.S, and self.K are stored in case you want to\n", + " inspect these variables. Strictly speaking they are not part of the\n", + " output of the Kalman filter, however, it is often useful to know\n", + " what these values are in various scenarios.\n", + " \"\"\"\n", + "\n", + " if Z is None:\n", + " return\n", + "\n", + " if R is None:\n", + " R = self.R\n", + " elif np.isscalar(R):\n", + " R = np.eye(self.dim_z) * R\n", + "\n", + " # error (residual) between measurement and prediction\n", + " self.residual = Z - self.H.dot(self.x)\n", + "\n", + " # project system uncertainty into measurement space\n", + " self.S = self.H.dot(self.P).dot(self.H.T) + R\n", + "\n", + " # map system uncertainty into kalman gain\n", + " self.K = self.P.dot(self.H.T).dot(linalg.inv(self.S))\n", + "\n", + " # predict new x with residual scaled by the kalman gain\n", + " self.x = self.x + self.K.dot(self.residual)\n", + "\n", + " KH = self.K.dot(self.H)\n", + " I_KH = self._I - KH\n", + " self.P = (I_KH.dot(self.P.dot(I_KH.T)) +\n", + " self.K.dot(self.R.dot(self.K.T)))\n", + "\n", + "There are a few more complications in this piece of code compared to `predict()` but it should still be quite clear. \n", + "\n", + "The first complication are the lines:\n", + "\n", + " if Z is None:\n", + " return\n", + " \n", + "This just lets you deal with missing data in a natural way. It is typical to use `None` to indicate the absense of data. If there is no data for an update we skip the update equations. This bit of code means you can write something like:\n", + "\n", + " z = read_sensor() # may return None if no data\n", + " my_kf.update(z)\n", + " \n", + "instead of:\n", + " z = read_sensor()\n", + " if z is not None:\n", + " my_kf.update(z)\n", + " \n", + "Reasonable people will argue whether my choice is cleaner, or obscures the fact that we do not update if the measurement is `None`. Having written a lot of avionics code my proclivity is always to do the safe thing. If we pass 'None' into the function I do not want an exception to occur; instead, I want the reasonable thing to happen, which is to just return without doing anything. If you feel that my choice obscures that fact, go ahead and write the explicit `if` statement prior to calling `update()` and get the best of both worlds.\n", + "\n", + "The next bit of code lets you optionally pass in a value to override `R`. It is common for the sensor noise to vary over time; if it does you can pass in the value as the optional parameter `R`.\n", + "\n", + " if R is None:\n", + " R = self.R\n", + " elif np.isscalar(R):\n", + " R = np.eye(self.dim_z) * R\n", + " \n", + "This code will use self.R if you do not provide a value for `R`. If you did provide a value, it will check if you provided a scalar (number); if so it constructs a matrix of the correct dimension for you. Otherwise it assumes that you passed in a matrix of the correct dimension.\n", + "\n", + "The rest of the code implements the Kalman filter equations, with one exception. Instead of implementing \n", + "\n", + "$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}$$\n", + "\n", + "it implements the somewhat more complicated form \n", + "\n", + "$$\\mathbf{P} = (\\mathbf{I} - \\mathbf{KH})\\mathbf{P}(\\mathbf{I} - \\mathbf{KRK})^T + \\mathbf{KRK}^T$$.\n", + "\n", + "The reason for this altered equation is that it is more numerically stable than the former equation, at the cost of being a bit more expensive to compute. It is not always possible to find the optimal value for $\\text{K}$, in which case the former equation will not produce good results because it assumes optimality. The longer reformulation used in the code is derived from more general math that does not assume optimality, and hence provides good results for nonoptimal filters (such as when we can not correctly model our measurement error).\n", + "\n", + "Various texts differ as to whether this form of the equation should always be used, or only used when you know you need it. I choose to expend a bit more processing power to ensure stability; if your hardware is very constrained and you are able to prove that the simpler equation is correct for your problem then you might choose to use it instead. Personally, I find that a risky approach and do not recommend it to non-experts. Brown's *Introduction to Random Signals and Applied Kalman Filtering* [3] discusses this issue in some detail, if you are interested." + ] + }, { "cell_type": "heading", "level": 2, @@ -2665,7 +2941,10 @@ "\n", "[2] https://en.wikipedia.org/wiki/Kalman_filter\n", "\n", - "[3] Brown, Robert Grover. *Introduction to Random Signals and Applied Kalman Filtering* John Wiley & Sons, Inc. 2012" + "[3] Brown, Robert Grover. *Introduction to Random Signals and Applied Kalman Filtering* John Wiley & Sons, Inc. 2012\n", + "\n", + "[4] `filterpy` library. Roger Labbe.\n", + "https://github.com/rlabbe/filterpy" ] } ],