{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "%matplotlib inline\n",
    "import matplotlib.dates as mdates\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy.stats import norm\n",
    "from sympy import Symbol, symbols, Matrix, sin, cos\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Extended Kalman Filter Implementation for Constant Turn Rate and Velocity (CTRV) Vehicle Model in Python\n",
    "\n",
    "![Extended Kalman Filter Step](Extended-Kalman-Filter-Step.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "[Wikipedia](http://en.wikipedia.org/wiki/Extended_Kalman_filter) writes: In the extended Kalman filter, the state transition and observation models need not be linear functions of the state but may instead be differentiable functions.\n",
    "\n",
    "$\\boldsymbol{x}_{k} = g(\\boldsymbol{x}_{k-1}, \\boldsymbol{u}_{k-1}) + \\boldsymbol{w}_{k-1}$\n",
    "\n",
    "$\\boldsymbol{z}_{k} = h(\\boldsymbol{x}_{k}) + \\boldsymbol{v}_{k}$\n",
    "\n",
    "Where $w_k$ and $v_k$ are the process and observation noises which are both assumed to be zero mean Multivariate Gaussian noises with covariance matrix $Q$ and $R$ respectively.\n",
    "\n",
    "The function $g$ can be used to compute the predicted state from the previous estimate and similarly the function $h$ can be used to compute the predicted measurement from the predicted state. However, $g$ and $h$ cannot be applied to the covariance directly. Instead a matrix of partial derivatives (the Jacobian matrix) is computed.\n",
    "\n",
    "At each time step, the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations. This process essentially linearizes the non-linear function around the current estimate."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## State Vector - Constant Turn Rate and Velocity Vehicle Model (CTRV)\n",
    "\n",
    "Situation covered: You have a velocity sensor, which measures the vehicle speed ($v$) in heading direction ($\\psi$) and a yaw rate sensor ($\\dot \\psi$) which both have to fused with the position ($x$ & $y$) from a GPS sensor."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "Constant Turn Rate, Constant Velocity Model for a vehicle ![CTRV Model](CTRV-Model.png)\n",
    "\n",
    "$$x_k= \\left[ \\matrix{ x^{*} \\\\ y^{*} \\\\ \\psi \\\\ v \\\\ \\dot\\psi} \\right] = \\left[ \\matrix{ \\text{Position X} \\\\ \\text{Position Y} \\\\ \\text{Heading} \\\\ \\text{Velocity} \\\\ \\text{Yaw Rate}} \\right]$$\n",
    "\n",
    "$^{*}$=actually measured values in this implementation example!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "numstates=5 # States"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "We have different frequency of sensor readings."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "dt = 1.0/50.0 # Sample Rate of the Measurements is 50Hz\n",
    "dtGPS=1.0/10.0 # Sample Rate of GPS is 10Hz"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Developing the math behind dynamic model\n",
    "\n",
    "All symbolic calculations are made with [Sympy](http://nbviewer.ipython.org/github/jrjohansson/scientific-python-lectures/blob/master/Lecture-5-Sympy.ipynb). Thanks!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "vs, psis, dpsis, dts, xs, ys, lats, lons = symbols('v \\psi \\dot\\psi T x y lat lon')\n",
    "\n",
    "gs = Matrix([[xs+(vs/dpsis)*(sin(psis+dpsis*dts)-sin(psis))],\n",
    "             [ys+(vs/dpsis)*(-cos(psis+dpsis*dts)+cos(psis))],\n",
    "             [psis+dpsis*dts],\n",
    "             [vs],\n",
    "             [dpsis]])\n",
    "state = Matrix([xs,ys,psis,vs,dpsis])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Dynamic Function $g$\n",
    "\n",
    "This formulas calculate how the state is evolving from one to the next time step"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "gs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Calculate the Jacobian of the Dynamic function $g$ with respect to the state vector $x$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "state"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "It has to be computed on every filter step because it consists of state variables!\n",
    "\n",
    "To Sympy Team: A `.to_python` and `.to_c` and `.to_matlab` whould be nice to generate code, like it already works with `print latex()`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Initial Uncertainty $P_0$\n",
    "\n",
    "Initialized with $0$ means you are pretty sure where the vehicle starts"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "P = np.diag([1000.0, 1000.0, 1000.0, 1000.0, 1000.0])\n",
    "print(P, P.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(5, 5))\n",
    "im = plt.imshow(P, interpolation=\"none\", cmap=plt.get_cmap('binary'))\n",
    "plt.title('Initial Covariance Matrix $P$')\n",
    "ylocs, ylabels = plt.yticks()\n",
    "# set the locations of the yticks\n",
    "plt.yticks(np.arange(6))\n",
    "# set the locations and labels of the yticks\n",
    "plt.yticks(np.arange(5),('$x$', '$y$', '$\\psi$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "xlocs, xlabels = plt.xticks()\n",
    "# set the locations of the yticks\n",
    "plt.xticks(np.arange(6))\n",
    "# set the locations and labels of the yticks\n",
    "plt.xticks(np.arange(5),('$x$', '$y$', '$\\psi$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "plt.xlim([-0.5,4.5])\n",
    "plt.ylim([4.5, -0.5])\n",
    "\n",
    "from mpl_toolkits.axes_grid1 import make_axes_locatable\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax)\n",
    "\n",
    "\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Process Noise Covariance Matrix $Q$\n",
    "\n",
    "\"*The state uncertainty model models the disturbances which excite the linear system. Conceptually, it estimates how bad things can get when the system is run open loop for a given period of time.*\" - Kelly, A. (1994). A 3D state space formulation of a navigation Kalman filter for autonomous vehicles, (May). Retrieved from http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix=html&identifier=ADA282853"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "sGPS     = 0.5*8.8*dt**2  # assume 8.8m/s2 as maximum acceleration, forcing the vehicle\n",
    "sCourse  = 0.1*dt # assume 0.1rad/s as maximum turn rate for the vehicle\n",
    "sVelocity= 8.8*dt # assume 8.8m/s2 as maximum acceleration, forcing the vehicle\n",
    "sYaw     = 1.0*dt # assume 1.0rad/s2 as the maximum turn rate acceleration for the vehicle\n",
    "\n",
    "Q = np.diag([sGPS**2, sGPS**2, sCourse**2, sVelocity**2, sYaw**2])\n",
    "print(Q, Q.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(5, 5))\n",
    "im = plt.imshow(Q, interpolation=\"none\", cmap=plt.get_cmap('binary'))\n",
    "plt.title('Process Noise Covariance Matrix $Q$')\n",
    "ylocs, ylabels = plt.yticks()\n",
    "# set the locations of the yticks\n",
    "plt.yticks(np.arange(8))\n",
    "# set the locations and labels of the yticks\n",
    "plt.yticks(np.arange(7),('$x$', '$y$', '$\\psi$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "xlocs, xlabels = plt.xticks()\n",
    "# set the locations of the yticks\n",
    "plt.xticks(np.arange(8))\n",
    "# set the locations and labels of the yticks\n",
    "plt.xticks(np.arange(7),('$x$', '$y$', '$\\psi$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "plt.xlim([-0.5,4.5])\n",
    "plt.ylim([4.5, -0.5])\n",
    "\n",
    "from mpl_toolkits.axes_grid1 import make_axes_locatable\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Real Measurements"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "#path = './../RaspberryPi-CarPC/TinkerDataLogger/DataLogs/2014/'\n",
    "datafile = '2014-03-26-000-Data.csv'\n",
    "\n",
    "date, \\\n",
    "time, \\\n",
    "millis, \\\n",
    "ax, \\\n",
    "ay, \\\n",
    "az, \\\n",
    "rollrate, \\\n",
    "pitchrate, \\\n",
    "yawrate, \\\n",
    "roll, \\\n",
    "pitch, \\\n",
    "yaw, \\\n",
    "speed, \\\n",
    "course, \\\n",
    "latitude, \\\n",
    "longitude, \\\n",
    "altitude, \\\n",
    "pdop, \\\n",
    "hdop, \\\n",
    "vdop, \\\n",
    "epe, \\\n",
    "fix, \\\n",
    "satellites_view, \\\n",
    "satellites_used, \\\n",
    "temp = np.loadtxt(datafile, delimiter=',', unpack=True, \n",
    "                  converters={1: mdates.strpdate2num('%H%M%S%f'),\n",
    "                              0: mdates.strpdate2num('%y%m%d')},\n",
    "                  skiprows=1)\n",
    "\n",
    "print('Read \\'%s\\' successfully.' % datafile)\n",
    "\n",
    "# A course of 0° means the Car is traveling north bound\n",
    "# and 90° means it is traveling east bound.\n",
    "# In the Calculation following, East is Zero and North is 90°\n",
    "# We need an offset.\n",
    "course =(-course+90.0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Measurement Function $h$\n",
    "\n",
    "Matrix $J_H$ is the Jacobian of the Measurement function $h$ with respect to the state. Function $h$ can be used to compute the predicted measurement from the predicted state.\n",
    "\n",
    "If a GPS measurement is available, the following function maps the state to the measurement."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "hs = Matrix([[xs],\n",
    "             [ys]])\n",
    "hs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "JHs=hs.jacobian(state)\n",
    "JHs"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "If no GPS measurement is available, simply set the corresponding values in $J_h$ to zero."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Measurement Noise Covariance $R$\n",
    "\n",
    "\"In practical use, the uncertainty estimates take on the significance of relative weights of state estimates and measurements. So it is not so much important that uncertainty is absolutely correct as it is that it be relatively consistent across all models\" - Kelly, A. (1994). A 3D state space formulation of a navigation Kalman filter for autonomous vehicles, (May). Retrieved from http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix=html&identifier=ADA282853"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "varGPS = 6.0 # Standard Deviation of GPS Measurement\n",
    "varspeed = 1.0 # Variance of the speed measurement\n",
    "varyaw = 0.1 # Variance of the yawrate measurement\n",
    "R = np.matrix([[varGPS**2, 0.0],\n",
    "               [0.0, varGPS**2]])\n",
    "\n",
    "print(R, R.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(4.5, 4.5))\n",
    "im = plt.imshow(R, interpolation=\"none\", cmap=plt.get_cmap('binary'))\n",
    "plt.title('Measurement Noise Covariance Matrix $R$')\n",
    "ylocs, ylabels = plt.yticks()\n",
    "# set the locations of the yticks\n",
    "plt.yticks(np.arange(5))\n",
    "# set the locations and labels of the yticks\n",
    "plt.yticks(np.arange(4),('$x$', '$y$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "xlocs, xlabels = plt.xticks()\n",
    "# set the locations of the yticks\n",
    "plt.xticks(np.arange(5))\n",
    "# set the locations and labels of the yticks\n",
    "plt.xticks(np.arange(4),('$x$', '$y$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "plt.xlim([-0.5,3.5])\n",
    "plt.ylim([3.5, -0.5])\n",
    "\n",
    "from mpl_toolkits.axes_grid1 import make_axes_locatable\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Identity Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "I = np.eye(numstates)\n",
    "print(I, I.shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "## Approx. Lat/Lon to Meters to check Location"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "RadiusEarth = 6378388.0 # m\n",
    "arc= 2.0*np.pi*(RadiusEarth+altitude)/360.0 # m/°\n",
    "\n",
    "dx = arc * np.cos(latitude*np.pi/180.0) * np.hstack((0.0, np.diff(longitude))) # in m\n",
    "dy = arc * np.hstack((0.0, np.diff(latitude))) # in m\n",
    "\n",
    "mx = np.cumsum(dx)\n",
    "my = np.cumsum(dy)\n",
    "\n",
    "ds = np.sqrt(dx**2+dy**2)\n",
    "\n",
    "GPS=(ds!=0.0).astype('bool') # GPS Trigger for Kalman Filter"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Initial State"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "x = np.matrix([[mx[0], my[0], course[0]/180.0*np.pi, speed[0]/3.6+0.001, yawrate[0]/180.0*np.pi]]).T\n",
    "print(x, x.shape)\n",
    "\n",
    "U=float(np.cos(x[2])*x[3])\n",
    "V=float(np.sin(x[2])*x[3])\n",
    "\n",
    "plt.quiver(x[0], x[1], U, V)\n",
    "plt.scatter(float(x[0]), float(x[1]), s=100)\n",
    "plt.title('Initial Location')\n",
    "plt.axis('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "### Put everything together as a measurement vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "measurements = np.vstack((mx, my))\n",
    "# Lenth of the measurement\n",
    "m = measurements.shape[1]\n",
    "print(measurements.shape)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "# Preallocation for Plotting\n",
    "x0 = []\n",
    "x1 = []\n",
    "x2 = []\n",
    "x3 = []\n",
    "x4 = []\n",
    "x5 = []\n",
    "Zx = []\n",
    "Zy = []\n",
    "Px = []\n",
    "Py = []\n",
    "Pdx= []\n",
    "Pdy= []\n",
    "Pddx=[]\n",
    "Pddy=[]\n",
    "Kx = []\n",
    "Ky = []\n",
    "Kdx= []\n",
    "Kdy= []\n",
    "Kddx=[]\n",
    "dstate=[]\n",
    "\n",
    "\n",
    "def savestates(x, Z, P, K):\n",
    "    x0.append(float(x[0]))\n",
    "    x1.append(float(x[1]))\n",
    "    x2.append(float(x[2]))\n",
    "    x3.append(float(x[3]))\n",
    "    x4.append(float(x[4]))\n",
    "    Zx.append(float(Z[0]))\n",
    "    Zy.append(float(Z[1]))    \n",
    "    Px.append(float(P[0,0]))\n",
    "    Py.append(float(P[1,1]))\n",
    "    Pdx.append(float(P[2,2]))\n",
    "    Pdy.append(float(P[3,3]))\n",
    "    Pddx.append(float(P[4,4]))\n",
    "    Kx.append(float(K[0,0]))\n",
    "    Ky.append(float(K[1,0]))\n",
    "    Kdx.append(float(K[2,0]))\n",
    "    Kdy.append(float(K[3,0]))\n",
    "    Kddx.append(float(K[4,0]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Extended Kalman Filter\n",
    "\n",
    "![Extended Kalman Filter Step](Extended-Kalman-Filter-Step.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "$$x_k= \\begin{bmatrix} x \\\\ y \\\\ \\psi \\\\ v \\\\ \\dot\\psi \\end{bmatrix} = \\begin{bmatrix} \\text{Position X} \\\\ \\text{Position Y} \\\\ \\text{Heading} \\\\ \\text{Velocity} \\\\ \\text{Yaw Rate} \\end{bmatrix} =  \\underbrace{\\begin{matrix}x[0] \\\\ x[1] \\\\ x[2] \\\\ x[3] \\\\ x[4]  \\end{matrix}}_{\\textrm{Python Nomenclature}}$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "outputs": [],
   "source": [
    "for filterstep in range(m):\n",
    "\n",
    "    # Time Update (Prediction)\n",
    "    # ========================\n",
    "    # Project the state ahead\n",
    "    # see \"Dynamic Matrix\"\n",
    "    if np.abs(yawrate[filterstep])<0.0001: # Driving straight\n",
    "        x[0] = x[0] + x[3]*dt * np.cos(x[2])\n",
    "        x[1] = x[1] + x[3]*dt * np.sin(x[2])\n",
    "        x[2] = x[2]\n",
    "        x[3] = x[3]\n",
    "        x[4] = 0.0000001 # avoid numerical issues in Jacobians\n",
    "        dstate.append(0)\n",
    "    else: # otherwise\n",
    "        x[0] = x[0] + (x[3]/x[4]) * (np.sin(x[4]*dt+x[2]) - np.sin(x[2]))\n",
    "        x[1] = x[1] + (x[3]/x[4]) * (-np.cos(x[4]*dt+x[2])+ np.cos(x[2]))\n",
    "        x[2] = (x[2] + x[4]*dt + np.pi) % (2.0*np.pi) - np.pi\n",
    "        x[3] = x[3]\n",
    "        x[4] = x[4]\n",
    "        dstate.append(1)\n",
    "    \n",
    "    # Calculate the Jacobian of the Dynamic Matrix A\n",
    "    # see \"Calculate the Jacobian of the Dynamic Matrix with respect to the state vector\"\n",
    "    a13 = float((x[3]/x[4]) * (np.cos(x[4]*dt+x[2]) - np.cos(x[2])))\n",
    "    a14 = float((1.0/x[4]) * (np.sin(x[4]*dt+x[2]) - np.sin(x[2])))\n",
    "    a15 = float((dt*x[3]/x[4])*np.cos(x[4]*dt+x[2]) - (x[3]/x[4]**2)*(np.sin(x[4]*dt+x[2]) - np.sin(x[2])))\n",
    "    a23 = float((x[3]/x[4]) * (np.sin(x[4]*dt+x[2]) - np.sin(x[2])))\n",
    "    a24 = float((1.0/x[4]) * (-np.cos(x[4]*dt+x[2]) + np.cos(x[2])))\n",
    "    a25 = float((dt*x[3]/x[4])*np.sin(x[4]*dt+x[2]) - (x[3]/x[4]**2)*(-np.cos(x[4]*dt+x[2]) + np.cos(x[2])))\n",
    "    JA = np.matrix([[1.0, 0.0, a13, a14, a15],\n",
    "                    [0.0, 1.0, a23, a24, a25],\n",
    "                    [0.0, 0.0, 1.0, 0.0, dt],\n",
    "                    [0.0, 0.0, 0.0, 1.0, 0.0],\n",
    "                    [0.0, 0.0, 0.0, 0.0, 1.0]])\n",
    "    \n",
    "    \n",
    "    # Project the error covariance ahead\n",
    "    P = JA*P*JA.T + Q\n",
    "    \n",
    "    # Measurement Update (Correction)\n",
    "    # ===============================\n",
    "    # Measurement Function\n",
    "    hx = np.matrix([[float(x[0])],\n",
    "                    [float(x[1])]])\n",
    "\n",
    "    if GPS[filterstep]: # with 10Hz, every 5th step\n",
    "        JH = np.matrix([[1.0, 0.0, 0.0, 0.0, 0.0],\n",
    "                        [0.0, 1.0, 0.0, 0.0, 0.0]])\n",
    "    else: # every other step\n",
    "        JH = np.matrix([[0.0, 0.0, 0.0, 0.0, 0.0],\n",
    "                        [0.0, 0.0, 0.0, 0.0, 0.0]])        \n",
    "    \n",
    "    S = JH*P*JH.T + R\n",
    "    K = (P*JH.T) * np.linalg.inv(S)\n",
    "\n",
    "    # Update the estimate via\n",
    "    Z = measurements[:,filterstep].reshape(JH.shape[0],1)\n",
    "    y = Z - (hx)                         # Innovation or Residual\n",
    "    x = x + (K*y)\n",
    "\n",
    "    # Update the error covariance\n",
    "    P = (I - (K*JH))*P\n",
    "\n",
    "\n",
    "    \n",
    "    # Save states for Plotting\n",
    "    savestates(x, Z, P, K)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## Lets take a look at the filter performance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "def plotP():\n",
    "    fig = plt.figure(figsize=(16,9))\n",
    "    plt.semilogy(range(m),Px, label='$x$')\n",
    "    plt.step(range(m),Py, label='$y$')\n",
    "    plt.step(range(m),Pdx, label='$\\psi$')\n",
    "    plt.step(range(m),Pdy, label='$v$')\n",
    "    plt.step(range(m),Pddx, label='$\\dot \\psi$')\n",
    "\n",
    "    plt.xlabel('Filter Step')\n",
    "    plt.ylabel('')\n",
    "    plt.title('Uncertainty (Elements from Matrix $P$)')\n",
    "    plt.legend(loc='best',prop={'size':22})"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Uncertainties in $P$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "plotP()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(6, 6))\n",
    "im = plt.imshow(P, interpolation=\"none\", cmap=plt.get_cmap('binary'))\n",
    "plt.title('Covariance Matrix $P$ (after %i Filter Steps)' % (m))\n",
    "ylocs, ylabels = plt.yticks()\n",
    "# set the locations of the yticks\n",
    "plt.yticks(np.arange(6))\n",
    "# set the locations and labels of the yticks\n",
    "plt.yticks(np.arange(5),('$x$', '$y$', '$\\psi$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "xlocs, xlabels = plt.xticks()\n",
    "# set the locations of the yticks\n",
    "plt.xticks(np.arange(6))\n",
    "# set the locations and labels of the yticks\n",
    "plt.xticks(np.arange(5),('$x$', '$y$', '$\\psi$', '$v$', '$\\dot \\psi$'), fontsize=22)\n",
    "\n",
    "plt.xlim([-0.5,4.5])\n",
    "plt.ylim([4.5, -0.5])\n",
    "\n",
    "from mpl_toolkits.axes_grid1 import make_axes_locatable\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax)\n",
    "\n",
    "\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "### Kalman Gains"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "fig = plt.figure(figsize=(16,9))\n",
    "plt.step(range(len(measurements[0])),Kx, label='$x$')\n",
    "plt.step(range(len(measurements[0])),Ky, label='$y$')\n",
    "plt.step(range(len(measurements[0])),Kdx, label='$\\psi$')\n",
    "plt.step(range(len(measurements[0])),Kdy, label='$v$')\n",
    "plt.step(range(len(measurements[0])),Kddx, label='$\\dot \\psi$')\n",
    "\n",
    "\n",
    "plt.xlabel('Filter Step')\n",
    "plt.ylabel('')\n",
    "plt.title('Kalman Gain (the lower, the more the measurement fullfill the prediction)')\n",
    "plt.legend(prop={'size':18})\n",
    "plt.ylim([-0.1,0.1]);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## State Vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "def plotx():\n",
    "    fig = plt.figure(figsize=(16,16))\n",
    "\n",
    "    plt.subplot(411)\n",
    "    plt.step(range(len(measurements[0])),x0-mx[0], label='$x$')\n",
    "    plt.step(range(len(measurements[0])),x1-my[0], label='$y$')\n",
    "\n",
    "    plt.title('Extended Kalman Filter State Estimates (State Vector $x$)')\n",
    "    plt.legend(loc='best',prop={'size':22})\n",
    "    plt.ylabel('Position (relative to start) [m]')\n",
    "\n",
    "    plt.subplot(412)\n",
    "    plt.step(range(len(measurements[0])),x2, label='$\\psi$')\n",
    "    plt.step(range(len(measurements[0])),(course/180.0*np.pi+np.pi)%(2.0*np.pi) - np.pi, label='$\\psi$ (from GPS as reference)')\n",
    "    plt.ylabel('Course')\n",
    "    plt.legend(loc='best',prop={'size':16})\n",
    "\n",
    "    plt.subplot(413)\n",
    "    plt.step(range(len(measurements[0])),x3, label='$v$')\n",
    "    plt.step(range(len(measurements[0])),speed/3.6, label='$v$ (from GPS as reference)')\n",
    "    plt.ylabel('Velocity')\n",
    "    plt.ylim([0, 30])\n",
    "    plt.legend(loc='best',prop={'size':16})\n",
    "\n",
    "    plt.subplot(414)\n",
    "    plt.step(range(len(measurements[0])),x4, label='$\\dot \\psi$')\n",
    "    plt.step(range(len(measurements[0])),yawrate/180.0*np.pi, label='$\\dot \\psi$ (from IMU as reference)')\n",
    "    plt.ylabel('Yaw Rate')\n",
    "    plt.ylim([-0.6, 0.6])\n",
    "    plt.legend(loc='best',prop={'size':16})\n",
    "    plt.xlabel('Filter Step')\n",
    "\n",
    "    plt.savefig('Extended-Kalman-Filter-CTRV-State-Estimates.png', dpi=72, transparent=True, bbox_inches='tight')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "plotx()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "## Position x/y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "#%pylab --no-import-all"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "def plotxy():\n",
    "\n",
    "    fig = plt.figure(figsize=(16,9))\n",
    "\n",
    "    # EKF State\n",
    "    plt.quiver(x0,x1,np.cos(x2), np.sin(x2), color='#94C600', units='xy', width=0.05, scale=0.5)\n",
    "    plt.plot(x0,x1, label='EKF Position', c='k', lw=5)\n",
    "\n",
    "    # Measurements\n",
    "    plt.scatter(mx[::5],my[::5], s=50, label='GPS Measurements', marker='+')\n",
    "    #cbar=plt.colorbar(ticks=np.arange(20))\n",
    "    #cbar.ax.set_ylabel(u'EPE', rotation=270)\n",
    "    #cbar.ax.set_xlabel(u'm')\n",
    "\n",
    "    # Start/Goal\n",
    "    plt.scatter(x0[0],x1[0], s=60, label='Start', c='g')\n",
    "    plt.scatter(x0[-1],x1[-1], s=60, label='Goal', c='r')\n",
    "\n",
    "    plt.xlabel('X [m]')\n",
    "    plt.ylabel('Y [m]')\n",
    "    plt.title('Position')\n",
    "    plt.legend(loc='best')\n",
    "    plt.axis('equal')\n",
    "    #plt.tight_layout()\n",
    "\n",
    "    #plt.savefig('Extended-Kalman-Filter-CTRV-Position.png', dpi=72, transparent=True, bbox_inches='tight')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "plotxy()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Detailed View"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "def plotxydetails():\n",
    "    fig = plt.figure(figsize=(12,9))\n",
    "\n",
    "    plt.subplot(221)\n",
    "    # EKF State\n",
    "    #plt.quiver(x0,x1,np.cos(x2), np.sin(x2), color='#94C600', units='xy', width=0.05, scale=0.5)\n",
    "    plt.plot(x0,x1, label='EKF Position', c='g', lw=5)\n",
    "\n",
    "    # Measurements\n",
    "    plt.scatter(mx[::5],my[::5], s=50, label='GPS Measurements', alpha=0.5, marker='+')\n",
    "    #cbar=plt.colorbar(ticks=np.arange(20))\n",
    "    #cbar.ax.set_ylabel(u'EPE', rotation=270)\n",
    "    #cbar.ax.set_xlabel(u'm')\n",
    "\n",
    "    plt.xlabel('X [m]')\n",
    "    plt.xlim(70, 130)\n",
    "    plt.ylabel('Y [m]')\n",
    "    plt.ylim(140, 200)\n",
    "    plt.title('Position')\n",
    "    plt.legend(loc='best')\n",
    "\n",
    "\n",
    "    plt.subplot(222)\n",
    "\n",
    "    # EKF State\n",
    "    #plt.quiver(x0,x1,np.cos(x2), np.sin(x2), color='#94C600', units='xy', width=0.05, scale=0.5)\n",
    "    plt.plot(x0,x1, label='EKF Position', c='g', lw=5)\n",
    "\n",
    "    # Measurements\n",
    "    plt.scatter(mx[::5],my[::5], s=50, label='GPS Measurements', alpha=0.5, marker='+')\n",
    "    #cbar=plt.colorbar(ticks=np.arange(20))\n",
    "    #cbar.ax.set_ylabel(u'EPE', rotation=270)\n",
    "    #cbar.ax.set_xlabel(u'm')\n",
    "\n",
    "    plt.xlabel('X [m]')\n",
    "    plt.xlim(160, 260)\n",
    "    plt.ylabel('Y [m]')\n",
    "    plt.ylim(110, 160)\n",
    "    plt.title('Position')\n",
    "    plt.legend(loc='best')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "plotxydetails()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Conclusion"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "As you can see, complicated analytic calculation of the Jacobian Matrices, but it works pretty well."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "## Write Google Earth KML"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "### Convert back from Meters to Lat/Lon (WGS84)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "latekf = latitude[0] + np.divide(x1,arc)\n",
    "lonekf = longitude[0]+ np.divide(x0,np.multiply(arc,np.cos(latitude*np.pi/180.0)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "### Create Data for KML Path"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "source": [
    "Coordinates and timestamps to be used to locate the car model in time and space\n",
    "The value can be expressed as yyyy-mm-ddThh:mm:sszzzzzz, where T is the separator between the date and the time, and the time zone is either Z (for UTC) or zzzzzz, which represents ±hh:mm in relation to UTC."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "import datetime\n",
    "car={}\n",
    "car['when']=[]\n",
    "car['coord']=[]\n",
    "car['gps']=[]\n",
    "for i in range(len(millis)):\n",
    "    d=datetime.datetime.fromtimestamp(millis[i]/1000.0)\n",
    "    car[\"when\"].append(d.strftime(\"%Y-%m-%dT%H:%M:%SZ\"))\n",
    "    car[\"coord\"].append((lonekf[i], latekf[i], 0))\n",
    "    car[\"gps\"].append((longitude[i], latitude[i], 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "from simplekml import Kml, Model, AltitudeMode, Orientation, Scale, Style, Color"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "# The model path and scale variables\n",
    "car_dae = r'https://raw.githubusercontent.com/balzer82/Kalman/master/car-model.dae'\n",
    "car_scale = 1.0\n",
    "\n",
    "# Create the KML document\n",
    "kml = Kml(name=d.strftime(\"%Y-%m-%d %H:%M\"), open=1)\n",
    "\n",
    "# Create the model\n",
    "model_car = Model(altitudemode=AltitudeMode.clamptoground,\n",
    "                            orientation=Orientation(heading=75.0),\n",
    "                            scale=Scale(x=car_scale, y=car_scale, z=car_scale))\n",
    "\n",
    "# Create the track\n",
    "trk = kml.newgxtrack(name=\"EKF\", altitudemode=AltitudeMode.clamptoground,\n",
    "                     description=\"State Estimation from Extended Kalman Filter with CTRV Model\")\n",
    "\n",
    "# Attach the model to the track\n",
    "trk.model = model_car\n",
    "trk.model.link.href = car_dae\n",
    "\n",
    "# Add all the information to the track\n",
    "trk.newwhen(car[\"when\"])\n",
    "trk.newgxcoord(car[\"coord\"])\n",
    "\n",
    "# Style of the Track\n",
    "trk.iconstyle.icon.href = \"\"\n",
    "trk.labelstyle.scale = 1\n",
    "trk.linestyle.width = 4\n",
    "trk.linestyle.color = '7fff0000'\n",
    "\n",
    "# Add GPS measurement marker\n",
    "fol = kml.newfolder(name=\"GPS Measurements\")\n",
    "sharedstyle = Style()\n",
    "sharedstyle.iconstyle.icon.href = 'http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png'\n",
    "\n",
    "for m in range(len(latitude)):\n",
    "    if GPS[m]:\n",
    "        pnt = fol.newpoint(coords = [(longitude[m],latitude[m])])\n",
    "        pnt.style = sharedstyle\n",
    "\n",
    "# Saving\n",
    "#kml.save(\"Extended-Kalman-Filter-CTRV.kml\")\n",
    "kml.savekmz(\"Extended-Kalman-Filter-CTRV.kmz\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "print('Exported KMZ File for Google Earth')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "To use this notebook as a presentation type:\n",
    "\n",
    "`jupyter-nbconvert --to slides Extended-Kalman-Filter-CTRV.ipynb --reveal-prefix=reveal.js --post serve` \n",
    "\n",
    "Questions? [@Balzer82](https://twitter.com/balzer82)"
   ]
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "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.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}