6-Mapping-1-EKF.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 6.1 Mapping EFK - The Nirvana shopping mall\n",
    "\n",
    "The managers behind the ***<span style=\"color:seagreen\">Nirvana shopping mall</span>***, in the scope of their *experiencing the future* plan, has contacted ***<span style=\"color:seagreen\">UMA-MR</span>*** (our brand-new mobile robotics company at UMA) looking for mobile robots able to guide their visitors between different points of interest in their facilities, like information points, the entrance to relevant (paying well) shops, rescue points, etc. \n",
    "\n",
    "<br /><br />\n",
    "<center>\n",
    "<img src=\"images/6-mapping/nirvana-logo.png\" width=\"600\">\n",
    "</center>\n",
    "<br />\n",
    "\n",
    "These managers have placed identifying marks close to the points of interest they want to consider, but do not know their exact location in the mall. **Our mission as engineers at *UMA-MR* is to build a map of the mall containing such points, so the robot can operate within it.** We are going to use for that an **Extended Kalman Filter (EKF)**.\n",
    "\n",
    "Fortunately, our company has developed a system able to provide the exact location of our robots at each time instant, which are also equipped with a range-and-bearing nosiy sensor able to detect the identifying marks and take measurements to them."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Formalizing the problem\n",
    "\n",
    "Since we are going to build a map of $N$ landmarks, the position of those landmarks in the map $m$ are the random variables to be estimated:\n",
    "\n",
    "$$m = [m_1, m_2, \\cdots, m_N] = \n",
    "[x_1, y_1, \\underbrace{x_2, y_2}_{\\text{Position of}\\\\\\text{landmark 1}}, \\dots,x_N, y_N]^T, \\ \\ \\ len(m) = 2N$$\n",
    "\n",
    "In other words, we pursuit the estimation of the probability distribution:\n",
    "\n",
    "$$p(m | z_{1:t}, x_{1:t})$$\n",
    "\n",
    "being $z_{1:t}$ the sensor measurments taken until time instant $t$, and $x_{1:t}$ the robot poses from which those measurements were adquired. For simplifying the problem, it is usual to assume that the estimation of the postiion of the landmarks is independent one to another, only depending each one on its observations so:\n",
    "\n",
    "$$p(m_i|z_{1:t}, x_{1:t}) = p(m_i |z^i_{1:t}, x_{1:t})$$\n",
    "\n",
    "$$p(m | z_{1:t}, x_{1:t})= p(m_1, m_2, \\cdots, m_N | z_{1:t}, x_{1:t}) = \\prod_{k=1}^N p(m_i | z^i_{1:t}, x_{1:t})$$\n",
    "\n",
    "Unlike the localization case, where the robot pose changed over time, in this case it is assumed that the map is static, that is, the landmarks are still. This means that in the state transition model $m_k=m_{k-1}$, that is:\n",
    "\n",
    "$$m_t = A_t m_{t-1} + B_t u_t + \\epsilon_t \\ \\ \\ \\ (A=I, u=0, \\epsilon=0)$$\n",
    "\n",
    "So good news here!, there is no need for a prediction step in the EKF, we only have to model the correction (update) one. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Developing the EKF filter for mapping the mall"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib notebook\n",
    "\n",
    "import sys\n",
    "import time\n",
    "\n",
    "import numpy as np\n",
    "from numpy import random\n",
    "from scipy import linalg\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from utils.tcomp import tcomp\n",
    "from utils.AngleWrap import AngleWrap\n",
    "from utils.DrawRobot import DrawRobot\n",
    "from utils.PlotEllipse import PlotEllipse\n",
    "from utils.Jacobians import J2\n",
    "from utils.unit6.MapCanvas import MapCanvas"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. The provided robot\n",
    "\n",
    "Take a look at the following ``EFKMappingRobot()`` class, which is already provided by your colleages at ***<span style=\"color:seagreen\">UMA-MR</span>***, modellng a robot able to keep the following information:\n",
    "- ``true_pose``: The exact pose of the robot in the environment, which is perfectly known.\n",
    "- ``Q``: Uncertainty of the range and bearing sensor, which has the form:\n",
    "$\\Sigma_{r\\theta} = \\begin{bmatrix} \\sigma^2_r & 0 \\\\ 0 & \\sigma^2_\\theta  \\end{bmatrix}$\n",
    "- ``xEst``: Vector with the estimated state, in this case the position of the $M$ observed landmarks: $[x_1, y_1, x_2, y_2, \\dots,x_M, y_M]^T$. Its size changes over time.\n",
    "- ``PEst``: uncertainty associated with those predictions, with size ($M\\times2$,$M\\times2$). Its size also changes over time.\n",
    "- ``MappedLandmarks``: A vector with length equal to the number of landmarks in the map ($N$), which elements can take the following values:\n",
    "  - ``-1`` if the landmark with that index has not benn seen yet.\n",
    "  - ``idx_in_xEst``: an odd number indicating the position of that landmark in ``xEst``.\n",
    "\n",
    "and to perform the following actions:\n",
    "\n",
    "- ``step()``: Performs a motion command, without noise. \n",
    "- ``observe()``: Returns a range and bearing measurement (in polars) to a given landmark in the map.\n",
    "- ``get_random_observation()``: Returns a range and bearing measurement (in polars) to a random landmark."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class EFKMappingRobot():\n",
    "    def __init__(self, true_pose, sigma_r, sigma_theta, n_features):\n",
    "        # Robot description\n",
    "        self.true_pose = true_pose\n",
    "        self.Q = np.diag([sigma_r, sigma_theta])**2\n",
    "        \n",
    "        # Map -- Initially empty\n",
    "        self.xEst = np.empty((0, 0))\n",
    "        self.PEst = np.empty((0, 0))\n",
    "        self.QEst = 1.0*self.Q\n",
    "        \n",
    "        self.MappedLandmarks = -1*np.ones((n_features,1), int)\n",
    "        \n",
    "    def step(self, u):\n",
    "        self.true_pose = tcomp(self.true_pose, u)\n",
    "        \n",
    "    def observe(self, idx, world, noisy=True):\n",
    "        \"\"\" Generate a observation of a feature in our world\n",
    "        \n",
    "            Args:\n",
    "                world: Complete map of all landmarks in the world\n",
    "                idx: Landmark to observe (index in world matrix)\n",
    "                noisy: Add noise to z, prorportional to self.Q\n",
    "                \n",
    "            Returns:\n",
    "                z: One range and bearing observation\n",
    "        \"\"\"\n",
    "        \n",
    "        z = np.empty((2, 1))\n",
    "        delta = world[:, [idx]] - self.true_pose[0:2, :]\n",
    "        \n",
    "        # Range\n",
    "        z[0, :] = np.sqrt(np.sum(delta**2))\n",
    "        # Bearing\n",
    "        z[1, :] = np.arctan2(delta[1, 0],delta[0, 0]) - self.true_pose[2, 0]\n",
    "        z[1, :] = AngleWrap(z[1, :])\n",
    "        \n",
    "        if noisy:\n",
    "            z = z + np.sqrt(self.Q)@random.randn(2,1)\n",
    "            \n",
    "        return z\n",
    "    \n",
    "    def get_random_observation(self, world, noisy=True):\n",
    "        iLandmarks = world.shape[1]\n",
    "        iLandmark = random.randint(iLandmarks)\n",
    "        z = self.observe(iLandmark, world, noisy)\n",
    "        return z, [iLandmark]\n",
    "        "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. The prediction step\n",
    "\n",
    "As commented, the map is considered static, so the prediction step is reduced to consider as the predicted landmarks' positions the ones estimated in the previous step. The same holds for the predicted uncertainty, so this steps results in something like this:\n",
    "\n",
    "$$\n",
    "  \\begin{aligned}\n",
    "       \\verb!def !& \\verb!ExtendedKalmanFilter!(m_{t-1},\\Sigma_{t-1}, z_t): \\\\\n",
    "      & \\textbf{Prediction.} \\\\\n",
    "      & \\bar m_t = m_{t-1} &\\text{(1. Map prediction)}\\\\\n",
    "      & \\bar\\Sigma_t = \\Sigma_{t-1} &\\text{(2. Uncertainty of prediction)}\\\\      \n",
    "  \\end{aligned}\n",
    "$$\n",
    "\n",
    "**You are tasked to** implement this behaviour in the following function, which performs the prediction step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def prediction_step(robot: EFKMappingRobot): \n",
    "    \"\"\" Performs the prediction step of the EKF algorithm for mapping\n",
    "            robot: Robot base (contains state map: xEst, PEst)\n",
    "        \n",
    "        Returns: Nothing. But it modifies the state in robot\n",
    "            xPred: Predicted position of the landmarks\n",
    "            PPred: Predicted uncertainty of the landmarks positions\n",
    "    \"\"\"    \n",
    "    \n",
    "    # We assume that the map is static \n",
    "    xPred = None\n",
    "    PPred = None\n",
    "    \n",
    "    return xPred, PPred"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TRY IT!\n",
    "xVehicleTrue = np.vstack([0.5, 0.7, 0]) # We know the exact robot pose at any moment\n",
    "robot = EFKMappingRobot(xVehicleTrue, 1, 0.8, 1)\n",
    "robot.xEst = np.vstack([.5, .7])\n",
    "robot.PEst = np.diag([1.32, 0.8])\n",
    "[xPred,PPred] = prediction_step(robot)\n",
    "print('xPred:\\n' + str(xPred))\n",
    "print('PPred:\\n' + str(PPred))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected output:</span>\n",
    "\n",
    "```\n",
    "xPred:\n",
    "[[0.5]\n",
    " [0.7]]\n",
    "PPred:\n",
    "[[1.32 0.  ]\n",
    " [0.   0.8 ]]\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Observing a landmark for first time\n",
    "\n",
    "When the sensor onboard the robot detects a landmark for the first time, there is no need to do the EKF update step (indeed, since there is not previously knowledge about the landmark, there is nothing to update). Instead, we have to properly modify the vector of estimated landmark positions and their associated uncertainties to accomodate this new information:\n",
    "\n",
    "\n",
    "- **Modifying the state vector**: Insert the position of the landmark, using the sensor measurement $z_k=[r_k,\\theta_k]$, at the end of the vector containing the estimated positions ``xEst``, so:\n",
    "$$xEst=[x_1,y_y, \\cdots, x_M, y_M, x_{M+1}, y_{M+1}]$$\n",
    "Since the measurment is provided in polar coordinates in the robot local frame, we have to convert them first to cartensians and then to the world frame using the robot pose $[x_v,y_v]'$:\n",
    "\n",
    "$$ \n",
    "f(x_v,z_k)=\\begin{bmatrix} x_{M+1} \\\\ y_{M+1} \\end{bmatrix} =\n",
    "\\begin{bmatrix} x_v \\\\ y_v \\end{bmatrix} + \n",
    "r_k\\begin{bmatrix} cos \\alpha_k \\\\ sin \\alpha_k \\end{bmatrix}\n",
    ", \\ \\ \\alpha_k = \\theta_k + \\theta_v\n",
    "$$\n",
    "\n",
    "- **Extending the covariance matrix**. In order to acomodate the uncertainty regarding the position of the new landmark, we have to extend the covariance matrix in the following way:\n",
    "$$\n",
    "PEst=\\begin{bmatrix}\n",
    "    [\\Sigma^1_{xy}]_{2 \\times 2} & \\cdots & 0_{2 \\times 2} \\\\\n",
    "    \\vdots & \\ddots & \\vdots \\\\\n",
    "    0_{2 \\times 2} & \\cdots & [\\Sigma^{M+1}_{xy}]_{2 \\times 2}\n",
    "  \\end{bmatrix}_{2n \\times 2n}\n",
    "  $$  \n",
    "Notice that the covariance $\\Sigma^{M+1}_{xy}$ stands for the uncertainty in the measurement expressed in the world cartesian coordinates, retrieved by:\n",
    "$$\\Sigma^{M+1}_{xy} = J \\Sigma^{M+1}_{r\\theta} J^T$$\n",
    "being $\\Sigma^{M+1}_{r\\theta}$ the uncertainty characterizing the sensor measurements (``QEst`` in our code), and $J$ (``jGz`` in our code) the jacobian of the function $f(x_v,z_k)$ that expresses the measurement in global coordinates, which is:\n",
    "\n",
    "$$\n",
    "J =  \\begin{bmatrix} \\partial x / \\partial r  &  \\partial x / \\partial \\theta \\\\ \\partial y / \\partial r & \\partial y / \\partial \\theta \\end{bmatrix} =\n",
    "\\begin{bmatrix} cos \\alpha & -r sin \\alpha \\\\ sin \\alpha & r cos \\alpha \\end{bmatrix}\n",
    "$$\n",
    "\n",
    "**Your work here is to:**\n",
    "- Complete the ``get_new_landmark_jacobians()`` to compute the jacobian $J$.\n",
    "- Complete the ``incorporate_new_landmark()`` method to modify the state vector ``xEst`` and the convariance matrix ``PEst`` as explained above."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_new_landmark_jacobians(Xv, z):\n",
    "    \"\"\" Calculate the jacobian for transforming an observation to the world frame\n",
    "    \n",
    "        Args:\n",
    "            Xv: True pose of our robot\n",
    "            z: Observation of a landmark. In polar coordinates from the p.o.v. of our robot.\n",
    "        \n",
    "        Returns:\n",
    "            2x2 matrix containing the corresponding jacobian.\n",
    "    \"\"\"\n",
    "    r, a = None, None\n",
    "    c, s = None, None\n",
    "    jGz = np.array([\n",
    "        [None, None],\n",
    "        [None, None]\n",
    "    ])\n",
    "    return jGz"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TRY IT!\n",
    "z = np.vstack([1.2,0.35])\n",
    "Xv = np.vstack([2, 2.1, 0])\n",
    "jGz = get_new_landmark_jacobians(Xv, z)\n",
    "print('jGz:\\n' + str(jGz))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected output:</span>\n",
    "\n",
    "```\n",
    "jGz:\n",
    "[[ 0.93937271 -0.41147737]\n",
    " [ 0.34289781  1.12724726]]\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def incorporate_new_landmark(robot: EFKMappingRobot, z, iLandmark, xPred, PPred):\n",
    "    \"\"\" Incorporates the information relative to a new ladmark to our system\n",
    "        robot: Robot base (contains state map: xEst, PEst)\n",
    "        z: Observation of a landmark\n",
    "        iLandmark: Index of z in the world map\n",
    "        xPred: Predicted map\n",
    "        PPred: Uncertainty of the prediction\n",
    "\n",
    "        Returns: Nothing. But it modifies the state in robot\n",
    "    \"\"\"\n",
    "    # This is a new feature, so add it to the map        \n",
    "        \n",
    "    # The observation is in the local frame of the robot, it has to\n",
    "    # be translated to the global frame\n",
    "    xLandmark = None\n",
    "\n",
    "    # Add it to the current state\n",
    "    nStates = xPred.size \n",
    "        \n",
    "    if nStates == 0:\n",
    "        robot.xEst = xLandmark\n",
    "    else:\n",
    "        robot.xEst = np.vstack([robot.xEst, xLandmark]) #Each new feature two new rows\n",
    "\n",
    "    # Compute the jacobian\n",
    "    jGz = get_new_landmark_jacobians(robot.true_pose, z) #Dimension 2x2\n",
    "    \n",
    "    # Build a matrix M incorporating the jacobian to multiply the extendend PEst matrix by it (see below)\n",
    "    if nStates != 0:\n",
    "        # note we don't use jacobian w.r.t vehicle since the pose doesn’t have uncertainty\n",
    "        M = np.vstack([  \n",
    "                np.hstack([None, np.zeros((nStates, 2))]),\n",
    "                np.hstack([np.zeros((2, nStates)), None])\n",
    "            ])\n",
    "    else: \n",
    "        # First landmark observed!\n",
    "        M = jGz\n",
    "            \n",
    "    robot.PEst = M@linalg.block_diag(None, None)@M.T\n",
    "\n",
    "    #This can also be done directly PEst = [PEst,zeros(nStates,2);\n",
    "                                            # zeros(2,nStates),\n",
    "                                            # jGz*QEst*jGz']\n",
    "\n",
    "    #remember this landmark as being mapped: we store its ID for the state vector\n",
    "    robot.MappedLandmarks[iLandmark] = robot.xEst.size-2 #Always an odd number\n",
    "                "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. The correction (update) step\n",
    "\n",
    "Once a landmark has been detected and its provided information (location and uncertainty) properly incorporated to our mapping system, such an information can be updated with new measurements of such a landmark. For doing so, the EKF algorithm perform the following steps:\n",
    "\n",
    "$$\n",
    "  \\begin{aligned}     \n",
    "      & \\textbf{Correction.} \\\\\n",
    "      & K_t = \\bar\\Sigma_t H^T_t (H_t \\bar\\Sigma_t H^T_t + Q_t)^{-1} &\\text{(3. Kalman gain)}\\\\\n",
    "      & m_t = \\bar m_t + K_t (z_t - h(x_t,\\bar m_t)) &\\text{(4. Map estimation)}\\\\\n",
    "      & \\Sigma_t = (I - K_t H_t) \\bar\\Sigma_t &\\text{(5. Uncertainty of estimation)}\\\\\n",
    "      & \\verb!return ! m_t, \\Sigma_t\n",
    "  \\end{aligned}\n",
    "$$\n",
    "\n",
    "Notice that the map (landmark locations) is estimated according to the map estimation in the previous time step $t-1$, the *Kalman gain*, and the error (also called ***innovation***) between the obsevation taken by the sensor ($z_t$) and the one computed by the observation model ($z_t - h(x_t,\\bar m_t$)) given the robot pose and the predicted map.\n",
    "\n",
    "In such equations, H (``jH`` in our code) represents the jacobian of the observation model. The shape of such a jacobian is $(2,M\\times2)$, and has the following form:\n",
    "\n",
    "$$\n",
    "H = \\left[\n",
    "  \\begin{matrix}\n",
    "0 & 0 & \\cdots \\\\\n",
    "0 & 0 & \\cdots \\\\\n",
    "\\end{matrix}\n",
    "\\right .\n",
    "\\underbrace{\\begin{bmatrix}\n",
    "jHxl_{11} & jHxl_{12} \\\\\n",
    "jHxl_{21} & jHxl_{22} \\\\\n",
    "\\end{bmatrix}}_\\text{Jacobian for the observed landmark, $jHxl$}\n",
    "\\left .\n",
    "\\begin{matrix}\n",
    "\\cdots & 0 & 0 \\\\\n",
    "\\cdots & 0 & 0 \\\\\n",
    "\\end{matrix}\n",
    "\\right]\n",
    "$$\n",
    "\n",
    "$$\n",
    "jHxl = \n",
    "\\begin{bmatrix}\n",
    "(x_l - x)/d & (y_l - y)/d \\\\\n",
    "-(y_l - y)/d^2 & (x_l - x)/d^2 \\\\\n",
    "\\end{bmatrix}\n",
    "$$\n",
    "\n",
    "where $[x_l,y_l]$ is the position of the landmark, $[x,y]$ is the robot location, and $d=\\sqrt{(x_l-x)^2 + (y_l-y)^2}$.\n",
    "\n",
    "**Your task is:**\n",
    "- To implement the function ``get_observation_jacobian()`` returning the jacobian of the observed landmark, that is, $jHxl$.\n",
    "- To complete the ``update_step()`` method that performs the update step of the EKF algorithm."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def get_observation_jacobian(xPred, xLandmark):\n",
    "    \"\"\" Calculate the jacobian of the observation model.\n",
    "        \n",
    "        Needed to update a landmark we have already seen.\n",
    "        Hint. Similar to the one described in unit 5 (Localization)\n",
    "        \n",
    "        Args:\n",
    "            xPred: True pose of our robot.\n",
    "            xLandmark: Estimated pose of a landmark in our map. World p.o.v in cartesian coordinates.\n",
    "                Does not contain an angle.\n",
    "        \n",
    "        Return:\n",
    "            jHxl: 2x2 matrix containing the corresponding jacobian.\n",
    "    \"\"\"\n",
    "    xdist = None\n",
    "    ydist = None\n",
    "    r = None)\n",
    "    r2 = r**2\n",
    "    jHxl = np.array([\n",
    "        [None , None],\n",
    "        [None , None]\n",
    "    ])\n",
    "    return jHxl\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# TRY IT!\n",
    "xLandmark = np.vstack([2.5,2])\n",
    "xPred = np.vstack([2, 2.1, 0])\n",
    "jHxl = get_observation_jacobian(xPred, xLandmark)\n",
    "print('jHxl:\\n' + str(jHxl))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected output:</span>\n",
    "\n",
    "```\n",
    "jHxl:\n",
    "[[ 0.98058068 -0.19611614]\n",
    " [ 0.38461538  1.92307692]]\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def update_step(robot, z, iLandmark, xPred, PPred):\n",
    "    \"\"\" Performs the update step of EKF\n",
    "        robot: Robot base (contains state map: xEst, PEst)\n",
    "        z: Observation of a landmark\n",
    "        iLandmark: Index of z in the world map\n",
    "        xPred: Predicted map\n",
    "        PPred: Uncertainty of the prediction\n",
    "        \n",
    "        Returns: Nothing. But it modifies the state in robot\n",
    "    \"\"\"\n",
    "    # Find out where it is in state vector\n",
    "    landmarkIndex = robot.MappedLandmarks[iLandmark[0], 0]\n",
    "        \n",
    "    # xLandmark is the current estimation of the position of the\n",
    "    # landmard \"FeatureIndex\"\n",
    "    xLandmark = xPred[landmarkIndex:landmarkIndex+2]\n",
    "        \n",
    "    # DONE Predicts the observation\n",
    "    zPred = robot.observe(0, xLandmark, noisy=False) # Hint: use robot.observe function\n",
    "\n",
    "    # Get observation Jacobians\n",
    "    jHxf = get_observation_jacobian(None, None)\n",
    "        \n",
    "    # Fill in state jacobian\n",
    "    # (the jacobian is zero except for the observed landmark)\n",
    "    jH = np.zeros((2, xPred.size))\n",
    "    jH[:, landmarkIndex:landmarkIndex+2] = None\n",
    "        \n",
    "    #\n",
    "    # Kalman update\n",
    "    #\n",
    "    Innov = None-None # Innovation\n",
    "    Innov[1] = AngleWrap(Innov[1])\n",
    "    S = None@None@None + None\n",
    "    K = None@None@linalg.inv(S) # Gain\n",
    "    robot.xEst = None + None@None    \n",
    "    robot.PEst = (np.eye(robot.PEst.shape[0]) - None@None)@None\n",
    "    #robot.PEst = PPred - K@S@K.T # Alternative way\n",
    "    \n",
    "    #ensure P remains symmetric\n",
    "    robot.PEst = 0.5*(robot.PEst+robot.PEst.T)\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 5. Testing the mapping system\n",
    "\n",
    "Now that you have implemented the building blocks of the EKF filter for mapping the ***<span style=\"color:seagreen\">Nirvana shopping mall</span>***, it is time to write a simple function ``EKFMapping()`` putting them togeter. For that, **you have to** call each method with the appropriate parameters."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def EKFMapping(robot: EFKMappingRobot, z, iLandmark):\n",
    "    \"\"\" EFK algorithm for mapping\n",
    "        \n",
    "            robot: Robot base (contains state map: xEst, PEst)\n",
    "            z: Observation of a landmark\n",
    "            iLandmark: Index of z in the world map\n",
    "    \n",
    "        Returns: Nothing. But it modifies the state in robot\n",
    "    \"\"\"\n",
    "    \n",
    "    # Do prediction step\n",
    "    [xPred, PPred] = prediction_step(None)\n",
    "\n",
    "    # Check if feature observed is in map\n",
    "    if robot.MappedLandmarks[iLandmark] > -1:\n",
    "        update_step(None, None, None, None, None)\n",
    "        \n",
    "    else:\n",
    "        # This is a new feature, so add its information to the map        \n",
    "        incorporate_new_landmark(None, None, None, None, None)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Playing with one landmark\n",
    "\n",
    "The following function provides a demo where the robot is commanded to follow a squared trajectory while observing a landmark after each movement. Let's consider that the mall has only one lanmark to get things started (``nLandmarks=1``). The ***<span style=\"color:seagreen\">Nirvana</span>*** managers are curious about the state and dimensions of the variables storing the estimated positions `xEst` and their associated uncertainties `Pest`, so we show their content after each 5 iterations of the algorithm. **<span style=\"color:orangered\">Discuss the evolution of these variables, including their dimensions, in *your report to Nirvana*</span>**\n",
    "\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/6-mapping/fig6-1-2.png\" width=\"400\" alt=\"\">\n",
    "  <figcaption>\n",
    "      Fig. 1: Execution of the EKF algorithmn for mapping (only one landmark). <br/>\n",
    "      it shows the true pose (in red), <br/>\n",
    "      the real pose of the landmark (as a green star), <br/>\n",
    "      and the estimation from the EKF algorithm (pose and confidence ellipse).\n",
    "  </figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def demo_ekf_mapping(robot,\n",
    "                     Map,\n",
    "                     nLandmarks,\n",
    "                     mode='non_stop',\n",
    "                     logger=None,\n",
    "                     nSteps=100, # Number of motions\n",
    "                     turning= 40, # Number of motions before turning (square path)\n",
    "                     print_each=2):\n",
    "    \n",
    "    %matplotlib notebook\n",
    "    if mode == 'step_by_step':\n",
    "        matplotlib.use('TkAgg')\n",
    "\n",
    "    # storing the number of times a landmark has been seen\n",
    "    # also store the handler to the graphical info shown\n",
    "    canvas = MapCanvas(nLandmarks)\n",
    "    \n",
    "    canvas.ax.plot(Map[0, :], Map[1, :], 'g*')\n",
    "    hObsLine = canvas.ax.plot([0,0], [0,0], linestyle=':')\n",
    "    \n",
    "    # Control action\n",
    "    u = np.zeros((3, 1))\n",
    "    u[0] = (2.*MapSize/1.5)/turning\n",
    "    u[1] = 0.\n",
    "    \n",
    "    # Start the loop!\n",
    "    for k in range(nSteps):\n",
    "        #\n",
    "        # Move the robot\n",
    "        #\n",
    "        u[2]=0.\n",
    "        if k%turning == turning-1:\n",
    "            u[2] = np.pi/2\n",
    "        \n",
    "        robot.step(u) # Perfectly known robot pose\n",
    "        \n",
    "        z, iLandmark = robot.get_random_observation(world=Map)\n",
    "        \n",
    "        # Update the \"observedtimes\" for the feature and plot the reading\n",
    "        canvas.increment_observed_times(iLandmark)\n",
    "        canvas.PlotNumberOfReadings(robot.true_pose, iLandmark, Map)\n",
    "        \n",
    "        EKFMapping(robot, z, iLandmark)\n",
    "        \n",
    "        # Print map evolution each 5 steps\n",
    "        if not k%5:\n",
    "            with np.printoptions(precision=3):\n",
    "                print('Iteration: ' + str(k))\n",
    "                print('Estimated xEst:\\n' + str(robot.xEst))\n",
    "                print('Estimated PEst:\\n' + str(robot.PEst))\n",
    "                print('---------------------------------')\n",
    "        \n",
    "        # Log important values\n",
    "        if logger is not None:\n",
    "            logger.log(k, robot, Map)\n",
    "        \n",
    "        # Drawings\n",
    "        if k%print_each == print_each-1:\n",
    "            DrawRobot(canvas.fig, canvas.ax,robot.true_pose, 'r')#plot(xVehicleTrue(1),xVehicleTrue(2),'r*')\n",
    "            canvas.DoMapGraphics(robot) # Draw estimated poitns (in black) and ellipses\n",
    "            plt.axis([-MapSize-5, MapSize+5, -MapSize-5, MapSize+5]) # Set limits again\n",
    "            plt.draw()\n",
    "            \n",
    "            if mode == 'step_by_step':\n",
    "                plt.waitforbuttonpress(-1)\n",
    "            elif mode == 'visualize_process':\n",
    "                plt.pause(0.2)\n",
    "            elif mode == 'non_stop':\n",
    "                pass # non stop!\n",
    "\n",
    "    # Final drawings\n",
    "    %matplotlib inline\n",
    "    if logger is not None:\n",
    "        logger.plot()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#mode = 'step_by_step'\n",
    "mode = 'visualize_process'\n",
    "#mode = 'non_stop'\n",
    "\n",
    "# WORLD MAP\n",
    "# Num features/landmarks considered within the map\n",
    "nLandmarks = 1\n",
    "# Generation of the map\n",
    "MapSize = 100\n",
    "Map = MapSize*random.rand(2,nLandmarks)-MapSize/2\n",
    "\n",
    "# ROBOT\n",
    "# Covariances for our very bad&expensive sensor (in the system <d,theta>)\n",
    "Sigma_r = 8.0\n",
    "Sigma_theta = 8*np.pi/180\n",
    "# Initial robot pose\n",
    "xVehicleTrue = np.vstack([-MapSize/1.5, -MapSize/1.5, 0.]) # We know the exact robot pose at any moment\n",
    "\n",
    "robot = EFKMappingRobot(xVehicleTrue, Sigma_r, Sigma_theta, nLandmarks)\n",
    "\n",
    "demo_ekf_mapping(robot, Map ,nLandmarks, mode=mode)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Considering a larger number of landmarks \n",
    "\n",
    "Once our EKF implementation is working with one landmark, let's try it in a scenario with 5 landmarks. The ***<span style=\"color:seagreen\">Nirvana</span>*** mangers asked to have answers to the following concerns, **<span style=\"color:orangered\">discuss them in *your report to Nirvana*</span>**:\n",
    "    \n",
    "- why and how the content of the variables `xEst` and `Pest` has change?\n",
    "- What structure does the matrix of covariances have? Is there any kind of correlation among the observations of different landmarks?\n",
    "\n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/6-mapping/fig6-1-3.png\" width=\"400\" alt=\"\">\n",
    "  <figcaption>\n",
    "      Fig. 2: Execution of the EKF algorithmn for mapping (multiple landmarks). <br/>\n",
    "      Same as in Fig 1., each landmark is accompanied by a number of times observed.\n",
    "  </figcaption>\n",
    "</figure>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#mode = 'step_by_step'\n",
    "mode = 'visualize_process'\n",
    "#mode = 'non_stop'\n",
    "\n",
    "# WORLD MAP\n",
    "# Num features/landmarks considered within the map\n",
    "nLandmarks = 5\n",
    "# Generation of the map\n",
    "MapSize = 100\n",
    "Map = MapSize*random.rand(2,nLandmarks)-MapSize/2\n",
    "\n",
    "# ROBOT\n",
    "# Covariances for our very bad&expensive sensor (in the system <d,theta>)\n",
    "Sigma_r = 8.0\n",
    "Sigma_theta = 7*np.pi/180\n",
    "# Initial robot pose\n",
    "xVehicleTrue = np.vstack([-MapSize/1.5, -MapSize/1.5, 0.]) # We know the exact robot pose at any moment\n",
    "\n",
    "robot = EFKMappingRobot(xVehicleTrue, Sigma_r, Sigma_theta, nLandmarks)\n",
    "\n",
    "demo_ekf_mapping(robot, Map ,nLandmarks, mode=mode)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Getting performance results\n",
    "\n",
    "As is normal, the contracting company requires some information about how well our EFK implementation performs. For that, your colleages have implemented a looger, which is meant to store some information each loop regarding the method performance and plot it at the end of its execution.\n",
    "\n",
    "**<span style=\"color:orangered\">Include in *your report to Nirvana* a discussion about the information reported by this logger. Think about how a company would exepct this information to be, and provide it!</span>**:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import pandas as pd\n",
    "\n",
    "class Logger():\n",
    "    \"\"\" Logs info about the covariance and error of a map.\n",
    "    \n",
    "        Attrs:\n",
    "            n_features: Number of features in the world.\n",
    "            log_error: Matrix to store the error in the fitting for each landmark.\n",
    "            log_det: Matrix to store the determinant of the covariance matrix for each landmark.\n",
    "    \n",
    "    \"\"\"\n",
    "    def __init__(self, n_steps, n_features):\n",
    "        \"\"\" Initializes each matrix to log the information\n",
    "        \n",
    "            Args:\n",
    "                n_steps: Maximum number of steps our robot will take.\n",
    "                n_features: Number of features in the world.\n",
    "        \"\"\"\n",
    "        self.n_features = n_features\n",
    "        self.log_error = np.empty((n_steps,n_features))\n",
    "        self.log_det = np.empty((n_steps,n_features))\n",
    "            \n",
    "    def log(self, k: int, robot: EFKMappingRobot, Map: np.ndarray):\n",
    "        \"\"\" Computes relevant info about the error and covariances.\n",
    "        \n",
    "            It is called once per loop in the demo.\n",
    "        \n",
    "            Args:\n",
    "                k: Number of iteration we are at. Range: [0, n_steps)\n",
    "                robot: \n",
    "                Map:\n",
    "        \"\"\"\n",
    "        for idx in range(self.n_features):\n",
    "            tid = robot.MappedLandmarks[idx,0]\n",
    "            if tid <= -1:\n",
    "                self.log_error[k,idx]= np.Inf\n",
    "                self.log_det[k,idx]=np.Inf\n",
    "            else:\n",
    "                self.log_det[k,idx] = np.linalg.det(robot.PEst[tid:tid+2,tid:tid+2])\n",
    "                self.log_error[k,idx] = np.sqrt(np.sum((robot.xEst[tid:tid+2,0] - Map[:,idx])**2))\n",
    "                \n",
    "    def plot(self):\n",
    "        \"\"\" Plot all relevant figures. It is called at the end of the demo\"\"\"\n",
    "        fig1 , ax1 =plt.subplots(1, 1, sharex=True)\n",
    "        fig2 , ax2 =plt.subplots(1, 1, sharex=True)\n",
    "        fig2.tight_layout()\n",
    "        fig1.tight_layout()\n",
    "\n",
    "        df1 = pd.DataFrame(data= self.log_error, columns = ['Landmark {}'.format(i) for i in range(self.n_features)])\n",
    "        ax1.set_title('Error between map and est')\n",
    "        df1.plot(ax = ax1)\n",
    "        df2 = pd.DataFrame(data=np.log(self.log_det), columns=['Landmark {}'.format(i) for i in range(self.n_features)])\n",
    "        ax2.set_title('Det. of covar.')\n",
    "        df2.plot(ax = ax2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#mode = 'step_by_step'\n",
    "mode = 'visualize_process'\n",
    "#mode = 'non_stop'\n",
    "\n",
    "# WORLD MAP\n",
    "# Num features/landmarks considered within the map\n",
    "nLandmarks = 5\n",
    "# Generation of the map\n",
    "MapSize = 100\n",
    "Map = MapSize*random.rand(2,nLandmarks)-MapSize/2\n",
    "\n",
    "# ROBOT\n",
    "# Covariances for our very bad&expensive sensor (in the system <d,theta>)\n",
    "Sigma_r = 8.0\n",
    "Sigma_theta = 7*np.pi/180\n",
    "# Initial robot pose\n",
    "xVehicleTrue = np.vstack([-MapSize/1.5, -MapSize/1.5, 0.]) # We know the exact robot pose at any moment\n",
    "\n",
    "robot = EFKMappingRobot(xVehicleTrue, Sigma_r, Sigma_theta, nLandmarks)\n",
    "\n",
    "nSteps=100\n",
    "logger = Logger(n_features=nLandmarks, n_steps=nSteps)\n",
    "\n",
    "demo_ekf_mapping(robot,\n",
    "                 Map,\n",
    "                 nLandmarks,\n",
    "                 logger=logger,\n",
    "                 mode='non_stop',\n",
    "                 nSteps=nSteps)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Report to Nirvana\n",
    "In the cell below, include your report addressing the issues posed by the managers at ***<span style=\"color:seagreen\">Nirvana</span>***. You should include pictures, text, and whatever you need to justify your developments."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:orangered\">***Your report here***</span>"
   ]
  }
 ],
 "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.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}