1. Localization.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# This file is practice for the AI for Robotics course by Sebastian Thrun"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.2, 0.2, 0.2, 0.2, 0.2]\n"
     ]
    }
   ],
   "source": [
    "# Robot localization\n",
    "# Generalized Uniform Distribution\n",
    "\n",
    "# ------------ Probability prior to sensing ------------------\n",
    "#Lets say there are five locations ans the robot initially has the probability to be in any location\n",
    "#  Modify your code to create probability vectors, p, of arbitrary \n",
    "#  size, n. Use n=5 to verify that your new solution matches \n",
    "#  the previous one.\n",
    "\n",
    "\n",
    "p=[]\n",
    "n=5\n",
    "\n",
    "value = 1./n\n",
    "for i in range(n):\n",
    "    p.append(value)\n",
    "\n",
    "print (p)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### ------------- Probability after sensing  (Posterior) ------------\n",
    "\n",
    "- Multiply the probabilites of the objects you want to detect by a large number and the others with a smaller number\n",
    "- Only problem arises is that the sum of all probability distributions don't add up to one\n",
    "- We can bring it to 1 by normalizing it. Dividing all values by the sum"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.04000000000000001, 0.12, 0.12, 0.04000000000000001, 0.04000000000000001]\n"
     ]
    }
   ],
   "source": [
    "#Write code that outputs p after multiplying each entry \n",
    "#by pHit or pMiss at the appropriate places. Remember that\n",
    "#the red cells 1 and 2 are hits and the other green cells\n",
    "#are misses.\n",
    "\n",
    "p=[0.2,0.2,0.2,0.2,0.2]\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "\n",
    "#Enter code here\n",
    "for i in range(len(p)):\n",
    "    if i == 1 or i == 2:\n",
    "        p[i] *= pHit\n",
    "    else:\n",
    "        p[i] *= pMiss\n",
    "print(p)\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.04000000000000001, 0.12, 0.12, 0.04000000000000001, 0.04000000000000001]\n",
      "0.3600000000000001\n"
     ]
    }
   ],
   "source": [
    "#Modify the program to find and print the sum of all \n",
    "#the entries in the list p.\n",
    "\n",
    "p=[0.2,0.2,0.2,0.2,0.2]\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "sum = 0\n",
    "\n",
    "#Enter code here\n",
    "for i in range(len(p)):\n",
    "    if i == 1 or i == 2:\n",
    "        p[i] *= pHit\n",
    "    else:\n",
    "        p[i] *= pMiss\n",
    "    sum += p[i]\n",
    "print(p)\n",
    "print(sum)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.04000000000000001, 0.12, 0.12, 0.04000000000000001, 0.04000000000000001]\n"
     ]
    }
   ],
   "source": [
    "#Modify the code below so that the function sense, which \n",
    "#takes p and Z as inputs, will output the NON-normalized \n",
    "#probability distribution, q, after multiplying the entries \n",
    "#in p by pHit or pMiss according to the color in the \n",
    "#corresponding cell in world.\n",
    "\n",
    "\n",
    "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "Z = 'red'\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "\n",
    "def sense(p, Z):\n",
    "    #Enter code here\n",
    "    for i in range(len(p)):\n",
    "        if world[i] == Z:\n",
    "            p[i] *= pHit\n",
    "        else:\n",
    "            p[i] *= pMiss\n",
    "    return p\n",
    "\n",
    "print (sense(p,Z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.1111111111111111, 0.3333333333333332, 0.3333333333333332, 0.1111111111111111, 0.1111111111111111]\n"
     ]
    }
   ],
   "source": [
    "#Modify your code so that it normalizes the output for \n",
    "#the function sense. This means that the entries in q \n",
    "#should sum to one.\n",
    "\n",
    "\n",
    "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "Z = 'red'\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    sum = 0\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "        sum += q[i]\n",
    "    q = [x/sum for x in q]\n",
    "    return q\n",
    "print (sense(p,Z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.2727272727272727, 0.09090909090909093, 0.09090909090909093, 0.2727272727272727, 0.2727272727272727]\n"
     ]
    }
   ],
   "source": [
    "#Try using your code with a measurement of 'green' and \n",
    "#make sure the resulting probability distribution is correct.\n",
    "\n",
    "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "Z = 'green'\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    sum = 0\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "        sum += q[i]\n",
    "    for i in range(len(p)):\n",
    "        q[i]=q[i]/sum \n",
    "    return q\n",
    "print (sense(p, Z))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.20000000000000004, 0.19999999999999996, 0.19999999999999996, 0.20000000000000004, 0.20000000000000004]\n"
     ]
    }
   ],
   "source": [
    "#Modify the code so that it updates the probability twice\n",
    "#and gives the posterior distribution after both \n",
    "#measurements are incorporated. Make sure that your code \n",
    "#allows for any sequence of measurement of any length.\n",
    "\n",
    "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "measurements = ['red', 'green']\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    sum = 0\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "        sum += q[i]\n",
    "    for i in range(len(q)):\n",
    "        q[i] = q[i] / sum\n",
    "    return q\n",
    "for measurement in measurements:\n",
    "    p = sense(p, measurement)\n",
    "print (p)\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Robot Motion\n",
    "Cyclic rotation of robot<br>\n",
    "Exact Motion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0, 0, 1, 0, 0]\n"
     ]
    }
   ],
   "source": [
    "#Program a function that returns a new distribution \n",
    "#q, shifted to the right by U units. If U=0, q should \n",
    "#be the same as p.\n",
    "\n",
    "p=[0, 1, 0, 0, 0]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "measurements = ['red', 'green']\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "    s = sum(q)\n",
    "    for i in range(len(q)):\n",
    "        q[i] = q[i] / s\n",
    "    return q\n",
    "\n",
    "def move(p, U):\n",
    "    U = U%len(p)\n",
    "    q = p[-U:] + p[:-U]\n",
    "    return q\n",
    "\n",
    "print (move(p, 1))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Inexact Motion<br>\n",
    "Robot tries to travel a certain number of cells but occasionally either falls short or overshoots the travelled cell distance.<br>\n",
    "This is an innacurate motion. This is the reason why localization is very difficult"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.0, 0.1, 0.8, 0.1, 0.0]\n",
      "[0.010000000000000002, 0.010000000000000002, 0.16000000000000003, 0.6600000000000001, 0.16000000000000003]\n",
      "[0.20000000000000365, 0.20000000000000373, 0.20000000000000365, 0.2000000000000035, 0.2000000000000035]\n"
     ]
    }
   ],
   "source": [
    "#Modify the move function to accommodate the added \n",
    "#probabilities of overshooting or undershooting \n",
    "#the intended destination.\n",
    "\n",
    "p=[0, 1, 0, 0, 0]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "measurements = ['red', 'green']\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "pExact = 0.8\n",
    "pOvershoot = 0.1\n",
    "pUndershoot = 0.1\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "    s = sum(q)\n",
    "    for i in range(len(q)):\n",
    "        q[i] = q[i] / s\n",
    "    return q\n",
    "\n",
    "def move(p, U):\n",
    "    q = []\n",
    "    for i in range(len(p)):\n",
    "        exact = p[(i-U)%len(p)] * pExact\n",
    "        under = p[(i-U-1)%len(p)] * pUndershoot\n",
    "        over = p[(i-U+1)%len(p)] * pOvershoot\n",
    "        q.append(exact+under+over)\n",
    "    return q\n",
    "    \n",
    "\n",
    "print (move(p, 1)) # Move Once\n",
    "print (move((move(p, 1)), 1)) # Move Twice\n",
    "\n",
    "for _ in range(1000):\n",
    "    p = move(p, 1)\n",
    "\n",
    "print(p) # moved a thousand times\n"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Sensing and Moving<br>\n",
    "The robot moving a certain distance and then sensing. We end up with a posterior distribution belief after each"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.21157894736842103, 0.1515789473684211, 0.08105263157894739, 0.16842105263157897, 0.3873684210526316]\n"
     ]
    }
   ],
   "source": [
    "#Given the list motions=[1,1] which means the robot \n",
    "#moves right and then right again, compute the posterior \n",
    "#distribution if the robot first senses red, then moves \n",
    "#right one, then senses green, then moves right again, \n",
    "#starting with a uniform prior distribution.\n",
    "\n",
    "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "measurements = ['red', 'green']\n",
    "motions = [1,1]\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "pExact = 0.8\n",
    "pOvershoot = 0.1\n",
    "pUndershoot = 0.1\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "    s = sum(q)\n",
    "    for i in range(len(q)):\n",
    "        q[i] = q[i] / s\n",
    "    return q\n",
    "\n",
    "def move(p, U):\n",
    "    q = []\n",
    "    for i in range(len(p)):\n",
    "        s = pExact * p[(i-U) % len(p)]\n",
    "        s = s + pOvershoot * p[(i-U-1) % len(p)]\n",
    "        s = s + pUndershoot * p[(i-U+1) % len(p)]\n",
    "        q.append(s)\n",
    "    return q\n",
    "\n",
    "for i in range(len(measurements)):\n",
    "    p = sense(p, measurements[i])\n",
    "    p = move(p, motions[i])\n",
    "print (p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.1111111111111111, 0.3333333333333332, 0.3333333333333332, 0.1111111111111111, 0.1111111111111111]\n",
      "[0.058823529411764726, 0.211764705882353, 0.49411764705882344, 0.16470588235294117, 0.07058823529411765]\n",
      "[0.07882352941176471, 0.07529411764705884, 0.22470588235294123, 0.4329411764705882, 0.18823529411764706]\n"
     ]
    }
   ],
   "source": [
    "# Sense and move 2\n",
    "#Modify the previous code so that the robot senses red twice.\n",
    "\n",
    "p=[0.2, 0.2, 0.2, 0.2, 0.2]\n",
    "world=['green', 'red', 'red', 'green', 'green']\n",
    "measurements = ['red', 'green']\n",
    "motions = [1,1]\n",
    "pHit = 0.6\n",
    "pMiss = 0.2\n",
    "pExact = 0.8\n",
    "pOvershoot = 0.1\n",
    "pUndershoot = 0.1\n",
    "\n",
    "def sense(p, Z):\n",
    "    q=[]\n",
    "    for i in range(len(p)):\n",
    "        hit = (Z == world[i])\n",
    "        print((hit * pHit + (1-hit) * pMiss))\n",
    "        q.append(p[i] * (hit * pHit + (1-hit) * pMiss))\n",
    "    s = sum(q)\n",
    "    for i in range(len(q)):\n",
    "        q[i] = q[i] / s\n",
    "    return q\n",
    "\n",
    "def move(p, U):\n",
    "    q = []\n",
    "    for i in range(len(p)):\n",
    "        s = pExact * p[(i-U) % len(p)]\n",
    "        s = s + pOvershoot * p[(i-U-1) % len(p)]\n",
    "        s = s + pUndershoot * p[(i-U+1) % len(p)]\n",
    "        q.append(s)\n",
    "    return q\n",
    "\n",
    "for k in range(len(measurements)):\n",
    "    p = sense(p, 'red')\n",
    "    p = move(p, motions[k])\n",
    "    \n",
    "print (p)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Localization: Problem set - Programming Assignment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0.01106,0.02464,0.06800,0.04472,0.02465],\n",
      " [0.00715,0.01017,0.08697,0.07988,0.00935],\n",
      " [0.00740,0.00894,0.11273,0.35351,0.04066],\n",
      " [0.00911,0.00715,0.01435,0.04313,0.03643]]\n"
     ]
    }
   ],
   "source": [
    "# The function localize takes the following arguments:\n",
    "#\n",
    "# colors:\n",
    "#        2D list, each entry either 'R' (for red cell) or 'G' (for green cell)\n",
    "#\n",
    "# measurements:\n",
    "#        list of measurements taken by the robot, each entry either 'R' or 'G'\n",
    "#\n",
    "# motions:\n",
    "#        list of actions taken by the robot, each entry of the form [dy,dx],\n",
    "#        where dx refers to the change in the x-direction (positive meaning\n",
    "#        movement to the right) and dy refers to the change in the y-direction\n",
    "#        (positive meaning movement downward)\n",
    "#        NOTE: the *first* coordinate is change in y; the *second* coordinate is\n",
    "#              change in x\n",
    "#\n",
    "# sensor_right:\n",
    "#        float between 0 and 1, giving the probability that any given\n",
    "#        measurement is correct; the probability that the measurement is\n",
    "#        incorrect is 1-sensor_right\n",
    "#\n",
    "# p_move:\n",
    "#        float between 0 and 1, giving the probability that any given movement\n",
    "#        command takes place; the probability that the movement command fails\n",
    "#        (and the robot remains still) is 1-p_move; the robot will NOT overshoot\n",
    "#        its destination in this exercise\n",
    "#\n",
    "# The function should RETURN (not just show or print) a 2D list (of the same\n",
    "# dimensions as colors) that gives the probabilities that the robot occupies\n",
    "# each cell in the world.\n",
    "#\n",
    "# Compute the probabilities by assuming the robot initially has a uniform\n",
    "# probability of being in any cell.\n",
    "#\n",
    "# Also assume that at each step, the robot:\n",
    "# 1) first makes a movement,\n",
    "# 2) then takes a measurement.\n",
    "#\n",
    "# Motion:\n",
    "#  [0,0] - stay\n",
    "#  [0,1] - right\n",
    "#  [0,-1] - left\n",
    "#  [1,0] - down\n",
    "#  [-1,0] - up\n",
    "\n",
    "def sense(p, measurement, colors, sensor_right):\n",
    "\n",
    "    a=[]\n",
    "    for i in range(len(p)):\n",
    "        q = []\n",
    "        for j in range(len(p[0])):\n",
    "            hit = (measurement == colors[i][j])\n",
    "            q.append(p[i][j] * (hit * sensor_right + (1-hit) * (1-sensor_right)))\n",
    "        a.append(q)\n",
    "    si = sum(sum(a, []))\n",
    "    for i in range(len(a)):  # Normalizing the distribution\n",
    "        for j in range(len(a[0])):\n",
    "            a[i][j] = a[i][j] / si\n",
    "    return a\n",
    "\n",
    "def move(p, motion, p_move):\n",
    "    dy = motion[0]\n",
    "    dx = motion[1]\n",
    "    b = [[0.0 for row in range(len(p[0]))] for col in range(len(p))]\n",
    "    for i in range(len(p)):\n",
    "        for j in range(len(p[0])):\n",
    "            s = p_move * (p[(i-dy) % len(p)][(j-dx) % len(p[i])]) # Movement takes place\n",
    "            s += (1 - p_move) * p[i][j] # Movement doesn't take place\n",
    "            b[i][j] = s\n",
    "    return b\n",
    "        \n",
    "\n",
    "def localize(colors,measurements,motions,sensor_right,p_move):\n",
    "    # initializes p to a uniform distribution over a grid of the same dimensions as colors\n",
    "    pinit = 1.0 / float(len(colors)) / float(len(colors[0]))\n",
    "    p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]\n",
    "    \n",
    "    # >>> Insert your code here <<<\n",
    "\n",
    "    \n",
    "\n",
    "    # def sense(p, Z):\n",
    "    for k in range(len(measurements)):\n",
    "        p = move(p, motions[k], p_move)\n",
    "        p = sense(p, measurements[k], colors, sensor_right)\n",
    "\n",
    "    #     # Second we sense the robot\n",
    "    #     a=[]\n",
    "    #     for i in range(len(p)):\n",
    "    #         q = []\n",
    "    #         for j in range(len(p[0])):\n",
    "    #             hit = (measurements[k] == colors[i][j])\n",
    "    #             q.append(p[i][j] * (hit * sensor_right + (1-hit) * (1-sensor_right)))\n",
    "    #         a.append(q)\n",
    "    #     si = sum(sum(a, []))\n",
    "    #     for i in range(len(a)):  # Normalizing the distribution\n",
    "    #         for j in range(len(a[0])):\n",
    "    #             a[i][j] = a[i][j] / si\n",
    "\n",
    "\n",
    "    # # First we move\n",
    "    #     dy = motions[k][0]\n",
    "    #     dx = motions[k][1]\n",
    "    #     b = [[0.0 for row in range(len(p[0]))] for col in range(len(p))]\n",
    "    #     for i in range(len(p)):\n",
    "    #         for j in range(len(p[0])):\n",
    "    #             s = p_move * (a[(i-dy) % len(p)][(j-dx) % len(p[i])]) # Movement takes place\n",
    "    #             s = (1 - p_move) * a[i][j]\n",
    "    #             b[i][j] = s\n",
    "\n",
    "    return p\n",
    "\n",
    "def show(p):\n",
    "    rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]\n",
    "    print ('[' + ',\\n '.join(rows) + ']')\n",
    "    \n",
    "#############################################################\n",
    "# For the following test case, your output should be \n",
    "# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],\n",
    "#  [0.00715, 0.01017, 0.08696, 0.07988, 0.00935],\n",
    "#  [0.00739, 0.00894, 0.11272, 0.35350, 0.04065],\n",
    "#  [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]]\n",
    "# (within a tolerance of +/- 0.001 for each entry)\n",
    "\n",
    "colors = [['R','G','G','R','R'],\n",
    "          ['R','R','G','R','R'],\n",
    "          ['R','R','G','G','R'],\n",
    "          ['R','R','R','R','R']]\n",
    "measurements = ['G','G','G','G','G']\n",
    "motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]\n",
    "p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)\n",
    "show(p) # displays your answer\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.7 (PyTorch_env)",
   "language": "python",
   "name": "torch"
  },
  "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.9.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}