PFilterRobot.ipynb

{
 "metadata": {
  "name": "PFilterRobot"
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "import random\n",
      "print random.gauss(0, 5)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "-3.16326560262\n"
       ]
      }
     ],
     "prompt_number": 8
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "import random\n",
      "from numpy import cumsum, sort, sum, searchsorted\n",
      "from numpy.random import rand\n",
      "\n",
      "S = 0.001\n",
      "M = 1000\n",
      "N = 100\n",
      "\n",
      "partX = [0.0]*M\n",
      "partY = [0.0]*M\n",
      "partTheta = [0.0]*M\n",
      "partW = [0.0]*M\n",
      "\n",
      "\n",
      "X = 0\n",
      "Y = 0\n",
      "theta = 0\n",
      "\n",
      "for i in range(M):\n",
      "    partX[i]=0\n",
      "    partY[i]=0\n",
      "    partTheta[i]=0\n",
      "    partW[i]=1.0/M\n",
      "    \n",
      "def sampleParts():\n",
      "    n_picks=1\n",
      "    t = cumsum(partW)\n",
      "    s = sum(partW)\n",
      "    return searchsorted(t, rand(n_picks)*s)\n",
      "\n",
      "j = sampleParts()\n",
      "print \"j = \" + str(j[0])"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "j = 832\n"
       ]
      }
     ],
     "prompt_number": 33
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "print partW"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "[0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]\n"
       ]
      }
     ],
     "prompt_number": 34
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "# William Lyon\n",
      "# CSCI 555\n",
      "# HW 9 - Robot Localization Particle Filter algorithm\n",
      "# PFilterRobot.py - takes 3 command line arguments: S, M, N\n",
      "# S = std deviation of random pertuerbation of theta\n",
      "# M = number of particles\n",
      "# N = number of time steps\n",
      "\n",
      "# X = x coord of robot\n",
      "# Y = y coord of robot\n",
      "# theta = angle of orientation of robot\n",
      "\n",
      "# output is X, Y, theta position of robot, theta adjustment, avg y value of particles, # particles above/below track for every timestep\n",
      "\n",
      "import random\n",
      "from numpy import cumsum, sort, sum, searchsorted\n",
      "from numpy.random import rand\n",
      "\n",
      "S = 0.01\n",
      "M = 1000\n",
      "N = 100\n",
      "\n",
      "partX = [0.0]*M\n",
      "partY = [0.0]*M\n",
      "partTheta = [0.0]*M\n",
      "partW = [0.0]*M\n",
      "\n",
      "\n",
      "X = 0\n",
      "Y = 0\n",
      "theta = 0\n",
      "\n",
      "for i in range(M):\n",
      "    partX[i]=0.0\n",
      "    partY[i]=0.0\n",
      "    partTheta[i]=0.0\n",
      "    partW[i]=1.0/M\n",
      "    \n",
      "def sampleParts():\n",
      "    n_picks=1\n",
      "    t = cumsum(partW)\n",
      "    s = sum(partW)\n",
      "    return searchsorted(t, rand(n_picks)*s)[0]\n",
      "\n",
      "def robotMoveX():\n",
      "    return X + (10*math.cos(theta))\n",
      "\n",
      "def robotMoveY():\n",
      "    return Y + (10*math.sin(theta))\n",
      "\n",
      "def robotMoveTheta():\n",
      "    pass\n",
      "\n",
      "def getSensor():\n",
      "    if Y > 2:\n",
      "        return 'L'\n",
      "    elif Y < -2:\n",
      "        return 'R'\n",
      "    elif Y >= 0:\n",
      "        rnd = random.random()\n",
      "        if rnd < 1-Y/2:\n",
      "            return 'C'\n",
      "        else:\n",
      "            return 'L'\n",
      "    else:\n",
      "        rnd = random.random()\n",
      "        if rnd < 1+Y/2:\n",
      "            return 'C'\n",
      "        else:\n",
      "            return 'R'\n",
      "\n",
      "def RobotUpdate(intAdjust):\n",
      "    randomAdj = random.gauss(0, S)\n",
      "    global theta\n",
      "    global X\n",
      "    global Y\n",
      "    \n",
      "    theta = theta + randomAdj + intAdjust\n",
      "    X = robotMoveX()\n",
      "    Y = robotMoveY()\n",
      "    #theta = robotMoveTheta() ???\n",
      "    sensor = getSensor()\n",
      "    print \"Robot position (X, Y, theta): (\" + str(X) + \", \" + str(Y) + \", \" + str(theta)+ \")\",\n",
      "    return sensor\n",
      "\n",
      "def getWeight(part_sensor, part_y):\n",
      "    if part_sensor=='L' and part_y>2:\n",
      "        return 1\n",
      "    elif part_sensor=='R' and part_y<-2:\n",
      "        return 1\n",
      "    elif part_y >= 0:\n",
      "        if part_sensor == 'C':\n",
      "            return 1-part_y/2\n",
      "        else:\n",
      "            return part_y/2\n",
      "    else:\n",
      "        if part_sensor == 'C':\n",
      "            return 1+part_y/2\n",
      "        else:\n",
      "            return -part_y/2\n",
      "        \n",
      "def ParticleUpdate(sensor):\n",
      "    global partX\n",
      "    global partY\n",
      "    global partW\n",
      "    global partTheta\n",
      "\n",
      "    partX_prime = [0.0]*M\n",
      "    partY_prime = [0.0]*M\n",
      "    partTheta_prime = [0.0]*M\n",
      "    partW_prime = [0.0]*M\n",
      "    sum_Y = 0.0\n",
      "    sum_above = 0\n",
      "    sum_below = 0\n",
      "    for i in range(M):\n",
      "        j = sampleParts()\n",
      "       # print \"J = \" + str(j)\n",
      "        X_prime = partX[j]+ (10*math.cos(partTheta[j]))\n",
      "        Y_prime = partY[j]+ (10*math.sin(partTheta[j]))\n",
      "        theta_prime = partTheta[j] # how to move theta????\n",
      "        sum_Y += Y_prime\n",
      "        if Y_prime > 1:\n",
      "            sum_above += 1\n",
      "        elif Y_prime < -1:\n",
      "            sum_below += 1\n",
      "        W_prime = getWeight(sensor, Y_prime)\n",
      "        \n",
      "        partX_prime[i] = X_prime\n",
      "        partY_prime[i] = Y_prime\n",
      "        partTheta_prime[i] = theta_prime\n",
      "        partW_prime[i] = W_prime\n",
      "    \n",
      "    avg_Y = sum_Y / M\n",
      "    print \"avg y = \" + str(avg_Y),\n",
      "    \n",
      "    if avg_Y >= -1 and avg_Y <= 1:\n",
      "        adjustment = 0\n",
      "    elif avg_Y < -1:\n",
      "        adjustment = 0.1\n",
      "    else:\n",
      "        adjustment = -0.1\n",
      "        \n",
      "    for i in range(M):\n",
      "        partTheta_prime[i] += adjustment + random.gauss(0, S)\n",
      "        \n",
      "    print \"Adjustment: \" + str(adjustment),\n",
      "    print \"Particles above track: \" + str(sum_above),\n",
      "    print \"Particles below track: \" + str(sum_below)\n",
      "    \n",
      "    partX = partX_prime\n",
      "    partY = partY_prime\n",
      "    partTheta = partTheta_prime\n",
      "    partW = partW_prime\n",
      "    return adjustment\n",
      "\n",
      "def Assignment9(S, M, N):\n",
      "    intAdjust = 0\n",
      "    for t in range(N):\n",
      "        print \"Timestep \"+str(t),\n",
      "        sensor = RobotUpdate(intAdjust)\n",
      "        intAdjust = ParticleUpdate(sensor)\n",
      "        \n",
      "\n",
      "Assignment9(S, M, N)\n",
      "\n"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "Timestep 0 Robot position (X, Y, theta): (9.99978262675, 0.0659349513694, 0.00659354291234) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.0 Adjustment: 0 Particles above track: 0 Particles below track: 0\n",
        "Timestep 1 Robot position (X, Y, theta): (19.9997110988, 0.103757606217, 0.00378227450269) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -0.00307884041996 Adjustment: 0 Particles above track: 0 Particles below track: 0\n",
        "Timestep 2 Robot position (X, Y, theta): (29.9997110343, 0.104893563227, 0.000113595701242) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -0.000397755670809 Adjustment: 0 Particles above track: 0 Particles below track: 0\n",
        "Timestep 3 Robot position (X, Y, theta): (39.9990766196, 0.217534107942, 0.0112642926803) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -0.00843227923166 Adjustment: 0 Particles above track: 1 Particles below track: 5\n",
        "Timestep 4 Robot position (X, Y, theta): (49.9921318297, 0.590156662163, 0.0372708837702) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.00955266474703 Adjustment: 0 Particles above track: 93 Particles below track: 69\n",
        "Timestep 5 Robot position (X, Y, theta): (59.9890921306, 0.836702279282, 0.0246570600972) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.0758302490815 Adjustment: 0 Particles above track: 317 Particles below track: 227\n",
        "Timestep 6 Robot position (X, Y, theta): (69.9789003216, 1.28806943237, 0.0451520557218) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.117858460503 Adjustment: 0 Particles above track: 460 Particles below track: 367\n",
        "Timestep 7 Robot position (X, Y, theta): (79.9672585421, 1.77045937767, 0.0482577228434) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.0481319330398 Adjustment: 0 Particles above track: 507 Particles below track: 447\n",
        "Timestep 8 Robot position (X, Y, theta): (89.9473843219, 2.40060969248, 0.0630568105159) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -0.48756656716 Adjustment: 0 Particles above track: 444 Particles below track: 547\n",
        "Timestep 9 Robot position (X, Y, theta): (99.9325388022, 2.9453022699, 0.0544962279066) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -1.42857600447 Adjustment: 0.1 Particles above track: 337 Particles below track: 657\n",
        "Timestep 10 Robot position (X, Y, theta): (109.812936361, 4.48729581029, 0.15481706421) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -1.78699943396 Adjustment: 0.1 Particles above track: 215 Particles below track: 778\n",
        "Timestep 11 Robot position (X, Y, theta): (119.463410646, 7.10804876906, 0.265172036985) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -1.22609550354 Adjustment: 0.1 Particles above track: 132 Particles below track: 787\n",
        "Timestep 12 Robot position (X, Y, theta): (128.772082208, 10.761626982, 0.374017169747) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.547094904591 Adjustment: 0 Particles above track: 171 Particles below track: 391\n",
        "Timestep 13 Robot position (X, Y, theta): (138.077537508, 14.4233890802, 0.374896489505) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 3.24999344147 Adjustment: -0.1 Particles above track: 435 Particles below track: 64\n",
        "Timestep 14 Robot position (X, Y, theta): (147.685216682, 17.1969248882, 0.281038526407) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 7.72852064826 Adjustment: -0.1 Particles above track: 823 Particles below track: 1\n",
        "Timestep 15 Robot position (X, Y, theta): (157.536098292, 18.9174281611, 0.172910656711) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 9.72772045577 Adjustment: -0.1 Particles above track: 956 Particles below track: 9\n",
        "Timestep 16 Robot position (X, Y, theta): (167.516926469, 19.5363537672, 0.0619321440747) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 10.2875223615 Adjustment: -0.1 Particles above track: 941 Particles below track: 11\n",
        "Timestep 17 Robot position (X, Y, theta): (177.510006451, 19.1643961591, -0.0372043430233) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 9.60134472525 Adjustment: -0.1 Particles above track: 796 Particles below track: 36\n",
        "Timestep 18 Robot position (X, Y, theta): (187.402408183, 17.701389671, -0.146827640862) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 9.01924684907 Adjustment: -0.1 Particles above track: 718 Particles below track: 145\n",
        "Timestep 19 Robot position (X, Y, theta): (197.127736053, 15.3737303718, -0.234920767376) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 4.45171527209 Adjustment: -0.1 Particles above track: 532 Particles below track: 386\n",
        "Timestep 20 Robot position (X, Y, theta): (206.629119996, 12.255445078, -0.317116913668) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -9.53354164062 Adjustment: 0.1 Particles above track: 229 Particles below track: 771\n",
        "Timestep 21 Robot position (X, Y, theta): (216.439047482, 10.3150011289, -0.1952832357) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -19.8610168956 Adjustment: 0.1 Particles above track: 41 Particles below track: 959\n",
        "Timestep 22 Robot position (X, Y, theta): (226.380456122, 9.23407825616, -0.10830389243) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -25.158726103 Adjustment: 0.1 Particles above track: 3 Particles below track: 997\n",
        "Timestep 23 Robot position (X, Y, theta): (236.379306858, 9.08247368779, -0.015161037642) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -28.0540324605 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 24 Robot position (X, Y, theta): (246.361167196, 9.68452368701, 0.060241429636) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -30.0052391892 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 25 Robot position (X, Y, theta): (256.230946095, 11.2930834429, 0.161557861039) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -30.8519026797 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 26 Robot position (X, Y, theta): (265.852253701, 14.0189667751, 0.276082219675) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -30.705524932 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 27 Robot position (X, Y, theta): (275.124047102, 17.7651444453, 0.383984486594) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -29.5548027602 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 28 Robot position (X, Y, theta): (283.955184227, 22.4567344828, 0.488338228857) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -27.4399946928 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 29 Robot position (X, Y, theta): (292.277437037, 28.0011107467, 0.587687067059) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -24.5803412751 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 30 Robot position (X, Y, theta): (300.087457943, 34.2463948438, 0.674527559872) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -20.963730405 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 31 Robot position (X, Y, theta): (307.256165474, 41.2184556825, 0.771492694325) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -16.5308661533 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 32 Robot position (X, Y, theta): (313.732604183, 48.8378875281, 0.8663082331) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -11.9581325431 Adjustment: 0.1 Particles above track: 1 Particles below track: 998\n",
        "Timestep 33 Robot position (X, Y, theta): (319.272335618, 57.1632329036, 0.983667277533) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -6.77381072477 Adjustment: 0.1 Particles above track: 40 Particles below track: 901\n",
        "Timestep 34 Robot position (X, Y, theta): (324.019850089, 65.9644320461, 1.07611472458) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -1.61590373191 Adjustment: 0.1 Particles above track: 167 Particles below track: 659\n",
        "Timestep 35 Robot position (X, Y, theta): (327.915484878, 75.174428224, 1.17063874618) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 4.73528600918 Adjustment: -0.1 Particles above track: 955 Particles below track: 0\n",
        "Timestep 36 Robot position (X, Y, theta): (332.65238962, 83.9813421817, 1.07731981988) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 12.0495781322 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 37 Robot position (X, Y, theta): (338.246130483, 92.2704955258, 0.977165819072) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 18.1836964048 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 38 Robot position (X, Y, theta): (344.637461622, 99.9614568621, 0.877425739056) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 23.4422950717 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 39 Robot position (X, Y, theta): (351.693371662, 107.047650022, 0.787539504911) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 27.9844724434 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 40 Robot position (X, Y, theta): (359.401205542, 113.418623015, 0.690726467493) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 31.4993022251 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 41 Robot position (X, Y, theta): (367.722783782, 118.964011694, 0.587808723518) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 34.1717179031 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 42 Robot position (X, Y, theta): (376.572492594, 123.620475366, 0.484364853475) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 35.7426889121 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 43 Robot position (X, Y, theta): (385.835335422, 127.38872997, 0.386366720888) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 36.5766772036 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 44 Robot position (X, Y, theta): (395.49455227, 129.977074976, 0.261815388527) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 36.4506228488 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 45 Robot position (X, Y, theta): (405.389827639, 131.420517174, 0.144850219859) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 35.240604942 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 46 Robot position (X, Y, theta): (415.379612298, 131.872404853, 0.0452041615113) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 32.8182633012 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 47 Robot position (X, Y, theta): (425.365010464, 131.332197977, -0.0540469963317) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 29.5358339868 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 48 Robot position (X, Y, theta): (435.253718997, 129.844433365, -0.149330848302) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 24.882657989 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 49 Robot position (X, Y, theta): (444.929412419, 127.318377783, -0.255372202874) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 19.9719936657 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 50 Robot position (X, Y, theta): (454.281757449, 123.778089315, -0.361875456761) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 14.031102412 Adjustment: -0.1 Particles above track: 984 Particles below track: 5\n",
        "Timestep 51 Robot position (X, Y, theta): (463.273321416, 119.401814937, -0.452958320263) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 7.47554039757 Adjustment: -0.1 Particles above track: 759 Particles below track: 179\n",
        "Timestep 52 Robot position (X, Y, theta): (471.845657537, 114.252537956, -0.540923303811) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -0.920433104328 Adjustment: 0 Particles above track: 401 Particles below track: 507\n",
        "Timestep 53 Robot position (X, Y, theta): (480.454309238, 109.16420661, -0.533828789899) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -15.3631903305 Adjustment: 0.1 Particles above track: 62 Particles below track: 913\n",
        "Timestep 54 Robot position (X, Y, theta): (489.538370834, 104.983318555, -0.43134040017) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -26.7206059461 Adjustment: 0.1 Particles above track: 5 Particles below track: 994\n",
        "Timestep 55 Robot position (X, Y, theta): (499.054087079, 101.909047203, -0.312488041564) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -34.401993925 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 56 Robot position (X, Y, theta): (508.845334094, 99.8764424978, -0.204686761646) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -40.5933605332 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 57 Robot position (X, Y, theta): (518.780958562, 98.7435848602, -0.113529485841) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -46.1726419194 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 58 Robot position (X, Y, theta): (528.780936307, 98.7224874927, -0.00210973831859) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -50.8719259186 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 59 Robot position (X, Y, theta): (538.741645177, 99.6080826642, 0.0886756865367) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -54.946206772 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 60 Robot position (X, Y, theta): (548.570843235, 101.448426527, 0.185089378608) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -58.0358857468 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 61 Robot position (X, Y, theta): (558.207437865, 104.119764009, 0.270417458895) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -60.1795442935 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 62 Robot position (X, Y, theta): (567.57680605, 107.614751821, 0.357036095527) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -61.3887675172 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 63 Robot position (X, Y, theta): (576.540699767, 112.047427022, 0.459240603699) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -61.8217028498 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 64 Robot position (X, Y, theta): (585.054790523, 117.292449291, 0.552130361524) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -61.2416337007 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 65 Robot position (X, Y, theta): (593.101115873, 123.230180201, 0.635739982682) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -59.8736523241 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 66 Robot position (X, Y, theta): (600.453661634, 130.008090707, 0.744754125524) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -57.6334811279 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 67 Robot position (X, Y, theta): (607.093132659, 137.485886138, 0.844711422444) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -54.63373336 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 68 Robot position (X, Y, theta): (613.045789881, 145.521175305, 0.933200024589) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -50.8293468766 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 69 Robot position (X, Y, theta): (618.063664272, 154.171084934, 1.04513236169) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -46.3920074569 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 70 Robot position (X, Y, theta): (622.234360334, 163.259830412, 1.14057760159) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -41.1710813462 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 71 Robot position (X, Y, theta): (625.415368184, 172.740399438, 1.24707078566) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -34.9909662753 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 72 Robot position (X, Y, theta): (627.546931674, 182.510580454, 1.35599190491) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -28.4011567511 Adjustment: 0.1 Particles above track: 0 Particles below track: 1000\n",
        "Timestep 73 Robot position (X, Y, theta): (628.612197078, 192.453679047, 1.46406727508) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -21.4206200763 Adjustment: 0.1 Particles above track: 2 Particles below track: 998\n",
        "Timestep 74 Robot position (X, Y, theta): (628.762155374, 202.452554609, 1.5557999351) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -14.2912933014 Adjustment: 0.1 Particles above track: 5 Particles below track: 989\n",
        "Timestep 75 Robot position (X, Y, theta): (627.775215538, 212.403732919, 1.66965123824) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = -6.87575483513 Adjustment: 0.1 Particles above track: 46 Particles below track: 888\n",
        "Timestep 76 Robot position (X, Y, theta): (625.875280375, 222.22158637, 1.76195186935) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 0.686118038031 Adjustment: 0 Particles above track: 340 Particles below track: 300\n",
        "Timestep 77 Robot position (X, Y, theta): (623.833488336, 232.010921652, 1.7764215011) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 10.0738234385 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 78 Robot position (X, Y, theta): (622.718099789, 241.948522388, 1.68256776021) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 19.2220309896 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 79 Robot position (X, Y, theta): (622.674677074, 251.948428111, 1.57513861194) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 28.0033978161 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 80 Robot position (X, Y, theta): (623.637661895, 261.901953127, 1.47434838451) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 36.0707007531 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 81 Robot position (X, Y, theta): (625.67084133, 271.693080814, 1.36605086633) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 43.4198268348 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 82 Robot position (X, Y, theta): (628.473220776, 281.292386489, 1.28675434972) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 50.4802270676 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 83 Robot position (X, Y, theta): (632.231240317, 290.559386496, 1.185534317) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 56.4925597041 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 84 Robot position (X, Y, theta): (637.010660393, 299.343300415, 1.07248602227) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 61.8225143251 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 85 Robot position (X, Y, theta): (642.616693002, 307.624145685, 0.975682204741) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 66.1466319534 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 86 Robot position (X, Y, theta): (649.022773901, 315.302825749, 0.875506402731) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 69.6347815364 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 87 Robot position (X, Y, theta): (656.063229141, 322.404374172, 0.789718117302) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 72.3800608855 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 88 Robot position (X, Y, theta): (663.765572448, 328.781984036, 0.691587829358) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 73.688807555 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 89 Robot position (X, Y, theta): (671.972789994, 334.495265068, 0.608123159262) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 73.8172156061 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 90 Robot position (X, Y, theta): (680.804579342, 339.185627199, 0.488199191133) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 73.5034329965 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 91 Robot position (X, Y, theta): (690.052169865, 342.991158636, 0.390394371911) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 71.9720441293 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 92 Robot position (X, Y, theta): (699.68870952, 345.662694426, 0.270438037622) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 69.1034539012 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 93 Robot position (X, Y, theta): (709.566083962, 347.22393564, 0.156765428339) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 65.7880615365 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 94 Robot position (X, Y, theta): (719.549015977, 347.807946247, 0.0584343097443) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 61.1272996322 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 95 Robot position (X, Y, theta): (729.532745281, 347.237727714, -0.0570527997138) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 55.6931533044 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 96 Robot position (X, Y, theta): (739.392431283, 345.568418892, -0.167716048711) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 49.9981120311 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 97 Robot position (X, Y, theta): (749.051832011, 342.980760187, -0.261744337754) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 43.5622270943 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 98 Robot position (X, Y, theta): (758.407941599, 339.450432529, -0.360810611175) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 36.189785903 Adjustment: -0.1 Particles above track: 1000 Particles below track: 0\n",
        "Timestep 99 Robot position (X, Y, theta): (767.448269507, 335.175801397, -0.441684720106) "
       ]
      },
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "avg y = 27.7800702805 Adjustment: -0.1 Particles above track: 993 Particles below track: 7\n"
       ]
      }
     ],
     "prompt_number": 54
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "math.cos(-1.0)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "pyout",
       "prompt_number": 43,
       "text": [
        "0.5403023058681398"
       ]
      }
     ],
     "prompt_number": 43
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "math.cos(1.0)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "pyout",
       "prompt_number": 44,
       "text": [
        "0.5403023058681398"
       ]
      }
     ],
     "prompt_number": 44
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "math.cos(6.2)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "pyout",
       "prompt_number": 45,
       "text": [
        "0.9965420970232175"
       ]
      }
     ],
     "prompt_number": 45
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "math.cos(0.2)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "pyout",
       "prompt_number": 46,
       "text": [
        "0.9800665778412416"
       ]
      }
     ],
     "prompt_number": 46
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "math.degrees(-0.345)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "pyout",
       "prompt_number": 47,
       "text": [
        "-19.7670439320134"
       ]
      }
     ],
     "prompt_number": 47
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "math.degrees(0.345)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "pyout",
       "prompt_number": 48,
       "text": [
        "19.7670439320134"
       ]
      }
     ],
     "prompt_number": 48
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [],
     "language": "python",
     "metadata": {},
     "outputs": []
    }
   ],
   "metadata": {}
  }
 ]
}