{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# EKF SLAM"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 151804 characters]\n",
      "text/plain": [
       "<IPython.core.display.Image object>"
      ]
     },
     "execution_count": 11,
     "metadata": {
      "image/png": {
       "width": 600
      }
     },
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from IPython.display import Image\n",
    "Image(filename=\"animation.png\",width=600)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Simulation\n",
    "\n",
    "This is a simulation of EKF SLAM. \n",
    "\n",
    "- Black stars: landmarks\n",
    "- Green crosses: estimates of landmark positions\n",
    "- Black line: dead reckoning \n",
    "- Blue line: ground truth\n",
    "- Red line: EKF SLAM position estimation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction\n",
    "\n",
    "EKF SLAM models the SLAM problem in a single EKF where the modeled state is both the pose $(x, y, \\theta)$ and \n",
    "an array of landmarks $[(x_1, y_1), (x_2, x_y), ... , (x_n, y_n)]$ for $n$ landmarks. The covariance between each of the positions and landmarks are also tracked. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\\begin{equation}\n",
    "X = \\begin{bmatrix} x \\\\ y \\\\ \\theta \\\\ x_1 \\\\ y_1 \\\\ x_2 \\\\ y_2 \\\\ \\dots \\\\ x_n \\\\ y_n \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\n",
    "\\begin{equation}\n",
    "P = \\begin{bmatrix} \n",
    "\\sigma_{xx} & \\sigma_{xy} & \\sigma_{x\\theta} & \\sigma_{xx_1} & \\sigma_{xy_1} & \\sigma_{xx_2} & \\sigma_{xy_2} & \\dots & \\sigma_{xx_n} & \\sigma_{xy_n} \\\\\n",
    "\\sigma_{yx} & \\sigma_{yy} & \\sigma_{y\\theta} & \\sigma_{yx_1} & \\sigma_{yy_1} & \\sigma_{yx_2} & \\sigma_{yy_2} & \\dots & \\sigma_{yx_n} & \\sigma_{yy_n} \\\\\n",
    " &  &  &  & \\vdots &  &  &  &  &  \\\\\n",
    "\\sigma_{x_nx} & \\sigma_{x_ny} & \\sigma_{x_n\\theta} & \\sigma_{x_nx_1} & \\sigma_{x_ny_1} & \\sigma_{x_nx_2} & \\sigma_{x_ny_2} & \\dots & \\sigma_{x_nx_n} & \\sigma_{x_ny_n}\n",
    "\\end{bmatrix}\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A single estimate of the pose is tracked over time, while the confidence in the pose is tracked by the \n",
    "covariance matrix $P$. $P$ is a symmetric square matrix whith each element in the matrix corresponding to the \n",
    "covariance between two parts of the system. For example, $\\sigma_{xy}$ represents the covariance between the \n",
    "belief of $x$ and $y$ and is equal to $\\sigma_{yx}$. \n",
    "\n",
    "The state can be represented more concisely as follows. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\\begin{equation}\n",
    "X = \\begin{bmatrix} x \\\\ m \\end{bmatrix}\n",
    "\\end{equation}\n",
    "\\begin{equation}\n",
    "P = \\begin{bmatrix} \n",
    "\\Sigma_{xx} & \\Sigma_{xm}\\\\\n",
    "\\Sigma_{mx} & \\Sigma_{mm}\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here the state simplifies to a combination of pose ($x$) and map ($m$). The covariance matrix becomes easier to \n",
    "understand and simply reads as the uncertainty of the robots pose ($\\Sigma_{xx}$), the uncertainty of the \n",
    "map ($\\Sigma_{mm}$), and the uncertainty of the robots pose with respect to the map and vice versa \n",
    "($\\Sigma_{xm}$, $\\Sigma_{mx}$).\n",
    "\n",
    "Take care to note the difference between $X$ (state) and $x$ (pose). "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "Extended Kalman Filter SLAM example\n",
    "original author: Atsushi Sakai (@Atsushi_twi)\n",
    "notebook author: Andrew Tu (drewtu2)\n",
    "\"\"\"\n",
    "\n",
    "import math\n",
    "import numpy as np\n",
    "%matplotlib notebook\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "\n",
    "# EKF state covariance\n",
    "Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Change in covariance\n",
    "\n",
    "#  Simulation parameter\n",
    "Qsim = np.diag([0.2, np.deg2rad(1.0)])**2  # Sensor Noise\n",
    "Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 # Process Noise\n",
    "\n",
    "DT = 0.1  # time tick [s]\n",
    "SIM_TIME = 50.0  # simulation time [s]\n",
    "MAX_RANGE = 20.0  # maximum observation range\n",
    "M_DIST_TH = 2.0  # Threshold of Mahalanobis distance for data association.\n",
    "STATE_SIZE = 3  # State size [x,y,yaw]\n",
    "LM_SIZE = 2  # LM state size [x,y]\n",
    "\n",
    "show_animation = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Algorithm Walkthrough\n",
    "\n",
    "At each time step, the following is done. \n",
    "- predict the new state using the control functions\n",
    "- update the belief in landmark positions based on the estimated state and measurements"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ekf_slam(xEst, PEst, u, z):\n",
    "    \"\"\"\n",
    "    Performs an iteration of EKF SLAM from the available information. \n",
    "    \n",
    "    :param xEst: the belief in last position\n",
    "    :param PEst: the uncertainty in last position\n",
    "    :param u:    the control function applied to the last position \n",
    "    :param z:    measurements at this step\n",
    "    :returns:    the next estimated position and associated covariance\n",
    "    \"\"\"\n",
    "    S = STATE_SIZE\n",
    "\n",
    "    # Predict\n",
    "    xEst, PEst, G, Fx = predict(xEst, PEst, u)\n",
    "    initP = np.eye(2)\n",
    "\n",
    "    # Update\n",
    "    xEst, PEst = update(xEst, PEst, u, z, initP)\n",
    "\n",
    "    return xEst, PEst\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 1- Predict\n",
    "**Predict State update:** The following equations describe the predicted motion model of the robot in case we provide only the control $(v,w)$, which are the linear and angular velocity repsectively. \n",
    "\n",
    "$\\begin{equation*}\n",
    "F=\n",
    "\\begin{bmatrix}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & 1 & 0 \\\\\n",
    "0 & 0 & 1 \n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "B=\n",
    "\\begin{bmatrix}\n",
    "\\Delta t cos(\\theta) & 0\\\\\n",
    "\\Delta t sin(\\theta) & 0\\\\\n",
    "0 & \\Delta t\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "U=\n",
    "\\begin{bmatrix}\n",
    "v_t\\\\\n",
    "w_t\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "X = FX + BU \n",
    "\\end{equation*}$\n",
    "\n",
    "\n",
    "$\\begin{equation*}\n",
    "\\begin{bmatrix}\n",
    "x_{t+1} \\\\\n",
    "y_{t+1} \\\\\n",
    "\\theta_{t+1}\n",
    "\\end{bmatrix}=\n",
    "\\begin{bmatrix}\n",
    "1 & 0 & 0 \\\\\n",
    "0 & 1 & 0 \\\\\n",
    "0 & 0 & 1 \n",
    "\\end{bmatrix}\\begin{bmatrix}\n",
    "x_{t} \\\\\n",
    "y_{t} \\\\\n",
    "\\theta_{t}\n",
    "\\end{bmatrix}+\n",
    "\\begin{bmatrix}\n",
    "\\Delta t cos(\\theta) & 0\\\\\n",
    "\\Delta t sin(\\theta) & 0\\\\\n",
    "0 & \\Delta t\n",
    "\\end{bmatrix}\n",
    "\\begin{bmatrix}\n",
    "v_{t} + \\sigma_v\\\\\n",
    "w_{t} + \\sigma_w\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "Notice that while $U$ is only defined by $v_t$ and $w_t$, in the actual calcuations, a $+\\sigma_v$ and \n",
    "$+\\sigma_w$ appear. These values represent the error bewteen the given control inputs and the actual control \n",
    "inputs. \n",
    "\n",
    "As a result, the simulation is set up as the following. $R$ represents the process noise which is added to the \n",
    "control inputs to simulate noise experienced in the real world. A set of truth values are computed from the raw \n",
    "control values while the values dead reckoning values incorporate the error into the estimation. \n",
    "\n",
    "$\\begin{equation*}\n",
    "R=\n",
    "\\begin{bmatrix}\n",
    "\\sigma_v\\\\\n",
    "\\sigma_w\\\\\n",
    "\\end{bmatrix}\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "X_{true} = FX + B(U)\n",
    "\\end{equation*}$\n",
    "\n",
    "$\\begin{equation*}\n",
    "X_{DR} = FX + B(U + R)\n",
    "\\end{equation*}$\n",
    "\n",
    "The implementation of the motion model prediciton code is shown in `motion_model`. The `observation` function \n",
    "shows how the simulation uses (or doesn't use) the process noise `Rsim` to the find the ground truth and dead reckoning estimtates of the pose.\n",
    "\n",
    "**Predict covariance:** Add the state covariance to the the current uncertainty of the EKF. At each time step, the uncertainty in the system grows by the covariance of the pose, $Cx$. \n",
    "\n",
    "$\n",
    "P = G^TPG + Cx\n",
    "$\n",
    "\n",
    "Notice this uncertainty is only growing with respect to the pose, not the landmarks. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def predict(xEst, PEst, u):\n",
    "    \"\"\"\n",
    "    Performs the prediction step of EKF SLAM\n",
    "    \n",
    "    :param xEst: nx1 state vector\n",
    "    :param PEst: nxn covariacne matrix\n",
    "    :param u:    2x1 control vector\n",
    "    :returns:    predicted state vector, predicted covariance, jacobian of control vector, transition fx\n",
    "    \"\"\"\n",
    "    S = STATE_SIZE\n",
    "    xEst[0:S] = motion_model(xEst[0:S], u)\n",
    "    G, Fx = jacob_motion(xEst[0:S], u)\n",
    "    # Fx is an an identity matrix of size (STATE_SIZE)\n",
    "    # sigma = G*sigma*G.T + Noise\n",
    "    PEst[0:S, 0:S] = G.T * PEst[0:S, 0:S] * G + Fx.T * Cx * Fx\n",
    "    return xEst, PEst, G, Fx"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def motion_model(x, u):\n",
    "    \"\"\"\n",
    "    Computes the motion model based on current state and input function. \n",
    "    \n",
    "    :param x: 3x1 pose estimation\n",
    "    :param u: 2x1 control input [v; w]\n",
    "    :returns: the resutling state after the control function is applied\n",
    "    \"\"\"\n",
    "    F = np.array([[1.0, 0, 0],\n",
    "                  [0, 1.0, 0],\n",
    "                  [0, 0, 1.0]])\n",
    "\n",
    "    B = np.array([[DT * math.cos(x[2, 0]), 0],\n",
    "                  [DT * math.sin(x[2, 0]), 0],\n",
    "                  [0.0, DT]])\n",
    "\n",
    "    x = (F @ x) + (B @ u)\n",
    "    return x"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2 - Update\n",
    "In the update phase, the observations of nearby landmarks are used to correct the location estimate. \n",
    "\n",
    "For every landmark observed, it is associated to a particular landmark in the known map. If no landmark exists \n",
    "in the position surrounding the landmark, it is taken as a NEW landmark. The distance threshold for how far a \n",
    "landmark must be from the next known landmark before its considered to be a new landmark is set by `M_DIST_TH`.\n",
    "\n",
    "With an observation associated to the appropriate landmark, the **innovation** can be calculated. Innovation \n",
    "($y$) is the difference between the observation and the observation that *should* have been made if the \n",
    "observation were made from the pose predicted in the predict stage.\n",
    "\n",
    "$\n",
    "y = z_t - h(X)\n",
    "$\n",
    "\n",
    "With the innovation calculated, the question becomes which to trust more - the observations or the predictions? \n",
    "To determine this, we calculate the Kalman Gain - a percent of how much of the innovation to add to the \n",
    "prediction based on the uncertainty in the predict step and the update step. \n",
    "\n",
    "$\n",
    "K = \\bar{P_t}H_t^T(H_t\\bar{P_t}H_t^T + Q_t)^{-1}\n",
    "$\n",
    "In these equations, $H$ is the jacobian of the measurement function. The multiplications by $H^T$ and $H$ \n",
    "represent the application of the delta to the measurement covariance. \n",
    "Intuitively, this equation is applying the following from the single variate Kalman equation but in the \n",
    "multivariate form, i.e. finding the ratio of the uncertianty of the process compared the measurment. \n",
    "\n",
    "$\n",
    "K = \\frac{\\bar{P_t}}{\\bar{P_t} + Q_t}\n",
    "$\n",
    "\n",
    "If $Q_t << \\bar{P_t}$, (i.e. the measurement covariance is low relative to the current estimate), then the \n",
    "Kalman gain will be $~1$. This results in adding all of the innovation to the estimate -- and therefore \n",
    "completely believing the measurement. \n",
    "\n",
    "However, if $Q_t >> \\bar{P_t}$ then the Kalman gain will go to 0, signaling a high trust in the process \n",
    "and little trust in the measurement.\n",
    "\n",
    "The update is captured in the following. \n",
    "\n",
    "$\n",
    "xUpdate = xEst + (K * y)\n",
    "$\n",
    "\n",
    "Of course, the covariance must also be updated as well to account for the changing uncertainty. \n",
    "\n",
    "$\n",
    "P_{t} = (I-K_tH_t)\\bar{P_t}\n",
    "$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "def update(xEst, PEst, u, z, initP):\n",
    "    \"\"\"\n",
    "    Performs the update step of EKF SLAM\n",
    "    \n",
    "    :param xEst:  nx1 the predicted pose of the system and the pose of the landmarks\n",
    "    :param PEst:  nxn the predicted covariance\n",
    "    :param u:     2x1 the control function \n",
    "    :param z:     the measurements read at new position\n",
    "    :param initP: 2x2 an identity matrix acting as the initial covariance\n",
    "    :returns:     the updated state and covariance for the system\n",
    "    \"\"\"\n",
    "    for iz in range(len(z[:, 0])):  # for each observation\n",
    "        minid = search_correspond_LM_ID(xEst, PEst, z[iz, 0:2]) # associate to a known landmark\n",
    "\n",
    "        nLM = calc_n_LM(xEst) # number of landmarks we currently know about\n",
    "        \n",
    "        if minid == nLM: # Landmark is a NEW landmark\n",
    "            print(\"New LM\")\n",
    "            # Extend state and covariance matrix\n",
    "            xAug = np.vstack((xEst, calc_LM_Pos(xEst, z[iz, :])))\n",
    "            PAug = np.vstack((np.hstack((PEst, np.zeros((len(xEst), LM_SIZE)))),\n",
    "                              np.hstack((np.zeros((LM_SIZE, len(xEst))), initP))))\n",
    "            xEst = xAug\n",
    "            PEst = PAug\n",
    "        \n",
    "        lm = get_LM_Pos_from_state(xEst, minid)\n",
    "        y, S, H = calc_innovation(lm, xEst, PEst, z[iz, 0:2], minid)\n",
    "\n",
    "        K = (PEst @ H.T) @ np.linalg.inv(S) # Calculate Kalman Gain\n",
    "        xEst = xEst + (K @ y)\n",
    "        PEst = (np.eye(len(xEst)) - (K @ H)) @ PEst\n",
    "    \n",
    "    xEst[2] = pi_2_pi(xEst[2])\n",
    "    return xEst, PEst\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_innovation(lm, xEst, PEst, z, LMid):\n",
    "    \"\"\"\n",
    "    Calculates the innovation based on expected position and landmark position\n",
    "    \n",
    "    :param lm:   landmark position\n",
    "    :param xEst: estimated position/state\n",
    "    :param PEst: estimated covariance\n",
    "    :param z:    read measurements\n",
    "    :param LMid: landmark id\n",
    "    :returns:    returns the innovation y, and the jacobian H, and S, used to calculate the Kalman Gain\n",
    "    \"\"\"\n",
    "    delta = lm - xEst[0:2]\n",
    "    q = (delta.T @ delta)[0, 0]\n",
    "    zangle = math.atan2(delta[1, 0], delta[0, 0]) - xEst[2, 0]\n",
    "    zp = np.array([[math.sqrt(q), pi_2_pi(zangle)]])\n",
    "    # zp is the expected measurement based on xEst and the expected landmark position\n",
    "    \n",
    "    y = (z - zp).T # y = innovation\n",
    "    y[1] = pi_2_pi(y[1])\n",
    "    \n",
    "    H = jacobH(q, delta, xEst, LMid + 1)\n",
    "    S = H @ PEst @ H.T + Cx[0:2, 0:2]\n",
    "\n",
    "    return y, S, H\n",
    "\n",
    "def jacobH(q, delta, x, i):\n",
    "    \"\"\"\n",
    "    Calculates the jacobian of the measurement function\n",
    "    \n",
    "    :param q:     the range from the system pose to the landmark\n",
    "    :param delta: the difference between a landmark position and the estimated system position\n",
    "    :param x:     the state, including the estimated system position\n",
    "    :param i:     landmark id + 1\n",
    "    :returns:     the jacobian H\n",
    "    \"\"\"\n",
    "    sq = math.sqrt(q)\n",
    "    G = np.array([[-sq * delta[0, 0], - sq * delta[1, 0], 0, sq * delta[0, 0], sq * delta[1, 0]],\n",
    "                  [delta[1, 0], - delta[0, 0], - 1.0, - delta[1, 0], delta[0, 0]]])\n",
    "\n",
    "    G = G / q\n",
    "    nLM = calc_n_LM(x)\n",
    "    F1 = np.hstack((np.eye(3), np.zeros((3, 2 * nLM))))\n",
    "    F2 = np.hstack((np.zeros((2, 3)), np.zeros((2, 2 * (i - 1))),\n",
    "                    np.eye(2), np.zeros((2, 2 * nLM - 2 * i))))\n",
    "\n",
    "    F = np.vstack((F1, F2))\n",
    "\n",
    "    H = G @ F\n",
    "\n",
    "    return H"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Observation Step\n",
    "The observation step described here is outside the main EKF SLAM process and is primarily used as a method of\n",
    "driving the simulation. The observations funciton is in charge of calcualting how the poses of the robots change \n",
    "and accumulate error over time, and the theoretical measuremnts that are expected as a result of each \n",
    "measurement. \n",
    "\n",
    "Observations are based on the TRUE position of the robot. Error in dead reckoning and control functions are \n",
    "passed along here as well. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "def observation(xTrue, xd, u, RFID):\n",
    "    \"\"\"\n",
    "    :param xTrue: the true pose of the system\n",
    "    :param xd:    the current noisy estimate of the system\n",
    "    :param u:     the current control input\n",
    "    :param RFID:  the true position of the landmarks\n",
    "    \n",
    "    :returns:     Computes the true position, observations, dead reckoning (noisy) position, \n",
    "                  and noisy control function\n",
    "    \"\"\"\n",
    "    xTrue = motion_model(xTrue, u)\n",
    "\n",
    "    # add noise to gps x-y\n",
    "    z = np.zeros((0, 3))\n",
    "\n",
    "    for i in range(len(RFID[:, 0])): # Test all beacons, only add the ones we can see (within MAX_RANGE)\n",
    "\n",
    "        dx = RFID[i, 0] - xTrue[0, 0]\n",
    "        dy = RFID[i, 1] - xTrue[1, 0]\n",
    "        d = math.sqrt(dx**2 + dy**2)\n",
    "        angle = pi_2_pi(math.atan2(dy, dx) - xTrue[2, 0])\n",
    "        if d <= MAX_RANGE:\n",
    "            dn = d + np.random.randn() * Qsim[0, 0]  # add noise\n",
    "            anglen = angle + np.random.randn() * Qsim[1, 1]  # add noise\n",
    "            zi = np.array([dn, anglen, i])\n",
    "            z = np.vstack((z, zi))\n",
    "\n",
    "    # add noise to input\n",
    "    ud = np.array([[\n",
    "        u[0, 0] + np.random.randn() * Rsim[0, 0],\n",
    "        u[1, 0] + np.random.randn() * Rsim[1, 1]]]).T\n",
    "\n",
    "    xd = motion_model(xd, ud)\n",
    "    return xTrue, z, xd, ud"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_n_LM(x):\n",
    "    \"\"\"\n",
    "    Calculates the number of landmarks currently tracked in the state\n",
    "    :param x: the state\n",
    "    :returns: the number of landmarks n\n",
    "    \"\"\"\n",
    "    n = int((len(x) - STATE_SIZE) / LM_SIZE)\n",
    "    return n\n",
    "\n",
    "\n",
    "def jacob_motion(x, u):\n",
    "    \"\"\"\n",
    "    Calculates the jacobian of motion model. \n",
    "    \n",
    "    :param x: The state, including the estimated position of the system\n",
    "    :param u: The control function\n",
    "    :returns: G:  Jacobian\n",
    "              Fx: STATE_SIZE x (STATE_SIZE + 2 * num_landmarks) matrix where the left side is an identity matrix\n",
    "    \"\"\"\n",
    "    \n",
    "    # [eye(3) [0 x y; 0 x y; 0 x y]]\n",
    "    Fx = np.hstack((np.eye(STATE_SIZE), np.zeros(\n",
    "        (STATE_SIZE, LM_SIZE * calc_n_LM(x)))))\n",
    "\n",
    "    jF = np.array([[0.0, 0.0, -DT * u[0] * math.sin(x[2, 0])],\n",
    "                   [0.0, 0.0, DT * u[0] * math.cos(x[2, 0])],\n",
    "                   [0.0, 0.0, 0.0]])\n",
    "\n",
    "    G = np.eye(STATE_SIZE) + Fx.T * jF * Fx\n",
    "    if calc_n_LM(x) > 0:\n",
    "        print(Fx.shape)\n",
    "    return G, Fx,\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "def calc_LM_Pos(x, z):\n",
    "    \"\"\"\n",
    "    Calcualtes the pose in the world coordinate frame of a landmark at the given measurement. \n",
    "\n",
    "    :param x: [x; y; theta]\n",
    "    :param z: [range; bearing]\n",
    "    :returns: [x; y] for given measurement\n",
    "    \"\"\"\n",
    "    zp = np.zeros((2, 1))\n",
    "\n",
    "    zp[0, 0] = x[0, 0] + z[0] * math.cos(x[2, 0] + z[1])\n",
    "    zp[1, 0] = x[1, 0] + z[0] * math.sin(x[2, 0] + z[1])\n",
    "    #zp[0, 0] = x[0, 0] + z[0, 0] * math.cos(x[2, 0] + z[0, 1])\n",
    "    #zp[1, 0] = x[1, 0] + z[0, 0] * math.sin(x[2, 0] + z[0, 1])\n",
    "\n",
    "    return zp\n",
    "\n",
    "\n",
    "def get_LM_Pos_from_state(x, ind):\n",
    "    \"\"\"\n",
    "    Returns the position of a given landmark\n",
    "    \n",
    "    :param x:   The state containing all landmark positions\n",
    "    :param ind: landmark id\n",
    "    :returns:   The position of the landmark\n",
    "    \"\"\"\n",
    "    lm = x[STATE_SIZE + LM_SIZE * ind: STATE_SIZE + LM_SIZE * (ind + 1), :]\n",
    "\n",
    "    return lm\n",
    "\n",
    "\n",
    "def search_correspond_LM_ID(xAug, PAug, zi):\n",
    "    \"\"\"\n",
    "    Landmark association with Mahalanobis distance.\n",
    "    \n",
    "    If this landmark is at least M_DIST_TH units away from all known landmarks, \n",
    "    it is a NEW landmark.\n",
    "    \n",
    "    :param xAug: The estimated state\n",
    "    :param PAug: The estimated covariance\n",
    "    :param zi:   the read measurements of specific landmark\n",
    "    :returns:    landmark id\n",
    "    \"\"\"\n",
    "\n",
    "    nLM = calc_n_LM(xAug)\n",
    "\n",
    "    mdist = []\n",
    "\n",
    "    for i in range(nLM):\n",
    "        lm = get_LM_Pos_from_state(xAug, i)\n",
    "        y, S, H = calc_innovation(lm, xAug, PAug, zi, i)\n",
    "        mdist.append(y.T @ np.linalg.inv(S) @ y)\n",
    "\n",
    "    mdist.append(M_DIST_TH)  # new landmark\n",
    "\n",
    "    minid = mdist.index(min(mdist))\n",
    "\n",
    "    return minid\n",
    "\n",
    "def calc_input():\n",
    "    v = 1.0  # [m/s]\n",
    "    yawrate = 0.1  # [rad/s]\n",
    "    u = np.array([[v, yawrate]]).T\n",
    "    return u\n",
    "\n",
    "def pi_2_pi(angle):\n",
    "    return (angle + math.pi) % (2 * math.pi) - math.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "def main():\n",
    "    print(\" start!!\")\n",
    "\n",
    "    time = 0.0\n",
    "\n",
    "    # RFID positions [x, y]\n",
    "    RFID = np.array([[10.0, -2.0],\n",
    "                     [15.0, 10.0],\n",
    "                     [3.0, 15.0],\n",
    "                     [-5.0, 20.0]])\n",
    "\n",
    "    # State Vector [x y yaw v]'\n",
    "    xEst = np.zeros((STATE_SIZE, 1))\n",
    "    xTrue = np.zeros((STATE_SIZE, 1))\n",
    "    PEst = np.eye(STATE_SIZE)\n",
    "\n",
    "    xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning\n",
    "\n",
    "    # history\n",
    "    hxEst = xEst\n",
    "    hxTrue = xTrue\n",
    "    hxDR = xTrue\n",
    "\n",
    "    while SIM_TIME >= time:\n",
    "        time += DT\n",
    "        u = calc_input()\n",
    "\n",
    "        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)\n",
    "\n",
    "        xEst, PEst = ekf_slam(xEst, PEst, ud, z)\n",
    "\n",
    "        x_state = xEst[0:STATE_SIZE]\n",
    "\n",
    "        # store data history\n",
    "        hxEst = np.hstack((hxEst, x_state))\n",
    "        hxDR = np.hstack((hxDR, xDR))\n",
    "        hxTrue = np.hstack((hxTrue, xTrue))\n",
    "\n",
    "        if show_animation:  # pragma: no cover\n",
    "            plt.cla()\n",
    "\n",
    "            plt.plot(RFID[:, 0], RFID[:, 1], \"*k\")\n",
    "            plt.plot(xEst[0], xEst[1], \".r\")\n",
    "\n",
    "            # plot landmark\n",
    "            for i in range(calc_n_LM(xEst)):\n",
    "                plt.plot(xEst[STATE_SIZE + i * 2],\n",
    "                         xEst[STATE_SIZE + i * 2 + 1], \"xg\")\n",
    "\n",
    "            plt.plot(hxTrue[0, :],\n",
    "                     hxTrue[1, :], \"-b\")\n",
    "            plt.plot(hxDR[0, :],\n",
    "                     hxDR[1, :], \"-k\")\n",
    "            plt.plot(hxEst[0, :],\n",
    "                     hxEst[1, :], \"-r\")\n",
    "            plt.axis(\"equal\")\n",
    "            plt.grid(True)\n",
    "            plt.pause(0.001)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      " start!!\n",
      "New LM\n",
      "New LM\n",
      "New LM\n"
     ]
    },
    {
     "data": {
      "application/javascript": [
       "/* Put everything inside the global mpl namespace */\n",
       "window.mpl = {};\n",
       "\n",
       "\n",
       "mpl.get_websocket_type = function() {\n",
       "    if (typeof(WebSocket) !== 'undefined') {\n",
       "        return WebSocket;\n",
       "    } else if (typeof(MozWebSocket) !== 'undefined') {\n",
       "        return MozWebSocket;\n",
       "    } else {\n",
       "        alert('Your browser does not have WebSocket support.' +\n",
       "              'Please try Chrome, Safari or Firefox ≥ 6. ' +\n",
       "              'Firefox 4 and 5 are also supported but you ' +\n",
       "              'have to enable WebSockets in about:config.');\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure = function(figure_id, websocket, ondownload, parent_element) {\n",
       "    this.id = figure_id;\n",
       "\n",
       "    this.ws = websocket;\n",
       "\n",
       "    this.supports_binary = (this.ws.binaryType != undefined);\n",
       "\n",
       "    if (!this.supports_binary) {\n",
       "        var warnings = document.getElementById(\"mpl-warnings\");\n",
       "        if (warnings) {\n",
       "            warnings.style.display = 'block';\n",
       "            warnings.textContent = (\n",
       "                \"This browser does not support binary websocket messages. \" +\n",
       "                    \"Performance may be slow.\");\n",
       "        }\n",
       "    }\n",
       "\n",
       "    this.imageObj = new Image();\n",
       "\n",
       "    this.context = undefined;\n",
       "    this.message = undefined;\n",
       "    this.canvas = undefined;\n",
       "    this.rubberband_canvas = undefined;\n",
       "    this.rubberband_context = undefined;\n",
       "    this.format_dropdown = undefined;\n",
       "\n",
       "    this.image_mode = 'full';\n",
       "\n",
       "    this.root = $('<div/>');\n",
       "    this._root_extra_style(this.root)\n",
       "    this.root.attr('style', 'display: inline-block');\n",
       "\n",
       "    $(parent_element).append(this.root);\n",
       "\n",
       "    this._init_header(this);\n",
       "    this._init_canvas(this);\n",
       "    this._init_toolbar(this);\n",
       "\n",
       "    var fig = this;\n",
       "\n",
       "    this.waiting = false;\n",
       "\n",
       "    this.ws.onopen =  function () {\n",
       "            fig.send_message(\"supports_binary\", {value: fig.supports_binary});\n",
       "            fig.send_message(\"send_image_mode\", {});\n",
       "            if (mpl.ratio != 1) {\n",
       "                fig.send_message(\"set_dpi_ratio\", {'dpi_ratio': mpl.ratio});\n",
       "            }\n",
       "            fig.send_message(\"refresh\", {});\n",
       "        }\n",
       "\n",
       "    this.imageObj.onload = function() {\n",
       "            if (fig.image_mode == 'full') {\n",
       "                // Full images could contain transparency (where diff images\n",
       "                // almost always do), so we need to clear the canvas so that\n",
       "                // there is no ghosting.\n",
       "                fig.context.clearRect(0, 0, fig.canvas.width, fig.canvas.height);\n",
       "            }\n",
       "            fig.context.drawImage(fig.imageObj, 0, 0);\n",
       "        };\n",
       "\n",
       "    this.imageObj.onunload = function() {\n",
       "        fig.ws.close();\n",
       "    }\n",
       "\n",
       "    this.ws.onmessage = this._make_on_message_function(this);\n",
       "\n",
       "    this.ondownload = ondownload;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_header = function() {\n",
       "    var titlebar = $(\n",
       "        '<div class=\"ui-dialog-titlebar ui-widget-header ui-corner-all ' +\n",
       "        'ui-helper-clearfix\"/>');\n",
       "    var titletext = $(\n",
       "        '<div class=\"ui-dialog-title\" style=\"width: 100%; ' +\n",
       "        'text-align: center; padding: 3px;\"/>');\n",
       "    titlebar.append(titletext)\n",
       "    this.root.append(titlebar);\n",
       "    this.header = titletext[0];\n",
       "}\n",
       "\n",
       "\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(canvas_div) {\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_canvas = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var canvas_div = $('<div/>');\n",
       "\n",
       "    canvas_div.attr('style', 'position: relative; clear: both; outline: 0');\n",
       "\n",
       "    function canvas_keyboard_event(event) {\n",
       "        return fig.key_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    canvas_div.keydown('key_press', canvas_keyboard_event);\n",
       "    canvas_div.keyup('key_release', canvas_keyboard_event);\n",
       "    this.canvas_div = canvas_div\n",
       "    this._canvas_extra_style(canvas_div)\n",
       "    this.root.append(canvas_div);\n",
       "\n",
       "    var canvas = $('<canvas/>');\n",
       "    canvas.addClass('mpl-canvas');\n",
       "    canvas.attr('style', \"left: 0; top: 0; z-index: 0; outline: 0\")\n",
       "\n",
       "    this.canvas = canvas[0];\n",
       "    this.context = canvas[0].getContext(\"2d\");\n",
       "\n",
       "    var backingStore = this.context.backingStorePixelRatio ||\n",
       "\tthis.context.webkitBackingStorePixelRatio ||\n",
       "\tthis.context.mozBackingStorePixelRatio ||\n",
       "\tthis.context.msBackingStorePixelRatio ||\n",
       "\tthis.context.oBackingStorePixelRatio ||\n",
       "\tthis.context.backingStorePixelRatio || 1;\n",
       "\n",
       "    mpl.ratio = (window.devicePixelRatio || 1) / backingStore;\n",
       "\n",
       "    var rubberband = $('<canvas/>');\n",
       "    rubberband.attr('style', \"position: absolute; left: 0; top: 0; z-index: 1;\")\n",
       "\n",
       "    var pass_mouse_events = true;\n",
       "\n",
       "    canvas_div.resizable({\n",
       "        start: function(event, ui) {\n",
       "            pass_mouse_events = false;\n",
       "        },\n",
       "        resize: function(event, ui) {\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "        stop: function(event, ui) {\n",
       "            pass_mouse_events = true;\n",
       "            fig.request_resize(ui.size.width, ui.size.height);\n",
       "        },\n",
       "    });\n",
       "\n",
       "    function mouse_event_fn(event) {\n",
       "        if (pass_mouse_events)\n",
       "            return fig.mouse_event(event, event['data']);\n",
       "    }\n",
       "\n",
       "    rubberband.mousedown('button_press', mouse_event_fn);\n",
       "    rubberband.mouseup('button_release', mouse_event_fn);\n",
       "    // Throttle sequential mouse events to 1 every 20ms.\n",
       "    rubberband.mousemove('motion_notify', mouse_event_fn);\n",
       "\n",
       "    rubberband.mouseenter('figure_enter', mouse_event_fn);\n",
       "    rubberband.mouseleave('figure_leave', mouse_event_fn);\n",
       "\n",
       "    canvas_div.on(\"wheel\", function (event) {\n",
       "        event = event.originalEvent;\n",
       "        event['data'] = 'scroll'\n",
       "        if (event.deltaY < 0) {\n",
       "            event.step = 1;\n",
       "        } else {\n",
       "            event.step = -1;\n",
       "        }\n",
       "        mouse_event_fn(event);\n",
       "    });\n",
       "\n",
       "    canvas_div.append(canvas);\n",
       "    canvas_div.append(rubberband);\n",
       "\n",
       "    this.rubberband = rubberband;\n",
       "    this.rubberband_canvas = rubberband[0];\n",
       "    this.rubberband_context = rubberband[0].getContext(\"2d\");\n",
       "    this.rubberband_context.strokeStyle = \"#000000\";\n",
       "\n",
       "    this._resize_canvas = function(width, height) {\n",
       "        // Keep the size of the canvas, canvas container, and rubber band\n",
       "        // canvas in synch.\n",
       "        canvas_div.css('width', width)\n",
       "        canvas_div.css('height', height)\n",
       "\n",
       "        canvas.attr('width', width * mpl.ratio);\n",
       "        canvas.attr('height', height * mpl.ratio);\n",
       "        canvas.attr('style', 'width: ' + width + 'px; height: ' + height + 'px;');\n",
       "\n",
       "        rubberband.attr('width', width);\n",
       "        rubberband.attr('height', height);\n",
       "    }\n",
       "\n",
       "    // Set the figure to an initial 600x600px, this will subsequently be updated\n",
       "    // upon first draw.\n",
       "    this._resize_canvas(600, 600);\n",
       "\n",
       "    // Disable right mouse context menu.\n",
       "    $(this.rubberband_canvas).bind(\"contextmenu\",function(e){\n",
       "        return false;\n",
       "    });\n",
       "\n",
       "    function set_focus () {\n",
       "        canvas.focus();\n",
       "        canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    window.setTimeout(set_focus, 100);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>')\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items) {\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) {\n",
       "            // put a spacer in here.\n",
       "            continue;\n",
       "        }\n",
       "        var button = $('<button/>');\n",
       "        button.addClass('ui-button ui-widget ui-state-default ui-corner-all ' +\n",
       "                        'ui-button-icon-only');\n",
       "        button.attr('role', 'button');\n",
       "        button.attr('aria-disabled', 'false');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "\n",
       "        var icon_img = $('<span/>');\n",
       "        icon_img.addClass('ui-button-icon-primary ui-icon');\n",
       "        icon_img.addClass(image);\n",
       "        icon_img.addClass('ui-corner-all');\n",
       "\n",
       "        var tooltip_span = $('<span/>');\n",
       "        tooltip_span.addClass('ui-button-text');\n",
       "        tooltip_span.html(tooltip);\n",
       "\n",
       "        button.append(icon_img);\n",
       "        button.append(tooltip_span);\n",
       "\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    var fmt_picker_span = $('<span/>');\n",
       "\n",
       "    var fmt_picker = $('<select/>');\n",
       "    fmt_picker.addClass('mpl-toolbar-option ui-widget ui-widget-content');\n",
       "    fmt_picker_span.append(fmt_picker);\n",
       "    nav_element.append(fmt_picker_span);\n",
       "    this.format_dropdown = fmt_picker[0];\n",
       "\n",
       "    for (var ind in mpl.extensions) {\n",
       "        var fmt = mpl.extensions[ind];\n",
       "        var option = $(\n",
       "            '<option/>', {selected: fmt === mpl.default_extension}).html(fmt);\n",
       "        fmt_picker.append(option)\n",
       "    }\n",
       "\n",
       "    // Add hover states to the ui-buttons\n",
       "    $( \".ui-button\" ).hover(\n",
       "        function() { $(this).addClass(\"ui-state-hover\");},\n",
       "        function() { $(this).removeClass(\"ui-state-hover\");}\n",
       "    );\n",
       "\n",
       "    var status_bar = $('<span class=\"mpl-message\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.request_resize = function(x_pixels, y_pixels) {\n",
       "    // Request matplotlib to resize the figure. Matplotlib will then trigger a resize in the client,\n",
       "    // which will in turn request a refresh of the image.\n",
       "    this.send_message('resize', {'width': x_pixels, 'height': y_pixels});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_message = function(type, properties) {\n",
       "    properties['type'] = type;\n",
       "    properties['figure_id'] = this.id;\n",
       "    this.ws.send(JSON.stringify(properties));\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.send_draw_message = function() {\n",
       "    if (!this.waiting) {\n",
       "        this.waiting = true;\n",
       "        this.ws.send(JSON.stringify({type: \"draw\", figure_id: this.id}));\n",
       "    }\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    var format_dropdown = fig.format_dropdown;\n",
       "    var format = format_dropdown.options[format_dropdown.selectedIndex].value;\n",
       "    fig.ondownload(fig, format);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.figure.prototype.handle_resize = function(fig, msg) {\n",
       "    var size = msg['size'];\n",
       "    if (size[0] != fig.canvas.width || size[1] != fig.canvas.height) {\n",
       "        fig._resize_canvas(size[0], size[1]);\n",
       "        fig.send_message(\"refresh\", {});\n",
       "    };\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_rubberband = function(fig, msg) {\n",
       "    var x0 = msg['x0'] / mpl.ratio;\n",
       "    var y0 = (fig.canvas.height - msg['y0']) / mpl.ratio;\n",
       "    var x1 = msg['x1'] / mpl.ratio;\n",
       "    var y1 = (fig.canvas.height - msg['y1']) / mpl.ratio;\n",
       "    x0 = Math.floor(x0) + 0.5;\n",
       "    y0 = Math.floor(y0) + 0.5;\n",
       "    x1 = Math.floor(x1) + 0.5;\n",
       "    y1 = Math.floor(y1) + 0.5;\n",
       "    var min_x = Math.min(x0, x1);\n",
       "    var min_y = Math.min(y0, y1);\n",
       "    var width = Math.abs(x1 - x0);\n",
       "    var height = Math.abs(y1 - y0);\n",
       "\n",
       "    fig.rubberband_context.clearRect(\n",
       "        0, 0, fig.canvas.width, fig.canvas.height);\n",
       "\n",
       "    fig.rubberband_context.strokeRect(min_x, min_y, width, height);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_figure_label = function(fig, msg) {\n",
       "    // Updates the figure title.\n",
       "    fig.header.textContent = msg['label'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_cursor = function(fig, msg) {\n",
       "    var cursor = msg['cursor'];\n",
       "    switch(cursor)\n",
       "    {\n",
       "    case 0:\n",
       "        cursor = 'pointer';\n",
       "        break;\n",
       "    case 1:\n",
       "        cursor = 'default';\n",
       "        break;\n",
       "    case 2:\n",
       "        cursor = 'crosshair';\n",
       "        break;\n",
       "    case 3:\n",
       "        cursor = 'move';\n",
       "        break;\n",
       "    }\n",
       "    fig.rubberband_canvas.style.cursor = cursor;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_message = function(fig, msg) {\n",
       "    fig.message.textContent = msg['message'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_draw = function(fig, msg) {\n",
       "    // Request the server to send over a new figure.\n",
       "    fig.send_draw_message();\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_image_mode = function(fig, msg) {\n",
       "    fig.image_mode = msg['mode'];\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Called whenever the canvas gets updated.\n",
       "    this.send_message(\"ack\", {});\n",
       "}\n",
       "\n",
       "// A function to construct a web socket function for onmessage handling.\n",
       "// Called in the figure constructor.\n",
       "mpl.figure.prototype._make_on_message_function = function(fig) {\n",
       "    return function socket_on_message(evt) {\n",
       "        if (evt.data instanceof Blob) {\n",
       "            /* FIXME: We get \"Resource interpreted as Image but\n",
       "             * transferred with MIME type text/plain:\" errors on\n",
       "             * Chrome.  But how to set the MIME type?  It doesn't seem\n",
       "             * to be part of the websocket stream */\n",
       "            evt.data.type = \"image/png\";\n",
       "\n",
       "            /* Free the memory for the previous frames */\n",
       "            if (fig.imageObj.src) {\n",
       "                (window.URL || window.webkitURL).revokeObjectURL(\n",
       "                    fig.imageObj.src);\n",
       "            }\n",
       "\n",
       "            fig.imageObj.src = (window.URL || window.webkitURL).createObjectURL(\n",
       "                evt.data);\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "        else if (typeof evt.data === 'string' && evt.data.slice(0, 21) == \"data:image/png;base64\") {\n",
       "            fig.imageObj.src = evt.data;\n",
       "            fig.updated_canvas_event();\n",
       "            fig.waiting = false;\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        var msg = JSON.parse(evt.data);\n",
       "        var msg_type = msg['type'];\n",
       "\n",
       "        // Call the  \"handle_{type}\" callback, which takes\n",
       "        // the figure and JSON message as its only arguments.\n",
       "        try {\n",
       "            var callback = fig[\"handle_\" + msg_type];\n",
       "        } catch (e) {\n",
       "            console.log(\"No handler for the '\" + msg_type + \"' message type: \", msg);\n",
       "            return;\n",
       "        }\n",
       "\n",
       "        if (callback) {\n",
       "            try {\n",
       "                // console.log(\"Handling '\" + msg_type + \"' message: \", msg);\n",
       "                callback(fig, msg);\n",
       "            } catch (e) {\n",
       "                console.log(\"Exception inside the 'handler_\" + msg_type + \"' callback:\", e, e.stack, msg);\n",
       "            }\n",
       "        }\n",
       "    };\n",
       "}\n",
       "\n",
       "// from http://stackoverflow.com/questions/1114465/getting-mouse-location-in-canvas\n",
       "mpl.findpos = function(e) {\n",
       "    //this section is from http://www.quirksmode.org/js/events_properties.html\n",
       "    var targ;\n",
       "    if (!e)\n",
       "        e = window.event;\n",
       "    if (e.target)\n",
       "        targ = e.target;\n",
       "    else if (e.srcElement)\n",
       "        targ = e.srcElement;\n",
       "    if (targ.nodeType == 3) // defeat Safari bug\n",
       "        targ = targ.parentNode;\n",
       "\n",
       "    // jQuery normalizes the pageX and pageY\n",
       "    // pageX,Y are the mouse positions relative to the document\n",
       "    // offset() returns the position of the element relative to the document\n",
       "    var x = e.pageX - $(targ).offset().left;\n",
       "    var y = e.pageY - $(targ).offset().top;\n",
       "\n",
       "    return {\"x\": x, \"y\": y};\n",
       "};\n",
       "\n",
       "/*\n",
       " * return a copy of an object with only non-object keys\n",
       " * we need this to avoid circular references\n",
       " * http://stackoverflow.com/a/24161582/3208463\n",
       " */\n",
       "function simpleKeys (original) {\n",
       "  return Object.keys(original).reduce(function (obj, key) {\n",
       "    if (typeof original[key] !== 'object')\n",
       "        obj[key] = original[key]\n",
       "    return obj;\n",
       "  }, {});\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.mouse_event = function(event, name) {\n",
       "    var canvas_pos = mpl.findpos(event)\n",
       "\n",
       "    if (name === 'button_press')\n",
       "    {\n",
       "        this.canvas.focus();\n",
       "        this.canvas_div.focus();\n",
       "    }\n",
       "\n",
       "    var x = canvas_pos.x * mpl.ratio;\n",
       "    var y = canvas_pos.y * mpl.ratio;\n",
       "\n",
       "    this.send_message(name, {x: x, y: y, button: event.button,\n",
       "                             step: event.step,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "\n",
       "    /* This prevents the web browser from automatically changing to\n",
       "     * the text insertion cursor when the button is pressed.  We want\n",
       "     * to control all of the cursor setting manually through the\n",
       "     * 'cursor' event from matplotlib */\n",
       "    event.preventDefault();\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    // Handle any extra behaviour associated with a key event\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.key_event = function(event, name) {\n",
       "\n",
       "    // Prevent repeat events\n",
       "    if (name == 'key_press')\n",
       "    {\n",
       "        if (event.which === this._key)\n",
       "            return;\n",
       "        else\n",
       "            this._key = event.which;\n",
       "    }\n",
       "    if (name == 'key_release')\n",
       "        this._key = null;\n",
       "\n",
       "    var value = '';\n",
       "    if (event.ctrlKey && event.which != 17)\n",
       "        value += \"ctrl+\";\n",
       "    if (event.altKey && event.which != 18)\n",
       "        value += \"alt+\";\n",
       "    if (event.shiftKey && event.which != 16)\n",
       "        value += \"shift+\";\n",
       "\n",
       "    value += 'k';\n",
       "    value += event.which.toString();\n",
       "\n",
       "    this._key_event_extra(event, name);\n",
       "\n",
       "    this.send_message(name, {key: value,\n",
       "                             guiEvent: simpleKeys(event)});\n",
       "    return false;\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onclick = function(name) {\n",
       "    if (name == 'download') {\n",
       "        this.handle_save(this, null);\n",
       "    } else {\n",
       "        this.send_message(\"toolbar_button\", {name: name});\n",
       "    }\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.toolbar_button_onmouseover = function(tooltip) {\n",
       "    this.message.textContent = tooltip;\n",
       "};\n",
       "mpl.toolbar_items = [[\"Home\", \"Reset original view\", \"fa fa-home icon-home\", \"home\"], [\"Back\", \"Back to previous view\", \"fa fa-arrow-left icon-arrow-left\", \"back\"], [\"Forward\", \"Forward to next view\", \"fa fa-arrow-right icon-arrow-right\", \"forward\"], [\"\", \"\", \"\", \"\"], [\"Pan\", \"Pan axes with left mouse, zoom with right\", \"fa fa-arrows icon-move\", \"pan\"], [\"Zoom\", \"Zoom to rectangle\", \"fa fa-square-o icon-check-empty\", \"zoom\"], [\"\", \"\", \"\", \"\"], [\"Download\", \"Download plot\", \"fa fa-floppy-o icon-save\", \"download\"]];\n",
       "\n",
       "mpl.extensions = [\"eps\", \"pdf\", \"png\", \"ps\", \"raw\", \"svg\"];\n",
       "\n",
       "mpl.default_extension = \"png\";var comm_websocket_adapter = function(comm) {\n",
       "    // Create a \"websocket\"-like object which calls the given IPython comm\n",
       "    // object with the appropriate methods. Currently this is a non binary\n",
       "    // socket, so there is still some room for performance tuning.\n",
       "    var ws = {};\n",
       "\n",
       "    ws.close = function() {\n",
       "        comm.close()\n",
       "    };\n",
       "    ws.send = function(m) {\n",
       "        //console.log('sending', m);\n",
       "        comm.send(m);\n",
       "    };\n",
       "    // Register the callback with on_msg.\n",
       "    comm.on_msg(function(msg) {\n",
       "        //console.log('receiving', msg['content']['data'], msg);\n",
       "        // Pass the mpl event to the overridden (by mpl) onmessage function.\n",
       "        ws.onmessage(msg['content']['data'])\n",
       "    });\n",
       "    return ws;\n",
       "}\n",
       "\n",
       "mpl.mpl_figure_comm = function(comm, msg) {\n",
       "    // This is the function which gets called when the mpl process\n",
       "    // starts-up an IPython Comm through the \"matplotlib\" channel.\n",
       "\n",
       "    var id = msg.content.data.id;\n",
       "    // Get hold of the div created by the display call when the Comm\n",
       "    // socket was opened in Python.\n",
       "    var element = $(\"#\" + id);\n",
       "    var ws_proxy = comm_websocket_adapter(comm)\n",
       "\n",
       "    function ondownload(figure, format) {\n",
       "        window.open(figure.imageObj.src);\n",
       "    }\n",
       "\n",
       "    var fig = new mpl.figure(id, ws_proxy,\n",
       "                           ondownload,\n",
       "                           element.get(0));\n",
       "\n",
       "    // Call onopen now - mpl needs it, as it is assuming we've passed it a real\n",
       "    // web socket which is closed, not our websocket->open comm proxy.\n",
       "    ws_proxy.onopen();\n",
       "\n",
       "    fig.parent_element = element.get(0);\n",
       "    fig.cell_info = mpl.find_output_cell(\"<div id='\" + id + \"'></div>\");\n",
       "    if (!fig.cell_info) {\n",
       "        console.error(\"Failed to find cell for figure\", id, fig);\n",
       "        return;\n",
       "    }\n",
       "\n",
       "    var output_index = fig.cell_info[2]\n",
       "    var cell = fig.cell_info[0];\n",
       "\n",
       "};\n",
       "\n",
       "mpl.figure.prototype.handle_close = function(fig, msg) {\n",
       "    var width = fig.canvas.width/mpl.ratio\n",
       "    fig.root.unbind('remove')\n",
       "\n",
       "    // Update the output cell to use the data from the current canvas.\n",
       "    fig.push_to_output();\n",
       "    var dataURL = fig.canvas.toDataURL();\n",
       "    // Re-enable the keyboard manager in IPython - without this line, in FF,\n",
       "    // the notebook keyboard shortcuts fail.\n",
       "    IPython.keyboard_manager.enable()\n",
       "    $(fig.parent_element).html('<img src=\"' + dataURL + '\" width=\"' + width + '\">');\n",
       "    fig.close_ws(fig, msg);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.close_ws = function(fig, msg){\n",
       "    fig.send_message('closing', msg);\n",
       "    // fig.ws.close()\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.push_to_output = function(remove_interactive) {\n",
       "    // Turn the data on the canvas into data in the output cell.\n",
       "    var width = this.canvas.width/mpl.ratio\n",
       "    var dataURL = this.canvas.toDataURL();\n",
       "    this.cell_info[1]['text/html'] = '<img src=\"' + dataURL + '\" width=\"' + width + '\">';\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.updated_canvas_event = function() {\n",
       "    // Tell IPython that the notebook contents must change.\n",
       "    IPython.notebook.set_dirty(true);\n",
       "    this.send_message(\"ack\", {});\n",
       "    var fig = this;\n",
       "    // Wait a second, then push the new image to the DOM so\n",
       "    // that it is saved nicely (might be nice to debounce this).\n",
       "    setTimeout(function () { fig.push_to_output() }, 1000);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._init_toolbar = function() {\n",
       "    var fig = this;\n",
       "\n",
       "    var nav_element = $('<div/>')\n",
       "    nav_element.attr('style', 'width: 100%');\n",
       "    this.root.append(nav_element);\n",
       "\n",
       "    // Define a callback function for later on.\n",
       "    function toolbar_event(event) {\n",
       "        return fig.toolbar_button_onclick(event['data']);\n",
       "    }\n",
       "    function toolbar_mouse_event(event) {\n",
       "        return fig.toolbar_button_onmouseover(event['data']);\n",
       "    }\n",
       "\n",
       "    for(var toolbar_ind in mpl.toolbar_items){\n",
       "        var name = mpl.toolbar_items[toolbar_ind][0];\n",
       "        var tooltip = mpl.toolbar_items[toolbar_ind][1];\n",
       "        var image = mpl.toolbar_items[toolbar_ind][2];\n",
       "        var method_name = mpl.toolbar_items[toolbar_ind][3];\n",
       "\n",
       "        if (!name) { continue; };\n",
       "\n",
       "        var button = $('<button class=\"btn btn-default\" href=\"#\" title=\"' + name + '\"><i class=\"fa ' + image + ' fa-lg\"></i></button>');\n",
       "        button.click(method_name, toolbar_event);\n",
       "        button.mouseover(tooltip, toolbar_mouse_event);\n",
       "        nav_element.append(button);\n",
       "    }\n",
       "\n",
       "    // Add the status bar.\n",
       "    var status_bar = $('<span class=\"mpl-message\" style=\"text-align:right; float: right;\"/>');\n",
       "    nav_element.append(status_bar);\n",
       "    this.message = status_bar[0];\n",
       "\n",
       "    // Add the close button to the window.\n",
       "    var buttongrp = $('<div class=\"btn-group inline pull-right\"></div>');\n",
       "    var button = $('<button class=\"btn btn-mini btn-primary\" href=\"#\" title=\"Stop Interaction\"><i class=\"fa fa-power-off icon-remove icon-large\"></i></button>');\n",
       "    button.click(function (evt) { fig.handle_close(fig, {}); } );\n",
       "    button.mouseover('Stop Interaction', toolbar_mouse_event);\n",
       "    buttongrp.append(button);\n",
       "    var titlebar = this.root.find($('.ui-dialog-titlebar'));\n",
       "    titlebar.prepend(buttongrp);\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._root_extra_style = function(el){\n",
       "    var fig = this\n",
       "    el.on(\"remove\", function(){\n",
       "\tfig.close_ws(fig, {});\n",
       "    });\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._canvas_extra_style = function(el){\n",
       "    // this is important to make the div 'focusable\n",
       "    el.attr('tabindex', 0)\n",
       "    // reach out to IPython and tell the keyboard manager to turn it's self\n",
       "    // off when our div gets focus\n",
       "\n",
       "    // location in version 3\n",
       "    if (IPython.notebook.keyboard_manager) {\n",
       "        IPython.notebook.keyboard_manager.register_events(el);\n",
       "    }\n",
       "    else {\n",
       "        // location in version 2\n",
       "        IPython.keyboard_manager.register_events(el);\n",
       "    }\n",
       "\n",
       "}\n",
       "\n",
       "mpl.figure.prototype._key_event_extra = function(event, name) {\n",
       "    var manager = IPython.notebook.keyboard_manager;\n",
       "    if (!manager)\n",
       "        manager = IPython.keyboard_manager;\n",
       "\n",
       "    // Check for shift+enter\n",
       "    if (event.shiftKey && event.which == 13) {\n",
       "        this.canvas_div.blur();\n",
       "        event.shiftKey = false;\n",
       "        // Send a \"J\" for go to next cell\n",
       "        event.which = 74;\n",
       "        event.keyCode = 74;\n",
       "        manager.command_mode();\n",
       "        manager.handle_keydown(event);\n",
       "    }\n",
       "}\n",
       "\n",
       "mpl.figure.prototype.handle_save = function(fig, msg) {\n",
       "    fig.ondownload(fig, null);\n",
       "}\n",
       "\n",
       "\n",
       "mpl.find_output_cell = function(html_output) {\n",
       "    // Return the cell and output element which can be found *uniquely* in the notebook.\n",
       "    // Note - this is a bit hacky, but it is done because the \"notebook_saving.Notebook\"\n",
       "    // IPython event is triggered only after the cells have been serialised, which for\n",
       "    // our purposes (turning an active figure into a static one), is too late.\n",
       "    var cells = IPython.notebook.get_cells();\n",
       "    var ncells = cells.length;\n",
       "    for (var i=0; i<ncells; i++) {\n",
       "        var cell = cells[i];\n",
       "        if (cell.cell_type === 'code'){\n",
       "            for (var j=0; j<cell.output_area.outputs.length; j++) {\n",
       "                var data = cell.output_area.outputs[j];\n",
       "                if (data.data) {\n",
       "                    // IPython >= 3 moved mimebundle to data attribute of output\n",
       "                    data = data.data;\n",
       "                }\n",
       "                if (data['text/html'] == html_output) {\n",
       "                    return [cell, data, j];\n",
       "                }\n",
       "            }\n",
       "        }\n",
       "    }\n",
       "}\n",
       "\n",
       "// Register the function which deals with the matplotlib target/channel.\n",
       "// The kernel may be null if the page has been refreshed.\n",
       "if (IPython.notebook.kernel != null) {\n",
       "    IPython.notebook.kernel.comm_manager.register_target('matplotlib', mpl.mpl_figure_comm);\n",
       "}\n"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/html": [
       "<img src=\"data:image/png;base64,[encoded data, 108156 characters]\" width=\"636\">"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "New LM\n"
     ]
    }
   ],
   "source": [
    "%matplotlib notebook\n",
    "%matplotlib notebook\n",
    "main()"
   ]
  }
 ],
 "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.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
