EKF_Localization.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "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.5.2"
    },
    "colab": {
      "name": "EKF Localization.ipynb",
      "provenance": [],
      "collapsed_sections": []
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nL6jJIopDNSV"
      },
      "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",
      "metadata": {
        "scrolled": true,
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 265
        },
        "id": "Ubv_vn3nDNSg",
        "outputId": "9dbdf4d0-2d7a-4868-b22d-608d13c7bfce"
      },
      "source": [
        "import pickle\n",
        "import numpy as np\n",
        "import matplotlib.pyplot as plt\n",
        "from ex9 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')"
      ],
      "execution_count": 1,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "image/png": "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\n",
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ]
          },
          "metadata": {
            "tags": [],
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "IGM7nICEDNSk"
      },
      "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",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "XsLKRufMDNSm",
        "outputId": "88ad55ff-5616-4b1e-f920-6f5f3589a7f3"
      },
      "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)"
      ],
      "execution_count": 2,
      "outputs": [
        {
          "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"
          ],
          "name": "stdout"
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "lWOQeSHyDNSn"
      },
      "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",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "Ui0p9NTsDNSn",
        "outputId": "6ea420f0-79d0-420d-d8f1-6457a46d4058"
      },
      "source": [
        "mu_t = np.array([2, 2, np.pi/2]) \n",
        "S_t = np.array([[1, 0, 0],[0, 1, 0], [0, 0, 1]])\n",
        "R_t = np.zeros((3,3))\n",
        "\n",
        "mu_p, S_p = ekf_predict(mu_t.reshape(3,1),S_t,[[0,0,0],[1,0,0]],R_t)\n",
        "print(mu_p)\n",
        "print(S_p)"
      ],
      "execution_count": 3,
      "outputs": [
        {
          "output_type": "stream",
          "text": [
            "[[2.        ]\n",
            " [3.        ]\n",
            " [1.57079633]]\n",
            "[[ 2.000000e+00 -6.123234e-17 -1.000000e+00]\n",
            " [-6.123234e-17  1.000000e+00  6.123234e-17]\n",
            " [-1.000000e+00  6.123234e-17  1.000000e+00]]\n"
          ],
          "name": "stdout"
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "wikUd9-1Hcmi"
      },
      "source": [
        "Here we can can see that without any process noise predict is equal to the command and uncertainity is increasing hence the model is correct"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "V24g0sD0DNSn"
      },
      "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",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "0kJe0LvcDNSo",
        "outputId": "e7879612-c7e4-424e-c7cc-dfcb1a7acb78"
      },
      "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_bar = np.array([2, 2, np.pi/2]) \n",
        "S_bar = np.array([[1, 0, 0],[0, 1, 0], [0, 0, np.pi/3]])\n",
        "\n",
        "\n",
        "for i in range(1,100):   \n",
        "    u_t = [data['odom'][i-1],data['odom'][i]]\n",
        "    mu_bar,S_bar = ekf_predict(mu_bar.reshape(3,1), S_bar, u_t,R)\n",
        "\n",
        "print(\"Only running predict\")\n",
        "print(mu_bar.T)\n",
        "print(S_bar)\n",
        "\n",
        "mu = np.array([2, 2, np.pi/2]) \n",
        "S = np.array([[1, 0, 0],[0, 1, 0], [0, 0, np.pi/3]])\n",
        "\n",
        "for i in range(1,100):   \n",
        "    u_t = [data['odom'][i-1],data['odom'][i]]\n",
        "    z = data['z'][i]\n",
        "    mu_bar, sigma_bar = ekf_predict(mu.reshape(3,1), S, u_t, R)\n",
        "    mu, S = ekf_correct(mu_bar, sigma_bar, z, Q, M)\n",
        "print(\"After running correct\")\n",
        "print(mu.T)\n",
        "print(S)\n",
        "\n",
        "print(gt_traj[100])"
      ],
      "execution_count": 4,
      "outputs": [
        {
          "output_type": "stream",
          "text": [
            "Only running predict\n",
            "[[1.89063269 5.01048817 1.62486271]]\n",
            "[[ 37.72779086   1.32764084 -16.78221971]\n",
            " [  1.32764084   7.24248271  -0.75091664]\n",
            " [-16.78221971  -0.75091664  10.25041001]]\n",
            "After running correct\n",
            "[[2.0040227 4.9373632 1.5636316]]\n",
            "[[ 2.75648868e-04  2.67331217e-05 -1.60102224e-05]\n",
            " [ 2.67331217e-05  2.15708967e-03  6.52565542e-05]\n",
            " [-1.60102224e-05  6.52565542e-05  1.25167499e-03]]\n",
            "[2.         5.03       1.57079633]\n"
          ],
          "name": "stdout"
        }
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Oh12mIgEHkiV"
      },
      "source": [
        "Here we can see that after running predict it is converging to ground truth and we can say that the function is working fine"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cz9bLaAqDNSo"
      },
      "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",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 281
        },
        "id": "MsmBRBmUDNSp",
        "outputId": "37cfaf3a-12bc-4ab7-e825-2f866fd2302b"
      },
      "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]) \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)"
      ],
      "execution_count": 5,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "image/png": "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\n",
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ]
          },
          "metadata": {
            "tags": [],
            "needs_background": "light"
          }
        }
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 281
        },
        "id": "FiT-znJ1DNSp",
        "outputId": "1bb9fe2e-9199-4384-fa0a-aa8ef8ea5d7f"
      },
      "source": [
        "for i in range(1,len(data['odom'])):   \n",
        "    u_t = [data['odom'][i-1], data['odom'][i]]\n",
        "    z = data['z'][i]\n",
        "\n",
        "    mu_bar, sigma_bar = ekf_predict(mu.reshape(3,1), S, u_t, R)\n",
        "    mu, S = ekf_correct(mu_bar, sigma_bar, z, Q, M)\n",
        "    plot_state(mu, S, M)"
      ],
      "execution_count": 6,
      "outputs": [
        {
          "output_type": "display_data",
          "data": {
            "image/png": "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\n",
            "text/plain": [
              "<Figure size 432x288 with 1 Axes>"
            ]
          },
          "metadata": {
            "tags": [],
            "needs_background": "light"
          }
        }
      ]
    }
  ]
}