diff --git a/Kalman Filters.ipynb b/Kalman Filters.ipynb index 0154494..6c3c3c3 100644 --- a/Kalman Filters.ipynb +++ b/Kalman Filters.ipynb @@ -32,16 +32,16 @@ "import numpy.random as random\n", "import math\n", "\n", - "class dog_sensor(object):\n", + "class DogSensor(object):\n", " \n", - " def __init__(self, x0 = 0, velocity=1, noise=0.0):\n", + " def __init__(self, x0=0, velocity=1, noise=0.0):\n", " \"\"\" x0 - initial position\n", " velocity - (+=right, -=left)\n", " noise - scaling factor for noise, 0== no noise\n", " \"\"\"\n", - " self.x = x0\n", + " self.x = x0\n", " self.velocity = velocity\n", - " self.noise = math.sqrt(noise)\n", + " self.noise = math.sqrt(noise)\n", "\n", " def sense(self):\n", " self.x = self.x + self.velocity\n", @@ -56,7 +56,7 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "The constructor (__init__()) initializes the dog_sensor class with an initial position (x0), velocity (vel), and an noise scaling factor. The *sense()* function has the dog move by the set velocity and returns its new position, with noise added. If you look at the code for *sense()* you will see a call to *numpy.random.randn()*. This returns a number sampled from a normal distribution with a mean of 0.0. Let's look at some example output for that.\n", + "The constructor (__init__()) initializes the DogSensor class with an initial position (x0), velocity (vel), and an noise scaling factor. The *sense()* function has the dog move by the set velocity and returns its new position, with noise added. If you look at the code for *sense()* you will see a call to *numpy.random.randn()*. This returns a number sampled from a normal distribution with a mean of 0.0. Let's look at some example output for that.\n", "\n" ] }, @@ -86,20 +86,20 @@ "source": [ "You should see a sequence of numbers near 0, some negative and some positive. Most are probably between -1 and 1, but a few might lie somewhat outside that range. This is what we expect from a normal distribution - values are clustered around the mean, and there are fewer values the further you get from the mean.\n", "\n", - "Okay, so lets look at the output of the *dog_sensor* class. We will start by setting the noise to 0 to check that the class does what we think it does" + "Okay, so lets look at the output of the *DogSensor* class. We will start by setting the noise to 0 to check that the class does what we think it does" ] }, { "cell_type": "code", "collapsed": false, "input": [ - "dog = dog_sensor (noise = 0.0)\n", + "dog = DogSensor (noise=0.0)\n", "xs = []\n", - "for i in range (10):\n", + "for i in range(10):\n", " x = dog.sense()\n", " xs.append(x)\n", - " print (\"%.4f\" % x),\n", - "plot (xs)\n", + " print(\"%.4f\" % x),\n", + "plot(xs)\n", "show()" ], "language": "python", @@ -144,19 +144,19 @@ "collapsed": false, "input": [ "def test_sensor(noise_scale):\n", - " dog = dog_sensor (noise = noise_scale)\n", + " dog = DogSensor(noise=noise_scale)\n", "\n", " xs = []\n", - " for i in range (100):\n", + " for i in range(100):\n", " x = dog.sense()\n", " xs.append(x)\n", - " p1, = plot (xs, c='b')\n", - " p2, = plot ([0,99],[1,100], 'r--')\n", + " p1, = plot(xs, c='b')\n", + " p2, = plot([0,99],[1,100], 'r--')\n", " xlabel('time')\n", " ylabel('pos')\n", - " ylim ([0,100])\n", + " ylim([0,100])\n", " title('noise = ' + str(noise_scale))\n", - " legend ([p1, p2], ['sensor', 'actual'], loc=2)\n", + " legend([p1, p2], ['sensor', 'actual'], loc=2)\n", " show()\n", " \n", "test_sensor(4.0)" @@ -249,7 +249,7 @@ "collapsed": false, "input": [ "import gaussian\n", - "gaussian.norm_plot (23, 5)" + "gaussian.norm_plot(23, 5)" ], "language": "python", "metadata": {}, @@ -280,13 +280,13 @@ "cell_type": "code", "collapsed": false, "input": [ - "dog = dog_sensor (23, 0, 5)\n", - "xs = range (100)\n", + "dog = DogSensor(23, 0, 5)\n", + "xs = range(100)\n", "ys = []\n", "for i in xs:\n", - " ys.append (dog.sense())\n", + " ys.append(dog.sense())\n", " \n", - "plot (xs,ys)\n", + "plot(xs,ys)\n", "show()" ], "language": "python", @@ -312,7 +312,7 @@ "\n", "Recall the histogram code for adding a measurement to a pre-existing belief:\n", "\n", - " def sense (pos, measure, p_hit, p_miss):\n", + " def sense(pos, measure, p_hit, p_miss):\n", " q = array(pos, dtype=float)\n", " for i in range(len(hallway)):\n", " if hallway[i] == measure:\n", @@ -350,25 +350,25 @@ "collapsed": false, "input": [ "import gaussian\n", - "def multiply (mu1, sig1, mu2, sig2):\n", + "def multiply(mu1, sig1, mu2, sig2):\n", " m = (sig1*mu2 + sig2*mu1) / (sig1+sig2)\n", " s = 1. / (1./sig1 + 1./ sig2)\n", " return (m,s)\n", "\n", "\n", - "xs = np.arange (16, 30, 0.1)\n", + "xs = np.arange(16, 30, 0.1)\n", "\n", "\n", "m1,s1 = 23, 5\n", - "m, s = multiply (m1,s1,m1,s1)\n", + "m, s = multiply(m1,s1,m1,s1)\n", "\n", - "ys = [gaussian.gaussian (x,m1,s1) for x in xs]\n", + "ys = [gaussian.gaussian(x,m1,s1) for x in xs]\n", "p1, =plot (xs,ys)\n", "\n", - "ys = [gaussian.gaussian (x,m,s) for x in xs]\n", + "ys = [gaussian.gaussian(x,m,s) for x in xs]\n", "p2, = plot (xs,ys)\n", "\n", - "legend ([p1,p2],['original', 'multiply'])\n", + "legend([p1,p2],['original', 'multiply'])\n", "show()" ], "language": "python", @@ -398,22 +398,22 @@ "cell_type": "code", "collapsed": false, "input": [ - "xs = np.arange (16, 30, 0.1)\n", + "xs = np.arange(16, 30, 0.1)\n", "\n", "\n", "m1,s1 = 23, 5\n", "m2,s2 = 25, 5\n", - "m, s = multiply (m1,s1,m2,s2)\n", + "m, s = multiply(m1,s1,m2,s2)\n", "\n", - "ys = [gaussian.gaussian (x,m1,s1) for x in xs]\n", + "ys = [gaussian.gaussian(x,m1,s1) for x in xs]\n", "p1, = plot (xs,ys)\n", "\n", - "ys = [gaussian.gaussian (x,m2,s2) for x in xs]\n", + "ys = [gaussian.gaussian(x,m2,s2) for x in xs]\n", "p2, = plot (xs,ys)\n", "\n", - "ys = [gaussian.gaussian (x,m,s) for x in xs]\n", - "p3, = plot (xs,ys)\n", - "legend ([p1,p2,p3],['measure 1', 'measure 2', 'multiply'])\n", + "ys = [gaussian.gaussian(x,m,s) for x in xs]\n", + "p3, = plot(xs,ys)\n", + "legend([p1,p2,p3],['measure 1', 'measure 2', 'multiply'])\n", "show()" ], "language": "python", @@ -458,8 +458,8 @@ "cell_type": "code", "collapsed": false, "input": [ - "def sense (mu, sigma, measurement, measurement_sigma):\n", - " return multiply (mu, sigma, measurement, measurement_sigma)" + "def sense(mu, sigma, measurement, measurement_sigma):\n", + " return multiply(mu, sigma, measurement, measurement_sigma)" ], "language": "python", "metadata": {}, @@ -477,8 +477,8 @@ "cell_type": "code", "collapsed": false, "input": [ - "def sense_dog (dog_pos, dog_sigma, measurement, measurement_sigma):\n", - " return multiply (dog_pos, dog_sigma, measurement, measurement_sigma)" + "def sense_dog(dog_pos, dog_sigma, measurement, measurement_sigma):\n", + " return multiply(dog_pos, dog_sigma, measurement, measurement_sigma)" ], "language": "python", "metadata": {}, @@ -489,20 +489,20 @@ "cell_type": "markdown", "metadata": {}, "source": [ - "That is less abstract, which is good, but it is poor coding practice. We want to write a Kalman filter that works for any problem, not just tracking dogs in a hallway, so we don't use variable names with 'dog' in them. Still, the *sense_dog()* function should make what we are doing very clear. \n", + "That is less abstract, which perhaps helps with comprehension, but it is poor coding practice. We are writing a Kalman filter that works for any problem, not just tracking dogs in a hallway, so we don't use variable names with 'dog' in them. Still, the *sense_dog()* function should make what we are doing very clear. \n", "\n", - "Let's look at an example. We will suppose that our current belief for the dog's position is $N(2,5)$. Don't worry about where that number came from. It may appear that we have a chicken and egg problem, in that how do we know the position before we sense it, but we will resolve that shortly. We will create a dog_sensor object initialized to be at position 0.0, and with no velocity, and modest noise. This corresponds to the dog standing still at the far left side of the hallway. Note that we mistakenly believe the dog is at postion 2.0, not 0.0." + "Let's look at an example. We will suppose that our current belief for the dog's position is $N(2,5)$. Don't worry about where that number came from. It may appear that we have a chicken and egg problem, in that how do we know the position before we sense it, but we will resolve that shortly. We will create a *DogSensor* object initialized to be at position 0.0, and with no velocity, and modest noise. This corresponds to the dog standing still at the far left side of the hallway. Note that we mistakenly believe the dog is at postion 2.0, not 0.0." ] }, { "cell_type": "code", "collapsed": false, "input": [ - "dog = dog_sensor(velocity = 0, noise = 1)\n", + "dog = DogSensor(velocity=0, noise=1)\n", "\n", "pos,s = 2, 5\n", - "for i in range (20):\n", - " pos,s = sense (pos, s, dog.sense(), 5)\n", + "for i in range(20):\n", + " pos,s = sense(pos, s, dog.sense(), 5)\n", " print 'time:', i, 'position = ', \"%.3f\" % pos, 'variance = ', \"%.3f\" % s\n", "\n" ], @@ -557,7 +557,7 @@ "\n", "How how do we perform the update function with gaussians? Recall the histogram method:\n", "\n", - " def update (pos, move, p_correct, p_under, p_over):\n", + " def update(pos, move, p_correct, p_under, p_over):\n", " n = len(pos)\n", " result = array(pos, dtype=float)\n", " for i in range(n):\n", @@ -611,30 +611,30 @@ "# assume dog is always moving 1m to the right\n", "movement = 1\n", "movement_error = 2\n", - "sensor_error = 10\n", + "sensor_error = 10\n", "pos = (0, 500) # gaussian N(0,50)\n", "\n", - "dog = dog_sensor (pos[0], velocity=movement, noise=sensor_error)\n", + "dog = DogSensor(pos[0], velocity=movement, noise=sensor_error)\n", "\n", "zs = []\n", "ps = []\n", "\n", "for i in range(10):\n", - " pos = update (pos[0], pos[1], movement, movement_error)\n", + " pos = update(pos[0], pos[1], movement, movement_error)\n", " print 'UPDATE:', \"%.4f\" %pos[0], \", %.4f\" %pos[1]\n", " \n", " Z = dog.sense()\n", " zs.append(Z)\n", " \n", - " pos = sense (pos[0], pos[1], Z, sensor_error)\n", + " pos = sense(pos[0], pos[1], Z, sensor_error)\n", " ps.append(pos[0])\n", " \n", " print 'SENSE:', \"%.4f\" %pos[0], \", %.4f\" %pos[1]\n", " print\n", " \n", - "p1, = plot (zs,c='r', linestyle='dashed')\n", - "p2, = plot (ps, c='b')\n", - "legend ([p1,p2], ['measurement', 'filter'], 2)\n", + "p1, = plot(zs,c='r', linestyle='dashed')\n", + "p2, = plot(ps, c='b')\n", + "legend([p1,p2], ['measurement', 'filter'], 2)\n", "show()" ], "language": "python", @@ -702,14 +702,14 @@ "\n", " movement = 1\n", " movement_error = 2\n", - " sensor_error = 10\n", + " sensor_error = 10\n", " pos = (0, 500) # gaussian N(0,500)\n", " \n", " \n", "The first lines just set up the initial conditions for our filter. We are assuming that the dog moves steadily to the right 1m at a time. We have a relatively low error of 2 for the movement sensor, and a higher error of 10 for the RFID position sensor. Finally, we set our belief of the dog's initial position as $N(0,500)$. Why those numbers. Well, 0 is as good as any number if we don't know where the dog is. But we set the variance to 500 to denote that we have no confidence in this value at all. 100m is almost as likely as 0 with this value for the variance. \n", "\n", "Next we initialize the RFID simulator with\n", - " dog = dog_sensor (pos[0], velocity=movement, noise=sensor_error)\n", + " dog = DogSensor(pos[0], velocity=movement, noise=sensor_error)\n", "\n", "It may seem very 'convienent' to set the simulator to the same position as our guess, and it is. Do not fret. In the next example we will see the effect of a wildly inaccurate guess for the dog's initial position.\n", "\n", @@ -724,10 +724,10 @@ "Now we just enter our *sense()->update()* loop.\n", "\n", " for i in range(10):\n", - " pos = update (pos[0], pos[1], movement, sensor_error)\n", + " pos = update(pos[0], pos[1], movement, sensor_error)\n", " print 'UPDATE:', \"%.4f\" %pos[0], \", %.4f\" %pos[1]\n", "\n", - "Wait, why *update()* before sense? It turns out the order does not matter once, but the first call to dog_sensor.sense() assumes that the dog has already moved, so we start with the update step. In practice you will order these calls based on the details of your sensor, and you will very typically do the *sense()* first.\n", + "Wait, why *update()* before sense? It turns out the order does not matter once, but the first call to DogSensor.sense() assumes that the dog has already moved, so we start with the update step. In practice you will order these calls based on the details of your sensor, and you will very typically do the *sense()* first.\n", "\n", "So we call the update function with the gaussian representing our current belief about our position, the another gaussian representing our belief as to where the dog is moving, and then print the output. Your output will differ, but when writing this I get this as output:\n", "\n", @@ -741,7 +741,7 @@ "Here we sense the dog's position, and store it in our array so we can plot the results later.\n", "\n", "Finally we call the sense function of our filter, save the result in our *ps* array, and print the updated position belief:\n", - " pos = sense (pos[0], pos[1], Z, movement_error)\n", + " pos = sense(pos[0], pos[1], Z, movement_error)\n", " ps.append(pos[0])\n", " print 'SENSE:', \"%.4f\" %pos[0], \", %.4f\" %pos[1]\n", " \n", @@ -771,24 +771,24 @@ "movement_error = 2\n", "pos = (0,500)\n", "\n", - "dog = dog_sensor (pos[0], velocity=movement, noise=sensor_error)\n", + "dog = DogSensor(pos[0], velocity=movement, noise=sensor_error)\n", "\n", "zs = []\n", "ps = []\n", "\n", "for i in range(1000):\n", - " pos = update (pos[0], pos[1], movement, movement_error)\n", + " pos = update(pos[0], pos[1], movement, movement_error)\n", " \n", " Z = dog.sense()\n", " zs.append(Z)\n", " \n", - " pos = sense (pos[0], pos[1], Z, sensor_error)\n", + " pos = sense(pos[0], pos[1], Z, sensor_error)\n", " ps.append(pos[0])\n", "\n", "\n", - "p1, = plot (zs,c='r', linestyle='dashed')\n", - "p2, = plot (ps, c='b')\n", - "legend ([p1,p2], ['measurement', 'filter'], 2)\n", + "p1, = plot(zs,c='r', linestyle='dashed')\n", + "p2, = plot(ps, c='b')\n", + "legend([p1,p2], ['measurement', 'filter'], 2)\n", "show()" ], "language": "python", @@ -822,24 +822,24 @@ "movement_error = 2\n", "pos = (1000,500)\n", "\n", - "dog = dog_sensor (0, velocity=movement, noise=sensor_error)\n", + "dog = DogSensor(0, velocity=movement, noise=sensor_error)\n", "\n", "zs = []\n", "ps = []\n", "\n", "for i in range(100):\n", - " pos = update (pos[0], pos[1], movement, movement_error)\n", + " pos = update(pos[0], pos[1], movement, movement_error)\n", " \n", " Z = dog.sense()\n", " zs.append(Z)\n", " \n", - " pos = sense (pos[0], pos[1], Z, sensor_error)\n", + " pos = sense(pos[0], pos[1], Z, sensor_error)\n", " ps.append(pos[0])\n", "\n", "\n", - "p1, = plot (zs,c='r', linestyle='dashed')\n", - "p2, = plot (ps, c='b')\n", - "legend ([p1,p2], ['measurement', 'filter'], 2)\n", + "p1, = plot(zs,c='r', linestyle='dashed')\n", + "p2, = plot(ps, c='b')\n", + "legend([p1,p2], ['measurement', 'filter'], 2)\n", "show()" ], "language": "python", @@ -873,7 +873,7 @@ "movement_error = 2\n", "pos = (1000,500)\n", "\n", - "dog = dog_sensor (0, velocity=movement, noise=sensor_error)\n", + "dog = DogSensor(0, velocity=movement, noise=sensor_error)\n", "\n", "zs = []\n", "ps = []\n", @@ -888,9 +888,9 @@ " ps.append(pos[0])\n", "\n", "\n", - "p1, = plot (zs,c='r', linestyle='dashed')\n", - "p2, = plot (ps, c='b')\n", - "legend ([p1,p2], ['measurement', 'filter'], 2)\n", + "p1, = plot(zs,c='r', linestyle='dashed')\n", + "p2, = plot(ps, c='b')\n", + "legend([p1,p2], ['measurement', 'filter'], 2)\n", "show()" ], "language": "python", @@ -924,7 +924,7 @@ "movement_error = 2\n", "pos = None\n", "\n", - "dog = dog_sensor (0, velocity=movement, noise=sensor_error)\n", + "dog = DogSensor(0, velocity=movement, noise=sensor_error)\n", "\n", "zs = []\n", "ps = []\n", @@ -940,9 +940,9 @@ "\n", " pos = update (pos[0], pos[1], movement, movement_error)\n", "\n", - "p1, = plot (zs,c='r', linestyle='dashed')\n", - "p2, = plot (ps, c='b')\n", - "legend ([p1,p2], ['measurement', 'filter'], 2)\n", + "p1, = plot(zs,c='r', linestyle='dashed')\n", + "p2, = plot(ps, c='b')\n", + "legend([p1,p2], ['measurement', 'filter'], 2)\n", "show()" ], "language": "python", @@ -986,7 +986,7 @@ "movement_sensor = 30000\n", "pos = (0,500)\n", "\n", - "dog = dog_sensor (0, velocity=movement, noise=sensor_error)\n", + "dog = DogSensor(0, velocity=movement, noise=sensor_error)\n", "\n", "zs = []\n", "ps = []\n", @@ -995,14 +995,14 @@ " Z = dog.sense()\n", " zs.append(Z)\n", " \n", - " pos = sense (pos[0], pos[1], Z, sensor_error)\n", + " pos = sense(pos[0], pos[1], Z, sensor_error)\n", " ps.append(pos[0])\n", "\n", - " pos = update (pos[0], pos[1], movement, movement_error)\n", + " pos = update(pos[0], pos[1], movement, movement_error)\n", "\n", - "p1, = plot (zs,c='r', linestyle='dashed')\n", - "p2, = plot (ps, c='b')\n", - "legend ([p1,p2], ['measurement', 'filter'], 2)\n", + "p1, = plot(zs,c='r', linestyle='dashed')\n", + "p2, = plot(ps, c='b')\n", + "legend([p1,p2], ['measurement', 'filter'], 2)\n", "show()" ], "language": "python", @@ -1039,7 +1039,10 @@ { "cell_type": "code", "collapsed": false, - "input": [], + "input": [ + "author notes:\n", + " clean up the code - same stuff duplicated over and over - write a 'clean implemntation' at the end." + ], "language": "python", "metadata": {}, "outputs": [], diff --git a/Multidimensional Kalman Filters.ipynb b/Multidimensional Kalman Filters.ipynb index eebcd82..0d5cf36 100644 --- a/Multidimensional Kalman Filters.ipynb +++ b/Multidimensional Kalman Filters.ipynb @@ -53,7 +53,7 @@ "input": [ "import numpy as np\n", "import math\n", - "def multivariate_gaussian (x, mu, cov):\n", + "def multivariate_gaussian(x, mu, cov):\n", " n = len(x)\n", " det = np.sqrt(np.prod(np.diag(cov)))\n", " frac = (2*math.pi)**(-n/2.) * (1./det)\n", @@ -134,7 +134,7 @@ "cell_type": "code", "collapsed": false, "input": [ - "print multivariate_gaussian (x,mu,cov)" + "print multivariate_gaussian(x,mu,cov)" ], "language": "python", "metadata": {}, @@ -161,7 +161,7 @@ "collapsed": false, "input": [ "x = np.array([2,7])\n", - "print multivariate_gaussian (x,mu,cov)" + "print multivariate_gaussian(x,mu,cov)" ], "language": "python", "metadata": {}, @@ -188,14 +188,14 @@ "collapsed": false, "input": [ "from mpl_toolkits.mplot3d import Axes3D\n", - "xs, ys = arange (-8, 13, .1), arange (-8, 20, .1)\n", + "xs, ys = arange(-8, 13, .1), arange(-8, 20, .1)\n", "xv, yv = meshgrid (xs, ys)\n", "\n", "zs = np.array([multivariate_gaussian(np.array([x,y]),mu,cov) for x,y in zip(np.ravel(xv), np.ravel(yv))])\n", "zv = zs.reshape(xv.shape)\n", "\n", "ax = plt.figure().add_subplot(111, projection='3d')\n", - "ax.plot_surface (xv, yv, zv)\n", + "ax.plot_surface(xv, yv, zv)\n", "show()" ], "language": "python", @@ -239,14 +239,14 @@ " scatter(x,y,marker='+') # mark the center\n", " show()\n", " \n", - "cov = array ([[2,0],[0,2]])\n", - "plot_sigma_ellipse (2,7,cov)\n", + "cov = array([[2,0],[0,2]])\n", + "plot_sigma_ellipse(2,7,cov)\n", "\n", - "cov = array ([[2,0],[0,12]])\n", - "plot_sigma_ellipse (2,7,cov)\n", + "cov = array([[2,0],[0,12]])\n", + "plot_sigma_ellipse(2,7,cov)\n", "\n", - "cov = array ([[2,3],[1,2.5]])\n", - "plot_sigma_ellipse (2,7,cov)\n" + "cov = array([[2,3],[1,2.5]])\n", + "plot_sigma_ellipse(2,7,cov)\n" ], "language": "python", "metadata": {},