{
"metadata": {
"name": "",
"signature": "sha256:c068d21433e0c270ee7ce58fb1aac2516124aedcba11654d0c4fd433a2a058b7"
},
"nbformat": 3,
"nbformat_minor": 0,
"worksheets": [
{
"cells": [
{
"cell_type": "heading",
"level": 1,
"metadata": {},
"source": [
"Particle Filters Part 2 (aka Sense and Move Revisited)"
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Problem 1: Move"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Write a function in the class robot called move()\n",
"\n",
"that takes self and a motion vector (this\n",
"motion vector contains a steering* angle and a\n",
"distance) as input and returns an instance of the class\n",
"robot with the appropriate x, y, and orientation\n",
"for the given motion.\n",
"\n",
"*steering is defined in the video\n",
"which accompanies this problem.\n",
"\n",
"For now, please do NOT add noise to your move function.\n",
"\n",
"Please do not modify anything except where indicated\n",
"below.\n",
"\n",
"There are test cases which you are free to use at the\n",
"bottom. "
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from math import *\n",
"import random\n",
"# --------\n",
"# \n",
"# the \"world\" has 4 landmarks.\n",
"# the robot's initial coordinates are somewhere in the square\n",
"# represented by the landmarks.\n",
"#\n",
"# NOTE: Landmark coordinates are given in (y, x) form and NOT\n",
"# in the traditional (x, y) format!\n",
"\n",
"landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks\n",
"world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel \"out of bounds\"\n",
"max_steering_angle = pi/4 # You don't need to use this value, but it is good to keep in mind the limitations of a real car.\n",
"\n",
"# ------------------------------------------------\n",
"# \n",
"# this is the robot class\n",
"#\n",
"\n",
"class robot:\n",
"\n",
" # --------\n",
"\n",
" # init: \n",
" #\tcreates robot and initializes location/orientation \n",
" #\n",
"\n",
" def __init__(self, length = 10.0):\n",
" self.x = random.random() * world_size # initial x position\n",
" self.y = random.random() * world_size # initial y position\n",
" self.orientation = random.random() * 2.0 * pi # initial orientation\n",
" self.length = length # length of robot\n",
" self.bearing_noise = 0.0 # initialize bearing noise to zero\n",
" self.steering_noise = 0.0 # initialize steering noise to zero\n",
" self.distance_noise = 0.0 # initialize distance noise to zero\n",
" \n",
" def __repr__(self):\n",
" return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))\n",
" # --------\n",
" # set: \n",
" #\tsets a robot coordinate\n",
" #\n",
"\n",
" def set(self, new_x, new_y, new_orientation):\n",
"\n",
" if new_orientation < 0 or new_orientation >= 2 * pi:\n",
" raise ValueError, 'Orientation must be in [0..2pi]'\n",
" self.x = float(new_x)\n",
" self.y = float(new_y)\n",
" self.orientation = float(new_orientation)\n",
"\n",
"\n",
" # --------\n",
" # set_noise: \n",
" #\tsets the noise parameters\n",
" #\n",
"\n",
" def set_noise(self, new_b_noise, new_s_noise, new_d_noise):\n",
" # makes it possible to change the noise parameters\n",
" # this is often useful in particle filters\n",
" self.bearing_noise = float(new_b_noise)\n",
" self.steering_noise = float(new_s_noise)\n",
" self.distance_noise = float(new_d_noise)\n",
" \n",
" ############# ONLY ADD/MODIFY CODE BELOW HERE ###################\n",
"\n",
" # --------\n",
" # move:\n",
" # move along a section of a circular path according to motion\n",
" #\n",
" \n",
" def move(self, motion): # Do not change the name of this function\n",
"\n",
" # ADD CODE HERE\n",
" \n",
" return result # make sure your move function returns an instance\n",
" # of the robot class with the correct coordinates.\n",
" \n",
" ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################\n",
" \n",
"\n",
"## IMPORTANT: You may uncomment the test cases below to test your code.\n",
"## But when you submit this code, your test cases MUST be commented\n",
"## out. Our testing program provides its own code for testing your\n",
"## move function with randomized motion data.\n",
"\n",
"## --------\n",
"## TEST CASE:\n",
"## \n",
"## 1) The following code should print:\n",
"## Robot: [x=0.0 y=0.0 orient=0.0]\n",
"## Robot: [x=10.0 y=0.0 orient=0.0]\n",
"## Robot: [x=19.861 y=1.4333 orient=0.2886]\n",
"## Robot: [x=39.034 y=7.1270 orient=0.2886]\n",
"##\n",
"##\n",
"##length = 20.\n",
"##bearing_noise = 0.0\n",
"##steering_noise = 0.0\n",
"##distance_noise = 0.0\n",
"##\n",
"##myrobot = robot(length)\n",
"##myrobot.set(0.0, 0.0, 0.0)\n",
"##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n",
"##\n",
"##motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]\n",
"##\n",
"##T = len(motions)\n",
"##\n",
"##print 'Robot: ', myrobot\n",
"##for t in range(T):\n",
"## myrobot = myrobot.move(motions[t])\n",
"## print 'Robot: ', myrobot\n",
"##\n",
"##\n",
"\n",
"## IMPORTANT: You may uncomment the test cases below to test your code.\n",
"## But when you submit this code, your test cases MUST be commented\n",
"## out. Our testing program provides its own code for testing your\n",
"## move function with randomized motion data.\n",
"\n",
" \n",
"## 2) The following code should print:\n",
"## Robot: [x=0.0 y=0.0 orient=0.0]\n",
"## Robot: [x=9.9828 y=0.5063 orient=0.1013]\n",
"## Robot: [x=19.863 y=2.0201 orient=0.2027]\n",
"## Robot: [x=29.539 y=4.5259 orient=0.3040]\n",
"## Robot: [x=38.913 y=7.9979 orient=0.4054]\n",
"## Robot: [x=47.887 y=12.400 orient=0.5067]\n",
"## Robot: [x=56.369 y=17.688 orient=0.6081]\n",
"## Robot: [x=64.273 y=23.807 orient=0.7094]\n",
"## Robot: [x=71.517 y=30.695 orient=0.8108]\n",
"## Robot: [x=78.027 y=38.280 orient=0.9121]\n",
"## Robot: [x=83.736 y=46.485 orient=1.0135]\n",
"##\n",
"##\n",
"##length = 20.\n",
"##bearing_noise = 0.0\n",
"##steering_noise = 0.0\n",
"##distance_noise = 0.0\n",
"##\n",
"##myrobot = robot(length)\n",
"##myrobot.set(0.0, 0.0, 0.0)\n",
"##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n",
"##\n",
"##motions = [[0.2, 10.] for row in range(10)]\n",
"##\n",
"##T = len(motions)\n",
"##\n",
"##print 'Robot: ', myrobot\n",
"##for t in range(T):\n",
"## myrobot = myrobot.move(motions[t])\n",
"## print 'Robot: ', myrobot\n",
"\n",
"## IMPORTANT: You may uncomment the test cases below to test your code.\n",
"## But when you submit this code, your test cases MUST be commented\n",
"## out. Our testing program provides its own code for testing your\n",
"## move function with randomized motion data.\n"
],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Problem 2: Sense"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Write a function in the class robot called sense()\n",
"that takes self as input\n",
"and returns a list, Z, of the four bearings* to the 4\n",
"different landmarks. you will have to use the robot's\n",
"x and y position, as well as its orientation, to\n",
"compute this.\n",
"\n",
"*bearing is defined in the video\n",
"which accompanies this problem.\n",
"\n",
"For now, please do NOT add noise to your sense function.\n",
"\n",
"Please do not modify anything except where indicated\n",
"below.\n",
"\n",
"There are test cases provided at the bottom which you are\n",
"free to use."
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"from math import *\n",
"import random\n",
"\n",
"# --------\n",
"# \n",
"# the \"world\" has 4 landmarks.\n",
"# the robot's initial coordinates are somewhere in the square\n",
"# represented by the landmarks.\n",
"#\n",
"# NOTE: Landmark coordinates are given in (y, x) form and NOT\n",
"# in the traditional (x, y) format!\n",
"\n",
"landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) form.\n",
"world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel \"out of bounds\"\n",
"\n",
"# ------------------------------------------------\n",
"# \n",
"# this is the robot class\n",
"#\n",
"\n",
"class robot:\n",
"\n",
" # --------\n",
" # init: \n",
" #\tcreates robot and initializes location/orientation\n",
" #\n",
"\n",
" def __init__(self, length = 10.0):\n",
" self.x = random.random() * world_size # initial x position\n",
" self.y = random.random() * world_size # initial y position\n",
" self.orientation = random.random() * 2.0 * pi # initial orientation\n",
" self.length = length # length of robot\n",
" self.bearing_noise = 0.0 # initialize bearing noise to zero\n",
" self.steering_noise = 0.0 # initialize steering noise to zero\n",
" self.distance_noise = 0.0 # initialize distance noise to zero\n",
"\n",
"\n",
"\n",
" def __repr__(self):\n",
" return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), \n",
" str(self.orientation))\n",
"\n",
"\n",
" # --------\n",
" # set: \n",
" #\tsets a robot coordinate\n",
" #\n",
"\n",
" def set(self, new_x, new_y, new_orientation):\n",
" if new_orientation < 0 or new_orientation >= 2 * pi:\n",
" raise ValueError, 'Orientation must be in [0..2pi]'\n",
" self.x = float(new_x)\n",
" self.y = float(new_y)\n",
" self.orientation = float(new_orientation)\n",
"\n",
" # --------\n",
" # set_noise: \n",
" #\tsets the noise parameters\n",
" #\n",
"\n",
" def set_noise(self, new_b_noise, new_s_noise, new_d_noise):\n",
" # makes it possible to change the noise parameters\n",
" # this is often useful in particle filters\n",
" self.bearing_noise = float(new_b_noise)\n",
" self.steering_noise = float(new_s_noise)\n",
" self.distance_noise = float(new_d_noise)\n",
"\n",
" ############# ONLY ADD/MODIFY CODE BELOW HERE ###################\n",
"\n",
" # --------\n",
" # sense:\n",
" # obtains bearings from positions\n",
" #\n",
" \n",
" def sense(self): #do not change the name of this function\n",
" Z = []\n",
"\n",
" # ENTER CODE HERE\n",
" # HINT: You will probably need to use the function atan2()\n",
"\n",
" return Z #Leave this line here. Return vector Z of 4 bearings.\n",
" \n",
" ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################\n",
"\n",
"\n",
"## IMPORTANT: You may uncomment the test cases below to test your code.\n",
"## But when you submit this code, your test cases MUST be commented\n",
"## out. Our testing program provides its own code for testing your\n",
"## sense function with randomized initial robot coordinates.\n",
" \n",
"## --------\n",
"## TEST CASES:\n",
"\n",
"\n",
"\n",
"##\n",
"## 1) The following code should print the list [6.004885648174475, 3.7295952571373605, 1.9295669970654687, 0.8519663271732721]\n",
"##\n",
"##\n",
"##length = 20.\n",
"##bearing_noise = 0.0\n",
"##steering_noise = 0.0\n",
"##distance_noise = 0.0\n",
"##\n",
"##myrobot = robot(length)\n",
"##myrobot.set(30.0, 20.0, 0.0)\n",
"##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n",
"##\n",
"##print 'Robot: ', myrobot\n",
"##print 'Measurements: ', myrobot.sense()\n",
"##\n",
"\n",
"## IMPORTANT: You may uncomment the test cases below to test your code.\n",
"## But when you submit this code, your test cases MUST be commented\n",
"## out. Our testing program provides its own code for testing your\n",
"## sense function with randomized initial robot coordinates.\n",
" \n",
"\n",
"##\n",
"## 2) The following code should print the list [5.376567117456516, 3.101276726419402, 1.3012484663475101, 0.22364779645531352]\n",
"##\n",
"##\n",
"##length = 20.\n",
"##bearing_noise = 0.0\n",
"##steering_noise = 0.0\n",
"##distance_noise = 0.0\n",
"##\n",
"##myrobot = robot(length)\n",
"##myrobot.set(30.0, 20.0, pi / 5.0)\n",
"##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n",
"##\n",
"##print 'Robot: ', myrobot\n",
"##print 'Measurements: ', myrobot.sense()\n",
"##\n"
],
"language": "python",
"metadata": {},
"outputs": []
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Problem 3: Putting it together"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Now you will put everything together.\n",
"\n",
"First make sure that your sense and move functions\n",
"work as expected for the test cases provided at the\n",
"bottom of the previous two programming assignments.\n",
"Once you are satisfied, copy your sense and move\n",
"definitions into the robot class on this page, BUT\n",
"now include noise.\n",
"\n",
"A good way to include noise in the sense step is to\n",
"add Gaussian noise, centered at zero with variance\n",
"of self.bearing_noise to each bearing. You can do this\n",
"with the command random.gauss(0, self.bearing_noise)\n",
"\n",
"In the move step, you should make sure that your\n",
"actual steering angle is chosen from a Gaussian\n",
"distribution of steering angles. This distribution\n",
"should be centered at the intended steering angle\n",
"with variance of self.steering_noise.\n",
"\n",
"Feel free to use the included set_noise function.\n"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [
"# Please do not modify anything except where indicated\n",
"# below.\n",
"\n",
"from math import *\n",
"import random\n",
"\n",
"# --------\n",
"# \n",
"# some top level parameters\n",
"#\n",
"\n",
"max_steering_angle = pi / 4.0 # You do not need to use this value, but keep in mind the limitations of a real car.\n",
"bearing_noise = 0.1 # Noise parameter: should be included in sense function.\n",
"steering_noise = 0.1 # Noise parameter: should be included in move function.\n",
"distance_noise = 5.0 # Noise parameter: should be included in move function.\n",
"\n",
"tolerance_xy = 15.0 # Tolerance for localization in the x and y directions.\n",
"tolerance_orientation = 0.25 # Tolerance for orientation.\n",
"\n",
"\n",
"# --------\n",
"# \n",
"# the \"world\" has 4 landmarks.\n",
"# the robot's initial coordinates are somewhere in the square\n",
"# represented by the landmarks.\n",
"#\n",
"# NOTE: Landmark coordinates are given in (y, x) form and NOT\n",
"# in the traditional (x, y) format!\n",
"\n",
"landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks in (y, x) format.\n",
"world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel \"out of bounds\"\n",
"\n",
"# ------------------------------------------------\n",
"# \n",
"# this is the robot class\n",
"#\n",
"\n",
"class robot:\n",
"\n",
" # --------\n",
" # init: \n",
" # creates robot and initializes location/orientation \n",
" #\n",
"\n",
" def __init__(self, length = 20.0):\n",
" self.x = random.random() * world_size # initial x position\n",
" self.y = random.random() * world_size # initial y position\n",
" self.orientation = random.random() * 2.0 * pi # initial orientation\n",
" self.length = length # length of robot\n",
" self.bearing_noise = 0.0 # initialize bearing noise to zero\n",
" self.steering_noise = 0.0 # initialize steering noise to zero\n",
" self.distance_noise = 0.0 # initialize distance noise to zero\n",
"\n",
" # --------\n",
" # set: \n",
" # sets a robot coordinate\n",
" #\n",
"\n",
" def set(self, new_x, new_y, new_orientation):\n",
"\n",
" if new_orientation < 0 or new_orientation >= 2 * pi:\n",
" raise ValueError, 'Orientation must be in [0..2pi]'\n",
" self.x = float(new_x)\n",
" self.y = float(new_y)\n",
" self.orientation = float(new_orientation)\n",
"\n",
" # --------\n",
" # set_noise: \n",
" # sets the noise parameters\n",
" #\n",
" def set_noise(self, new_b_noise, new_s_noise, new_d_noise):\n",
" # makes it possible to change the noise parameters\n",
" # this is often useful in particle filters\n",
" self.bearing_noise = float(new_b_noise)\n",
" self.steering_noise = float(new_s_noise)\n",
" self.distance_noise = float(new_d_noise)\n",
"\n",
" # --------\n",
" # measurement_prob\n",
" # computes the probability of a measurement\n",
" # \n",
"\n",
" def measurement_prob(self, measurements):\n",
"\n",
" # calculate the correct measurement\n",
" predicted_measurements = self.sense(0) # Our sense function took 0 as an argument to switch off noise.\n",
"\n",
"\n",
" # compute errors\n",
" error = 1.0\n",
" for i in range(len(measurements)):\n",
" error_bearing = abs(measurements[i] - predicted_measurements[i])\n",
" error_bearing = (error_bearing + pi) % (2.0 * pi) - pi # truncate\n",
" \n",
"\n",
" # update Gaussian\n",
" error *= (exp(- (error_bearing ** 2) / (self.bearing_noise ** 2) / 2.0) / \n",
" sqrt(2.0 * pi * (self.bearing_noise ** 2)))\n",
"\n",
" return error\n",
" \n",
" def __repr__(self): #allows us to print robot attributes.\n",
" return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), \n",
" str(self.orientation))\n",
" \n",
" ############# ONLY ADD/MODIFY CODE BELOW HERE ###################\n",
" \n",
" # --------\n",
" # move: \n",
" # \n",
" \n",
" # copy your code from the previous exercise\n",
" # and modify it so that it simulates motion noise\n",
" # according to the noise parameters\n",
" # self.steering_noise\n",
" # self.distance_noise\n",
"\n",
" # --------\n",
" # sense: \n",
" # \n",
"\n",
" # copy your code from the previous exercise\n",
" # and modify it so that it simulates bearing noise\n",
" # according to\n",
" # self.bearing_noise\n",
"\n",
" ############## ONLY ADD/MODIFY CODE ABOVE HERE ####################\n",
"\n",
"# --------\n",
"#\n",
"# extract position from a particle set\n",
"# \n",
"\n",
"def get_position(p):\n",
" x = 0.0\n",
" y = 0.0\n",
" orientation = 0.0\n",
" for i in range(len(p)):\n",
" x += p[i].x\n",
" y += p[i].y\n",
" # orientation is tricky because it is cyclic. By normalizing\n",
" # around the first particle we are somewhat more robust to\n",
" # the 0=2pi problem\n",
" orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi)) \n",
" + p[0].orientation - pi)\n",
" return [x / len(p), y / len(p), orientation / len(p)]\n",
"\n",
"# --------\n",
"#\n",
"# The following code generates the measurements vector\n",
"# You can use it to develop your solution.\n",
"# \n",
"\n",
"\n",
"def generate_ground_truth(motions):\n",
"\n",
" myrobot = robot()\n",
" myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n",
"\n",
" Z = []\n",
" T = len(motions)\n",
"\n",
" for t in range(T):\n",
" myrobot = myrobot.move(motions[t])\n",
" Z.append(myrobot.sense())\n",
" #print 'Robot: ', myrobot\n",
" return [myrobot, Z]\n",
"\n",
"# --------\n",
"#\n",
"# The following code prints the measurements associated\n",
"# with generate_ground_truth\n",
"#\n",
"\n",
"def print_measurements(Z):\n",
"\n",
" T = len(Z)\n",
"\n",
" print 'measurements = [[%.8s, %.8s, %.8s, %.8s],' % \\\n",
" (str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3]))\n",
" for t in range(1,T-1):\n",
" print ' [%.8s, %.8s, %.8s, %.8s],' % \\\n",
" (str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3]))\n",
" print ' [%.8s, %.8s, %.8s, %.8s]]' % \\\n",
" (str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3]))\n",
"\n",
"# --------\n",
"#\n",
"# The following code checks to see if your particle filter\n",
"# localizes the robot to within the desired tolerances\n",
"# of the true position. The tolerances are defined at the top.\n",
"#\n",
"\n",
"def check_output(final_robot, estimated_position):\n",
"\n",
" error_x = abs(final_robot.x - estimated_position[0])\n",
" error_y = abs(final_robot.y - estimated_position[1])\n",
" error_orientation = abs(final_robot.orientation - estimated_position[2])\n",
" error_orientation = (error_orientation + pi) % (2.0 * pi) - pi\n",
" correct = error_x < tolerance_xy and error_y < tolerance_xy \\\n",
" and error_orientation < tolerance_orientation\n",
" return correct\n",
"\n",
"\n",
"\n",
"def particle_filter(motions, measurements, N=500): # I know it's tempting, but don't change N!\n",
" # --------\n",
" #\n",
" # Make particles\n",
" # \n",
"\n",
" p = []\n",
" for i in range(N):\n",
" r = robot()\n",
" r.set_noise(bearing_noise, steering_noise, distance_noise)\n",
" p.append(r)\n",
"\n",
" # --------\n",
" #\n",
" # Update particles\n",
" # \n",
"\n",
" for t in range(len(motions)):\n",
" \n",
" # motion update (prediction)\n",
" p2 = []\n",
" for i in range(N):\n",
" p2.append(p[i].move(motions[t]))\n",
" p = p2\n",
"\n",
" # measurement update\n",
" w = []\n",
" for i in range(N):\n",
" w.append(p[i].measurement_prob(measurements[t]))\n",
"\n",
" # resampling\n",
" p3 = []\n",
" index = int(random.random() * N)\n",
" beta = 0.0\n",
" mw = max(w)\n",
" for i in range(N):\n",
" beta += random.random() * 2.0 * mw\n",
" while beta > w[index]:\n",
" beta -= w[index]\n",
" index = (index + 1) % N\n",
" p3.append(p[index])\n",
" p = p3\n",
" \n",
" return get_position(p)\n",
"\n",
"## IMPORTANT: You may uncomment the test cases below to test your code.\n",
"## But when you submit this code, your test cases MUST be commented\n",
"## out.\n",
"##\n",
"## You can test whether your particle filter works using the\n",
"## function check_output (see test case 2). We will be using a similar\n",
"## function. Note: Even for a well-implemented particle filter this\n",
"## function occasionally returns False. This is because a particle\n",
"## filter is a randomized algorithm. We will be testing your code\n",
"## multiple times. Make sure check_output returns True at least 80%\n",
"## of the time.\n",
"\n",
"\n",
" \n",
"## --------\n",
"## TEST CASES:\n",
"## \n",
"##1) Calling the particle_filter function with the following\n",
"## motions and measurements should return a [x,y,orientation]\n",
"## vector near [x=93.476 y=75.186 orient=5.2664], that is, the\n",
"## robot's true location.\n",
"##\n",
"##motions = [[2. * pi / 10, 20.] for row in range(8)]\n",
"##measurements = [[4.746936, 3.859782, 3.045217, 2.045506],\n",
"## [3.510067, 2.916300, 2.146394, 1.598332],\n",
"## [2.972469, 2.407489, 1.588474, 1.611094],\n",
"## [1.906178, 1.193329, 0.619356, 0.807930],\n",
"## [1.352825, 0.662233, 0.144927, 0.799090],\n",
"## [0.856150, 0.214590, 5.651497, 1.062401],\n",
"## [0.194460, 5.660382, 4.761072, 2.471682],\n",
"## [5.717342, 4.736780, 3.909599, 2.342536]]\n",
"##\n",
"##print particle_filter(motions, measurements)\n",
"\n",
"## 2) You can generate your own test cases by generating\n",
"## measurements using the generate_ground_truth function.\n",
"## It will print the robot's last location when calling it.\n",
"##\n",
"##\n",
"##number_of_iterations = 6\n",
"##motions = [[2. * pi / 20, 12.] for row in range(number_of_iterations)]\n",
"##\n",
"##x = generate_ground_truth(motions)\n",
"##final_robot = x[0]\n",
"##measurements = x[1]\n",
"##estimated_position = particle_filter(motions, measurements)\n",
"##print_measurements(measurements)\n",
"##print 'Ground truth: ', final_robot\n",
"##print 'Particle filter: ', estimated_position\n",
"##print 'Code check: ', check_output(final_robot, estimated_position)\n"
],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}
}
]
}