{ "cells": [ { "cell_type": "code", "execution_count": 1, "metadata": { "collapsed": false }, "outputs": [], "source": [ "from __future__ import division, print_function\n", "%matplotlib inline\n", "import sys\n", "sys.path.insert(0,'..') # allow us to format the book\n", "sys.path.insert(0,'../code') # allow us to format the book\n", "sys.path.insert(0,'./code') # allow us to format the book\n", "\n", "# use same formatting as rest of book so that the plots are\n", "# consistant with that look and feel.\n", "#import book_format\n", "#book_format.load_style('..')" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "This notebook creates the animations for the Multivariate Kalman Filter chapter. It is not really intended to be a readable part of the book, but of course you are free to look at the source code, and even modify it. However, if you are interested in running your own animations, I'll point you to the examples subdirectory of the book, which contains a number of python scripts that you can run and modify from an IDE or the command line. This module saves the animations to GIF files, which is quite slow and not very interactive. " ] }, { "cell_type": "code", "execution_count": 2, "metadata": { "collapsed": false }, "outputs": [], "source": [ "import filterpy.stats as stats\n", "import numpy as np\n", "from matplotlib.patches import Ellipse\n", "import matplotlib.pyplot as plt\n", "from matplotlib import cm\n", "from mpl_toolkits.mplot3d import Axes3D" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "collapsed": false }, "outputs": [ { "ename": "ModuleNotFoundError", "evalue": "No module named 'gif_animate'", "output_type": "error", "traceback": [ "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[1;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)", "\u001b[1;32m\u001b[0m in \u001b[0;36m\u001b[1;34m\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[1;31m#import mkf_internal\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mmatplotlib\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mpyplot\u001b[0m \u001b[1;32mas\u001b[0m \u001b[0mplt\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 4\u001b[1;33m \u001b[1;32mfrom\u001b[0m \u001b[0mgif_animate\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0manimate\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m 5\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 6\u001b[0m \u001b[1;32mdef\u001b[0m \u001b[0mellipse_animate\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mframe\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n", "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'gif_animate'" ] } ], "source": [ "import numpy as np\n", "#import mkf_internal\n", "import matplotlib.pyplot as plt\n", "from gif_animate import animate\n", "\n", "def ellipse_animate(frame):\n", " \n", " plt.cla()\n", " cov = frame/15.\n", " P = np.array([[2,cov],[cov,2]])\n", " stats.plot_covariance_ellipse((2,7), cov=P, facecolor='g', alpha=0.2, \n", " title='|2.0 {:.1f}|\\n|{:.1f} 2.0|'.format(cov, cov))\n", "fig = plt.figure()\n", "animate('multivariate_ellipse.gif', ellipse_animate, 30, 125, figsize=(4, 4))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "code", "execution_count": 4, "metadata": { "collapsed": false }, "outputs": [ { "ename": "NameError", "evalue": "name 'f' is not defined", "output_type": "error", "traceback": [ "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "\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[0mf\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mQ\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m", "\u001b[1;31mNameError\u001b[0m: name 'f' is not defined" ] } ], "source": [ "f.Q" ] }, { "cell_type": "code", "execution_count": 5, "metadata": { "collapsed": false }, "outputs": [ { "ename": "ModuleNotFoundError", "evalue": "No module named 'gif_animate'", "output_type": "error", "traceback": [ "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", "\u001b[1;31mModuleNotFoundError\u001b[0m Traceback (most recent call last)", "\u001b[1;32m\u001b[0m in \u001b[0;36m\u001b[1;34m\u001b[0m\n\u001b[0;32m 4\u001b[0m \u001b[1;32mfrom\u001b[0m \u001b[0mmatplotlib\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mcm\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 5\u001b[0m \u001b[1;32mfrom\u001b[0m \u001b[0mmpl_toolkits\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mmplot3d\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mAxes3D\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 6\u001b[1;33m \u001b[1;32mfrom\u001b[0m \u001b[0mgif_animate\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0manimate\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m 7\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mnumpy\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mrandom\u001b[0m \u001b[1;32mas\u001b[0m \u001b[0mrandom\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m 8\u001b[0m \u001b[1;32mfrom\u001b[0m \u001b[0mDogSimulation\u001b[0m \u001b[1;32mimport\u001b[0m \u001b[0mDogSimulation\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n", "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'gif_animate'" ] } ], "source": [ "import numpy as np\n", "from matplotlib.patches import Ellipse\n", "import matplotlib.pyplot as plt\n", "from matplotlib import cm\n", "from mpl_toolkits.mplot3d import Axes3D\n", "from gif_animate import animate\n", "import numpy.random as random\n", "from DogSimulation import DogSimulation\n", "import filterpy.stats\n", "from filterpy.common import Q_discrete_white_noise\n", "from filterpy.kalman import KalmanFilter\n", "import book_format\n", "book_format.load_style('..')\n", "\n", "def dog_tracking_filter(R, Q=0, cov=1.):\n", " dog_filter = KalmanFilter(dim_x=2, dim_z=1)\n", " dog_filter.x = np.array([0, 0]) # initial state (location and velocity)\n", " dog_filter.F = np.array([[1.,1],\n", " [0,1]]) # state transition matrix\n", " dog_filter.H = np.array([[1.,0]]) # Measurement function\n", " dog_filter.R *= R # measurement uncertainty\n", " dog_filter.P *= cov # covariance matrix \n", " if np.isscalar(Q):\n", " dog_filter.Q = Q_discrete_white_noise(2, var=Q)\n", " else:\n", " dog_filter.Q = Q\n", " return dog_filter\n", "\n", "R = 5.\n", "Q = .01\n", "noise = 2.\n", "P = 20.\n", "dog = DogSimulation(measurement_var=R, process_var=Q)\n", "f = dog_tracking_filter(R=R, Q=Q, cov=P)\n", "random.seed(200)\n", "zs = []\n", "xs = []\n", "def animate_track(frame):\n", " if frame > 30: return\n", " \n", " if frame == 0:\n", " stats.plot_covariance_ellipse((0, f.x[0]), cov=f.P, axis_equal=True, \n", " facecolor='g', edgecolor=None, alpha=0.2)\n", " xs.append(f.x[0])\n", " \n", " z = dog.move_and_sense()[1]\n", "\n", " zs.append(z)\n", " f.update(z)\n", " xs.append(f.x[0])\n", " \n", " stats.plot_covariance_ellipse((frame+1, f.x[0]), cov=f.P, axis_equal=True, \n", " facecolor='g', edgecolor=None, alpha=0.5,\n", " xlim=(-5,40), ylim=(-5,40))\n", " \n", " plt.plot(zs, color='r', linestyle='dashed')\n", " plt.plot(xs, color='b')\n", " f.predict()\n", "\n", "animate('multivariate_track1.gif', animate_track, 37, 200, figsize=(5.5, 5.5))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "\n" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "codemirror_mode": { "name": "ipython", "version": 3 }, "file_extension": ".py", "mimetype": "text/x-python", "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", "version": "3.6.10" } }, "nbformat": 4, "nbformat_minor": 0 }