EKF Localization/ex6.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Exercise 6 - EKF Localization\n",
    "\n",
    "In this assignment you need to implement an EKF algorithm for localizing a robot in a given landmark map. \n",
    "The data for this exercise is recorded on a differential drive robot equipped with a sensor able to detect the distance and the angle of landmarks (e.g., beacons). The figure below visualizes the landmark map and the actual trajectory (ground truth) taken by the robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import pickle\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from ex6 import *\n",
    "%matplotlib inline\n",
    "\n",
    "# read dataset\n",
    "data = pickle.load(open(\"dataset_2d_landmarks.p\", \"rb\"))\n",
    "\n",
    "# get landmark coordinates \n",
    "M = data['M']\n",
    "\n",
    "# get ground truth trajectory\n",
    "gt_traj = data['gt']\n",
    "\n",
    "# show map\n",
    "plt.figure(1)\n",
    "plt.plot(M[:,0], M[:,1], '^r')\n",
    "\n",
    "# show ground truth trajectory\n",
    "for i in range(0,len(gt_traj),10):\n",
    "    plt.plot(gt_traj[i][0],gt_traj[i][1], '.b')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following data is provided in `data`:\n",
    "\n",
    "- *M* is the map of the environment where the robot must localize\n",
    "- *odom* is the noisy odometry readings observed by the robot during navigation in the form: $\\hat{x}_t,\\hat{y}_t, \\hat{\\theta}_t$ in the odometry frame\n",
    "- *gt* is the ground truth trajectory (poses in the map frame), you may want to use it for checking your results\n",
    "- *z* are the sensor measurements\n",
    "\n",
    "Each measurement $z_t$ contains a set of observed landmarks $[\\rho_i; \\phi_i; id_i]$, where $\\rho_i$ is the measured distance, $\\phi_i$ is the measured angle, and $id_i$ is the id of the landmark.\n",
    "\n",
    "You can access the `data` as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Odom at step 10 is: \n",
      " [ 0.3449921   0.00058428 -0.02110238]\n",
      "Observation at step 10 is: \n",
      " [[ 2.1793557   1.27533718  3.5604415 ]\n",
      " [ 2.92652148 -1.4372858   0.4519207 ]\n",
      " [ 4.         11.         14.        ]]\n"
     ]
    }
   ],
   "source": [
    "# get odomety at timestamp 10\n",
    "odom_10 = data['odom'][10]\n",
    "print(\"Odom at step 10 is: \\n\",odom_10)\n",
    "\n",
    "# get observation at timestamp 10\n",
    "z_10 = data['z'][10]\n",
    "print(\"Observation at step 10 is: \\n\",z_10)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exploring the feature Map \n",
    "Just to know the shape and info provided  to develop correction step."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(30, 2)\n",
      "[9.84650853 3.26720195]\n"
     ]
    }
   ],
   "source": [
    "M = data[\"M\"]\n",
    "print(M.shape)\n",
    "#view the landmark with id 1\n",
    "print(M[1])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6.1 Prediction [3.0]\n",
    "\n",
    "The `ekf_predict` function computes a prediction about the robot's pose after moving by using the odometry motion model.\n",
    "\n",
    "It takes as input:\n",
    "\n",
    "- the current belief about the pose of the robot represented as a Gaussian distribution $\\mathcal{N}(\\mu_t,\\Sigma_t)$ \n",
    "- the odometry readings $u_t$\n",
    "\n",
    "The output is a prediction about the robot's pose $\\mathcal{N}(\\overline{\\mu}_{t+1},\\overline{\\Sigma}_{t+1})$.\n",
    "\n",
    "You can use your implementation of the `inverse_motion_model` function from Exercise 3 to compute the $u_t = [\\delta_{rot1}, \\delta_{trans}, \\delta_{rot2}]$  from the odometry information.\n",
    "\n",
    "Implement the `ekf_predict` function and verify that it is correct for some test input. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[2.]\n",
      " [1.]\n",
      " [0.]] \n",
      " [[ 2. -1. -1.]\n",
      " [-1.  2.  1.]\n",
      " [-1.  1.  1.]]\n"
     ]
    }
   ],
   "source": [
    "x_init = np.array([1, 0 , 0]).reshape(3,1)\n",
    "u = [[0,0,0],\n",
    "    [1, 1, 0]]\n",
    "P = np.eye(3,3)\n",
    "\n",
    "x_pred,P_pred = ekf_predict(x_init,P,u)\n",
    "print(x_pred,'\\n',P_pred)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the predicted output matches the required output for the desired model we can verify that it is true"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6.2 Correction [3.0]\n",
    "\n",
    "The `ekf_correct` implements the correction step of the EKF that corrects the prediction according to the sensor measurements.\n",
    "\n",
    "It takes as input:\n",
    "\n",
    "- the current prediction about the pose of the robot represented as a Gaussian distribution $\\mathcal{N}(\\overline{\\mu}_{t+1},\\overline{\\Sigma}_{t+1})$\n",
    "- the sensor measurements $z_t$\n",
    "\n",
    "The output is new belief about the robot's pose $\\mathcal{N}({\\mu}_{t+1},{\\Sigma}_{t+1})$.\n",
    "\n",
    "Implement the `ekf_correct` function and verify that it is correct for some test input"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[1.94772947]\n",
      " [2.26725661]\n",
      " [1.57772673]] \n",
      " [[ 7.84560095e-04  3.15219150e-05 -1.03225734e-04]\n",
      " [ 3.15219150e-05  4.17195332e-03 -5.57710648e-04]\n",
      " [-1.03225734e-04 -5.57710648e-04  2.55443628e-03]]\n",
      "[2.         2.33       1.57079633]\n"
     ]
    }
   ],
   "source": [
    "# 3x3 process noise\n",
    "sigma_x = 0.25  # [m]\n",
    "sigma_y = 0.25  # [m]\n",
    "sigma_theta = np.deg2rad(10)  # [rad]\n",
    "R = np.diag(np.array([sigma_x, sigma_y, sigma_theta])**2)\n",
    "\n",
    "# 2x2 observation noise\n",
    "sigma_r = 0.1  # [m]\n",
    "sigma_phi = np.deg2rad(5)  # [rad]\n",
    "Q = np.diag(np.array([sigma_r, sigma_phi])**2)\n",
    "\n",
    "x = np.array([2, 2, np.pi/2]).T \n",
    "P = np.array([[1, 0, 0],[0, 1, 0], [0, 0, np.pi/3]])\n",
    "\n",
    "\n",
    "for i in range(1,10):\n",
    "    u = [data['odom'][i-1],data['odom'][i]]\n",
    "    z = data['z'][i]\n",
    "    x_pred,P_pred = ekf_predict(x,P,u,R)\n",
    "    x,P = ekf_correct(x_pred, P_pred,z,data['M'],Q)\n",
    "print(x,'\\n',P)\n",
    "print(data[\"gt\"][10])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the output is close to the ground truth we can verify that the prediction and correction step have worked."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 6.3 Localization [3.0]\n",
    "\n",
    "Once you complete all the above functions, implement the main procedure of EKF localization `ekf_localization` which recursively estimates the pose of the robot using the odometry data and the sensor measurements.\n",
    "\n",
    "Assume the initial belief at time $t=0$ is:\n",
    "\n",
    "- $\\mu = [2, 2, \\pi/2]'$\n",
    "- $\n",
    "\\Sigma = \\left(\\begin{array}{cc} \n",
    "1 & 0 & 0\\\\\n",
    "0 & 1 & 0 \\\\\n",
    "0 & 0 & \\pi/3\n",
    "\\end{array}\\right)\n",
    "$ \n",
    "            \n",
    "The process noise $R$ and measurement noise $Q$ are defined as:\n",
    "- $R = \\left(\\begin{array}{cc} \n",
    "\\sigma_x^2 & 0 & 0 \\\\\n",
    "0 & \\sigma_y^2 & 0 \\\\\n",
    "0 & 0 &  \\sigma_{theta}^2\n",
    "\\end{array}\\right)\n",
    "$\n",
    "\n",
    "with $\\sigma_x = 0.25$ meters, $\\sigma_y = 0.25$ meters and $\\sigma_theta = 10$ degrees. \n",
    " \n",
    "- $Q = \n",
    "\\left(\\begin{array}{cc} \n",
    "\\sigma_r^2 & 0 \\\\\n",
    "0 & \\sigma_{phi}^2 \n",
    "\\end{array}\\right)\n",
    "$\n",
    "\n",
    "with $\\sigma_r = 0.10$ meters, $\\sigma_{phi} = 5$ degrees. \n",
    "\n",
    "Plot the belief about the position of the robot after the sequence of odometry readings and sensor measurements provided in `data` by using the `plot_state` function."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(3,)\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# 3x3 process noise\n",
    "sigma_x = 0.25  # [m]\n",
    "sigma_y = 0.25  # [m]\n",
    "sigma_theta = np.deg2rad(10)  # [rad]\n",
    "R = np.diag(np.array([sigma_x, sigma_y, sigma_theta])**2)\n",
    "\n",
    "# 2x2 observation noise\n",
    "sigma_r = 0.1  # [m]\n",
    "sigma_phi = np.deg2rad(5)  # [rad]\n",
    "Q = np.diag(np.array([sigma_r, sigma_phi])**2)\n",
    "\n",
    "# initial state\n",
    "mu = np.array([2, 2, np.pi/2]).T \n",
    "print(mu.shape)\n",
    "S = np.array([[1, 0, 0],[0, 1, 0], [0, 0, np.pi/3]])\n",
    "\n",
    "# visualize\n",
    "plt.figure()\n",
    "plot_state(mu, S, M)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 2.01745659]\n",
      " [ 1.82559368]\n",
      " [-3.13180168]]\n",
      "[ 1.97535062  1.80216482 -3.1292476 ]\n"
     ]
    }
   ],
   "source": [
    "mu = mu.reshape(3,1)\n",
    "est_traj =[[mu[0][0],mu[1][0]]]\n",
    "for i in range(1,len(data['odom'])):\n",
    "    u = [data['odom'][i-1],data['odom'][i]]\n",
    "    z = data['z'][i]\n",
    "    mu_pred,S_pred = ekf_predict(mu,S,u,R)\n",
    "    mu,S = ekf_correct(mu_pred, S_pred,z,M,Q)\n",
    "    est_traj.append([mu[0,0],mu[1,0]])   \n",
    "print(mu)\n",
    "print(data['gt'][2091])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# show map\n",
    "plt.figure(1)\n",
    "plt.plot(M[:,0], M[:,1], '^r')\n",
    "\n",
    "# show the estimated trajectory\n",
    "for i in range(0,len(est_traj),10):\n",
    "    plt.plot(est_traj[i][0],est_traj[i][1], '.g')\n",
    "    # plt.plot(gt_traj[i][0],gt_traj[i][1], '.y')"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.5 ('base')",
   "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.8.5"
  },
  "vscode": {
   "interpreter": {
    "hash": "668a9a6f29a1c92e0dfeece73ec52f37babcdd4312794a01ed8162fd33e4be24"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}