Ch3.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Ch. 3 Exercises\n",
    "<ol>\n",
    "<li>[Kalman Car](#Kalman_Car)</li>\n",
    "<li>[Measurement](#Measurement)</li>\n",
    "<li>[Derivation](#Derivation)</li>\n",
    "<li>[EKF Linearization](#EKF_Linearization)</li>\n",
    "</ol>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1. Kalman Car <a id=\"Kalman_Car\"></a>\n",
    "\\begin{equation}\n",
    "x_{t} = A_{t}x_{t-1} + B_{t}u_{t} + \\varepsilon_{t} \\\\\n",
    "z_{t} = C_{t}x_{t} + \\delta_{t}\n",
    "\\end{equation}\n",
    "\n",
    "\\begin{equation}\n",
    "Kalman\\_Filter(\\mu_{t-1}, \\Sigma_{t-1}, u_{t}, z_{t}) \\\\\n",
    "\\bar{\\mu}_{t} = A_{t}\\mu_{t-1} + B_{t}u_{t} \\\\\n",
    "\\bar{\\Sigma}_{t} = A_{t}\\Sigma_{t-1}A^{T}_{t} + R_{t}\\\\\n",
    "K_{t} = \\bar{\\Sigma}_{t}C^{T}_{t}(C_{t}\\bar{\\Sigma}_{t}C^{T}_{t} + Q_{t})^{-1} \\\\\n",
    "\\mu_{t} =  \\bar{\\mu}_{t} + K_{t}(z_{t}-C_{t}\\bar{\\mu_{t}}) \\\\\n",
    "\\Sigma_{t} = (I - K_{t}C_{t})\\bar{\\Sigma_{t}}\n",
    "\\end{equation}\n",
    "\n",
    "(a) Minimum State Vector:\n",
    "\\begin{equation}\n",
    "\\mu = \\left[\\begin{array}{c}\n",
    "x \\\\\n",
    "\\dot{x} \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "(b) State Transition Matricies\n",
    "\n",
    "\\begin{equation}\n",
    "A = \\left[\\begin{array}{cc}\n",
    "1 & 1 \\\\\n",
    "0 & 1 \\end{array}\\right],\n",
    "\\space\n",
    "B = \\left[\\begin{array}{c}\n",
    "0 \\\\\n",
    "0 \\end{array}\\right],\n",
    "\\space\n",
    "R = \\left[\\begin{array}{cc}\n",
    "\\frac{1}{4} & \\frac{1}{2} \\\\\n",
    "\\frac{1}{2} & 1 \\end{array}\\right]\n",
    "\\end{equation}\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "\n",
    "#(b) State Transition Matricies\n",
    "import numpy as np\n",
    "from copy import copy\n",
    "A =  np.array([[1, 1], [0, 1]])\n",
    "B = np.array([[0], [0]])\n",
    "# Noise for acceleration directly varies the velocity, but only affects the position with a factor of 1/2\n",
    "# v = a*t\n",
    "# d = v*t + 1/2 a*t**2\n",
    "# variance (V) = [0.5, 1]. R = V * V.T\n",
    "R = np.array([[0.25, 0.5], [0.5, 1.0]])\n",
    "#\n",
    "# p(xt|ut,xt_1) = det(2*pi*Rt)**-0.5 * exp(-0.5*(xt-A*xt_1 - B*ut).T*inv(R)*(xt-A*xt_1*B*ut)\n",
    "#\n",
    "#(c) State Prediction\n",
    "def state_prediction(mu_t_1, sigma_t_1, u_t):\n",
    "    mu_bar_t = A.dot(mu_t_1) + B.dot(u_t)\n",
    "    sigma_bar_t = A.dot(sigma_t_1).dot(A.T) + R\n",
    "    \n",
    "    return (mu_bar_t, sigma_bar_t)\n",
    "\n",
    "mu = np.array([[0], [0]])\n",
    "sigma = np.array([[0, 0], [0, 0]])\n",
    "u = np.array([[0]])\n",
    "\n",
    "for i in range(6):\n",
    "    print('t = %d' % (i))\n",
    "    print('\\t'+'mu = \\n\\t\\t%s' % str(mu).replace('\\n','\\n\\t\\t'))\n",
    "    print('\\t'+'sigma = \\n\\t\\t%s' % str(sigma).replace('\\n','\\n\\t\\t'))\n",
    "    print('')\n",
    "    mu, sigma = state_prediction(mu, sigma, u)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "#(d) Plot the posterior\n",
    "import pylab\n",
    "%pylab inline\n",
    "\n",
    "from matplotlib.patches import Ellipse\n",
    "\n",
    "def generate_ellipse(mu, sigma):\n",
    "    eigen_values, eigen_vectors = np.linalg.eig(sigma)\n",
    "    max_e_index = eigen_values.argmax()\n",
    "    min_e_index = eigen_values.argmin()\n",
    "    max_e_vect = eigen_vectors[:,max_e_index]\n",
    "    angle = np.arctan2(max_e_vect[1], max_e_vect[0])\n",
    "    return Ellipse( xy=mu.flatten(), \n",
    "                    width=np.sqrt(eigen_values[max_e_index]), \n",
    "                    height=np.sqrt(eigen_values[min_e_index]), \n",
    "                    angle=angle * 180.0/np.pi,\n",
    "                    fill=False)\n",
    "\n",
    "mu = np.array([[0], [0]])\n",
    "sigma = np.array([[0, 0], [0, 0]])\n",
    "\n",
    "ellipses = []\n",
    "for i in range(6):\n",
    "    ellipses.append(generate_ellipse(mu, sigma))\n",
    "    mu, sigma = state_prediction(mu, sigma, [0])\n",
    "\n",
    "fig = pylab.figure()\n",
    "ax = fig.add_subplot(111, aspect='equal')\n",
    "for e in ellipses:\n",
    "    ax.add_artist(e)\n",
    "    e.set_clip_box(ax.bbox)\n",
    "    #e.set_alpha(rand())\n",
    "    #e.set_facecolor(rand(3))\n",
    "    \n",
    "ax.set_xlim(-3, 3)\n",
    "ax.set_ylim(-3, 3)\n",
    "ax.set_xlabel('$x$')\n",
    "ax.set_ylabel('$\\dot{x}$')\n",
    "fig.suptitle('1 $\\sigma$ uncertianty ellipse')\n",
    "\n",
    "\n",
    "pylab.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "(f) As $t\\to\\infty$ the ellipse's major axes will be parallel with the x axis, meaning that the current velocity gives no extra information about the position\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "## 2. Measurement <a id=\"Measurement\"></a>\n",
    "We can measure the position with an uncertainty of $\\sigma^2 = 10$\n",
    "\n",
    "(a) Measurement Matricies\n",
    "\\begin{equation}\n",
    "C = \\left[\\begin{array}{cc}\n",
    "1 & 0 \\end{array}\\right],\n",
    "\\space\n",
    "Q = \\left[\\begin{array}{c}\n",
    "10 \\end{array}\\right],\n",
    "\\end{equation}\n",
    "\n",
    "(b) Measurement Update\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "c = np.array([[1,0]])\n",
    "q = np.array([[10]])\n",
    "def measurement_update(mu_bar_t, sigma_bar_t, z_t):\n",
    "    k_t = sigma_bar_t.dot(c.T).dot(np.linalg.inv(c.dot(sigma_bar_t).dot(c.T) + q))\n",
    "    mu_t = mu_bar_t + k_t.dot(z_t - c.dot(mu_bar_t))\n",
    "    sigma_t = (np.eye(k_t.shape[0]) - k_t.dot(c)).dot(sigma_bar_t)\n",
    "    \n",
    "    return mu_t, sigma_t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "mu = np.array([[0], [0]])\n",
    "sigma = np.array([[0, 0], [0, 0]])\n",
    "u = np.array([[0]])\n",
    "\n",
    "for i in range(5):\n",
    "    mu, sigma = state_prediction(mu, sigma, u)\n",
    "    \n",
    "before_ellipse = generate_ellipse(mu, sigma)\n",
    "\n",
    "mu, sigma = measurement_update(mu, sigma, np.array([[5]]))\n",
    "\n",
    "after_ellipse = generate_ellipse(mu, sigma)\n",
    "\n",
    "fig = pylab.figure()\n",
    "ax = fig.add_subplot(111, aspect='equal')\n",
    "for e in (before_ellipse, after_ellipse):\n",
    "    ax.add_artist(e)\n",
    "    e.set_clip_box(ax.bbox)\n",
    "    #e.set_alpha(rand())\n",
    "    #e.set_facecolor(rand(3))\n",
    "    \n",
    "ax.set_xlim(-5.5, 5.5)\n",
    "ax.set_ylim(-5.5, 5.5)\n",
    "ax.set_xlabel('$x$')\n",
    "ax.set_ylabel('$\\dot{x}$')\n",
    "fig.suptitle('1 $\\sigma$ uncertianty ellipse')\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Derivation  <a id=\"Derivation\"></a>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "## 4. EKF Linearization <a id=\"EKF_Linearization\"></a>\n",
    "\n",
    "\\begin{equation}\n",
    "Extended\\_Kalman\\_Filter(\\mu_{t-1}, \\Sigma_{t-1}, u_{t}, z_{t}) \\\\\n",
    "\\bar{\\mu}_{t} = g(u_{t}, \\mu_{t-1})\\\\\n",
    "\\bar{\\Sigma}_{t} = G_{t}\\Sigma_{t-1}G^{T}_{t} + R_{t}\\\\\n",
    "K_{t} = \\bar{\\Sigma}_{t}H^{T}_{t}(H_{t}\\bar{\\Sigma}_{t}H^{T}_{t} + Q_{t})^{-1} \\\\\n",
    "\\mu_{t} =  \\bar{\\mu}_{t} + K_{t}(z_{t}-h(\\bar{\\mu_{t}})) \\\\\n",
    "\\Sigma_{t} = (I - K_{t}H_{t})\\bar{\\Sigma_{t}}\n",
    "\\end{equation}\n",
    "\n",
    "We have have a robot whose location is well known, but it's orientation is unknown\n",
    "\\begin{equation}\n",
    "state = \\left[\\begin{array}{ccc}\n",
    "x \\\\ y \\\\ \\sigma \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "\\begin{equation}\n",
    "\\mu = \\left[\\begin{array}{c}\n",
    "0 \\\\ 0 \\\\ 0 \\end{array}\\right],\n",
    "\\space\n",
    "\\sigma = \\left[\\begin{array}{ccc}\n",
    "0.01 & 0 & 0 \\\\\n",
    "0 & 0.01 & 0 \\\\\n",
    "0 & 0 & 10000 \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "The expected location after the robot moves $d = 1$ unit forward is given by\n",
    "\n",
    "\\begin{equation}\n",
    "\\left[\\begin{array}{c}\n",
    "x^{\\prime} \\\\ y^{\\prime} \\\\ \\theta^{\\prime} \\end{array}\\right]\n",
    " = \\left[\\begin{array}{c}\n",
    "x + cos(\\theta) \\\\ y + sin(\\theta) \\\\ \\theta \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "(a) Draw your best model of the posterior over the robot pose in x and y.\n",
    "\n",
    "I have no knowledge of the heading so I'm going to guess that it will be within a circle of radius 1, plus or minus the uncertintanty of 0.01"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "my_model_inner = Ellipse( xy=[0,0], width=2*1-0.01, height=2*1-0.01, angle=0, fill=False)\n",
    "my_model_outer = Ellipse( xy=[0,0], width=2*1+0.01, height=2*1+0.01, angle=0, fill=False)\n",
    "\n",
    "fig = pylab.figure()\n",
    "ax = fig.add_subplot(111, aspect='equal')\n",
    "for e in (my_model_inner, my_model_outer):\n",
    "    ax.add_artist(e)\n",
    "    e.set_clip_box(ax.bbox)\n",
    "    \n",
    "ax.set_xlim(-1.1, 1.1); ax.set_ylim(-1.1, 1.1)\n",
    "ax.set_xlabel('$x$'); ax.set_ylabel('$y$')\n",
    "fig.suptitle('1 $\\sigma$ uncertianty ellipse')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(b) EKF Prediction\n",
    "\n",
    "Since the state does not depend on time the A matrix is the identity. \n",
    "\\begin{equation}\n",
    "A = \\left[\\begin{array}{ccc}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & 1 & 0 \\\\\n",
    "0 & 0 & 1 \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "The nonlinear update equation g is given by \n",
    "\\begin{equation}\n",
    "x_{t-1} = \\left[\\begin{array}{c}\n",
    "x_{1,t-1} \\\\\n",
    "x_{2,t-1} \\\\\n",
    "x_{3,t-1} \\end{array}\\right],\n",
    "\\space\n",
    "g(u_{t}, x_{t-1}) = \\left[\\begin{array}{c}\n",
    "x_{1,t-1} + u_{t}*cos(x_{3,t-1}) \\\\\n",
    "x_{2,t-1} + u_{t}*sin(x_{3,t-1}) \\\\\n",
    "x_{3,t-1} \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "The definition of the Jacobian (from Wikipedia)\n",
    "\\begin{equation}\n",
    "J = \\frac{d\\mathbf f}{d\\mathbf x} = \\begin{bmatrix}\n",
    "    \\dfrac{\\partial \\mathbf{f}}{\\partial x_1}; \\cdots; \\dfrac{\\partial \\mathbf{f}}{\\partial x_n} \\end{bmatrix}\n",
    "= \\begin{bmatrix}\n",
    "    \\dfrac{\\partial f_1}{\\partial x_1}; \\cdots; \\dfrac{\\partial f_1}{\\partial x_n}\\\\\n",
    "    \\vdots; \\ddots; \\vdots\\\\\n",
    "    \\dfrac{\\partial f_m}{\\partial x_1}; \\cdots; \\dfrac{\\partial f_m}{\\partial x_n} \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "The partial derivitive (Jacobain) $g^{\\prime}(u_{t}, x_{t-1})=\\frac{\\delta g(u_{t}, x_{t-1})}{\\delta x_{t-1}}$ is given by\n",
    "\n",
    "\\begin{equation}\n",
    "g^{\\prime}(u_{t}, x_{t-1}) = \\left[\\begin{array}{ccc}\n",
    "1 & 0 & u_{t}*sin(x_{3,t-1}) \\\\\n",
    "0 & 1 & -u_{t}*cos(x_{3,t-1}) \\\\\n",
    "0 & 0 & 1 \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "Linearizing at $t=0$ gives\n",
    "\\begin{equation}\n",
    "G_{0} = g^{\\prime}(u_{t}, x_{t-1}) =\n",
    "\\left[\\begin{array}{ccc}\n",
    "1 & 0 & 1*sin(0) \\\\\n",
    "0 & 1 & -1*cos(0) \\\\\n",
    "0 & 0 & 1 \\end{array}\\right] =\n",
    "\\left[\\begin{array}{ccc}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & 1 & -1 \\\\\n",
    "0 & 0 & 1 \\end{array}\\right]\n",
    "\\end{equation}\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "mu_t = np.array([[0], [0], [0]])\n",
    "sigma_t = np.array([[0.01, 0, 0], [0, 0.01, 0], [0, 0, 10000]])\n",
    "u_t = np.array([[1]])\n",
    "R_t = np.array([[0], [0], [0]])\n",
    "\n",
    "def g(u_t, mu_t_1):\n",
    "    \"\"\"Returns mu_bar_t\"\"\"\n",
    "    return mu_t_1 + np.array([[u_t[0,0]*np.cos(mu_t_1[0,0])],\n",
    "                              [u_t[0,0]*np.sin(mu_t_1[1,0])],\n",
    "                              [0]])\n",
    "\n",
    "def G(u_t, mu_t_1):\n",
    "    \"\"\"Returns G_t\"\"\"\n",
    "    return np.array([[1, 0, u_t[0,0]*np.sin(mu_t_1[2,0])], \n",
    "                     [0, 1, -u_t[0,0]*np.cos(mu_t_1[2,0])], \n",
    "                     [0, 0, 1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "mu_bar_t = g(u_t, mu_t)\n",
    "G_t = G(u_t, mu_t)\n",
    "sigma_bar_t = G_t.dot(sigma_t).dot(G_t.T)+R_t\n",
    "G_t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#(c) Draw the uncertainty ellipse\n",
    "\n",
    "#get just the x and y state variables\n",
    "mu_xy = mu_bar_t[:2,:]\n",
    "sigma_xy = sigma_bar_t[:2,:2]\n",
    "\n",
    "uncertainty_e = generate_ellipse(mu_xy, sigma_xy)\n",
    "\n",
    "my_model_inner = Ellipse( xy=[0,0], width=2*1-0.01, height=2*1-0.01, angle=0, fill=False)\n",
    "my_model_outer = Ellipse( xy=[0,0], width=2*1+0.01, height=2*1+0.01, angle=0, fill=False)\n",
    "\n",
    "fig = pylab.figure()\n",
    "ax1 = fig.add_subplot(111, aspect='equal')\n",
    "for e in (my_model_inner, my_model_outer, uncertainty_e):\n",
    "    ax1.add_artist(e)\n",
    "    e.set_clip_box(ax1.bbox)  \n",
    "ax1.set_xlim(-55, 55); ax1.set_ylim(-55, 55)\n",
    "ax1.set_xlabel('$x$'); ax1.set_ylabel('$y$')\n",
    "\n",
    "fig.suptitle('1 $\\sigma$ uncertianty ellipse')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "(d) Noisy measurement with $Q = 0.01$ of the robot's x position\n",
    "\n",
    "The measurement model \n",
    "\\begin{equation}\n",
    "H = \\left[\\begin{array}{ccc}\n",
    "1 & 0 & 0 \\end{array}\\right],\n",
    "\\space\n",
    "Q = \\left[\\begin{array}{c}\n",
    "0.01 \\end{array}\\right],\n",
    "\\end{equation}\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "H = np.array([[1, 0, 0]])\n",
    "Q = np.array([[0.01]])\n",
    "\n",
    "def ekf_measurement_update(mu_bar_t, sigma_bar_t, z_t):\n",
    "    k_t = sigma_bar_t.dot(H.T).dot(np.linalg.inv(H.dot(sigma_bar_t).dot(H.T) + Q))\n",
    "    mu_t = mu_bar_t + k_t.dot(z_t - H.dot(mu_bar_t))\n",
    "    sigma_t = (np.eye(k_t.shape[0]) - k_t.dot(H)).dot(sigma_bar_t)\n",
    "    return mu_t, sigma_t\n",
    "\n",
    "z_t = np.array([[0]])\n",
    "\n",
    "mu_t, sigma_t = ekf_measurement_update(mu_bar_t, sigma_bar_t, z_t)\n",
    "\n",
    "\n",
    "#get just the x and y state variables\n",
    "mu_xy = mu_t[:2,:]\n",
    "sigma_xy = sigma_t[:2,:2]\n",
    "\n",
    "uncertainty_e = generate_ellipse(mu_xy, sigma_xy)\n",
    "my_model_inner = Ellipse( xy=[0,0], width=2*1-0.01, height=2*1-0.01, angle=0, fill=False)\n",
    "my_model_outer = Ellipse( xy=[0,0], width=2*1+0.01, height=2*1+0.01, angle=0, fill=False)\n",
    "\n",
    "fig = pylab.figure()\n",
    "ax1 = fig.add_subplot(111, aspect='equal')\n",
    "for e in (my_model_inner, my_model_outer, uncertainty_e):\n",
    "    ax1.add_artist(e)\n",
    "    e.set_clip_box(ax1.bbox)  \n",
    "ax1.set_xlim(-55, 55); ax1.set_ylim(-55, 55)\n",
    "ax1.set_xlabel('$x$'); ax1.set_ylabel('$y$')\n",
    "\n",
    "fig.suptitle('1 $\\sigma$ uncertianty ellipse')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "The measurement moves the uncertainty ellipse along the $x$ axis to half way between the measurement and the prior estimation of $x=1$.\n",
    "Intutiviely, if we got the x measurement, we would be able to limit the y uncertainty to two locations.\n",
    "\n",
    "(e) The differences are rather significant between the EKF and my intutive results. The results could be made more accurate by reducing the uncertainty of the angle and increasing the uncertainty of the position. This would prevent the results from getting so distorted.\n",
    "\n",
    "If the angle was known, but the y coordinate was not known, the ellipse would look similar to after the linearization, but would track the robot more accurately."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}