Skip to content

Instantly share code, notes, and snippets.

@kmmbvnr

kmmbvnr/README

Created March 20, 2012 04:16
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save kmmbvnr/2131261 to your computer and use it in GitHub Desktop.
Save kmmbvnr/2131261 to your computer and use it in GitHub Desktop.
Download ZIP
Udacity cs373 code
Raw
01_matrix_fill_in.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Modify the previous code to adjust for a highly
# confident last measurement. Do this by adding a
# factor of 5 into your Omega and Xi matrices
# as described in the video.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy = 0):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ######################################################################
# ######################################################################
# Including the 5 times multiplier, your returned mu should now be:
#
# [[-3.0],
# [2.179],
# [5.714],
# [6.821]]
############## MODIFY CODE BELOW ##################
def doit(initial_pos, move1, move2, Z0, Z1, Z2):
Omega = matrix([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]])
Xi = matrix([[initial_pos],
[0.0],
[0.0]])
Omega += matrix([[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[0.0, 0.0, 0.0]])
Xi += matrix([[-move1],
[move1],
[0.0]])
Omega += matrix([[0.0, 0.0, 0.0],
[0.0, 1.0, -1.0],
[0.0, -1.0, 1.0]])
Xi += matrix([[0.0],
[-move2],
[move2]])
Omega = Omega.expand(5, 5, [0, 1, 2], [0, 1, 2])
Xi = Xi.expand(5, 1, [0, 1, 2], [0])
Omega += matrix([[1.0 / 0.5, 0.0, 0.0, -1.0 / 0.5, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[-1.0 / 0.5, 0.0, 0.0, 1.0 / 0.5, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0]])
Xi += matrix([[-Z0 / 0.5],
[0.0],
[0.0],
[Z0 / 0.5 ],
[0.0]])
Omega += matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1.0 / 0.5, 0.0, 0.0, -1.0 / 0.5],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, -1.0 / 0.5, 0.0, 0.0, 1.0 / 0.5]])
Xi += matrix([[0.0],
[-Z1 / 0.5],
[0.0],
[0.0],
[Z1 / 0.5]])
Omega += matrix([[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0 / 0.5, 0.0, -1.0 / 0.5],
[0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0 / 0.5, 0.0, 1.0 / 0.5]])
Xi += matrix([[0.0],
[0.0],
[-Z2 / 0.5],
[0.0],
[Z2 / 0.5]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Mu: ')
return mu
doit(5, 7, 2, 2, 4, 2)
Raw
02_cyclic_smoothing.py
#!/usr/bin/env python
# -------------
# User Instructions
#
# Here you will be implementing a cyclic smoothing
# algorithm. This algorithm should not fix the end
# points (as you did in the unit quizzes). You
# should use the gradient descent equations that
# you used previously.
#
# Your function should return the newpath that it
# calculates..
#
# Feel free to use the provided solution_check function
# to test your code. You can find it at the bottom.
#
# --------------
# Testing Instructions
#
# To test your code, call the solution_check function with
# two arguments. The first argument should be the result of your
# smooth function. The second should be the corresponding answer.
# For example, calling
#
# solution_check(smooth(testpath1), answer1)
#
# should return True if your answer is correct and False if
# it is not.
from math import *
# Do not modify path inside your function.
path=[[0, 0],
[1, 0],
[2, 0],
[3, 0],
[4, 0],
[5, 0],
[6, 0],
[6, 1],
[6, 2],
[6, 3],
[5, 3],
[4, 3],
[3, 3],
[2, 3],
[1, 3],
[0, 3],
[0, 2],
[0, 1]]
############# ONLY ENTER CODE BELOW THIS LINE ##########
# ------------------------------------------------
# smooth coordinates
# If your code is timing out, make the tolerance parameter
# larger to decrease run time.
#
def smooth(path, weight_data = 0.1, weight_smooth = 0.1, tolerance = 0.00001):
#
# Enter code here
#
# Make a deep copy of path into newpath
newpath = [[0 for row in range(len(path[0]))] for col in range(len(path))]
for i in range(len(path)):
for j in range(len(path[0])):
newpath[i][j] = path[i][j]
change = tolerance
while change >= tolerance:
change = 0.0
for i in range(0, len(path)):
for j in range(0, len(path[0])):
aux = newpath[i][j]
newpath[i][j] += weight_data * (path[i][j]-newpath[i][j])
newpath[i][j] += weight_smooth * (newpath[(i-1) % len(path)][j] \
+ newpath[(i+1) % len(path)][j] - 2.*newpath[i][j])
change += abs(aux - newpath[i][j])
return newpath
# thank you - EnTerr - for posting this on our discussion forum
#newpath = smooth(path)
#for i in range(len(path)):
# print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']'
##### TESTING ######
# --------------------------------------------------
# check if two numbers are 'close enough,'used in
# solution_check function.
#
def close_enough(user_answer, true_answer, epsilon = 0.001):
if abs(user_answer - true_answer) > epsilon:
return False
return True
# --------------------------------------------------
# check your solution against our reference solution for
# a variety of test cases (given below)
#
def solution_check(newpath, answer):
if type(newpath) != type(answer):
print "Error. You do not return a list."
return False
if len(newpath) != len(answer):
print 'Error. Your newpath is not the correct length.'
return False
if len(newpath[0]) != len(answer[0]):
print 'Error. Your entries do not contain an (x, y) coordinate pair.'
return False
for i in range(len(newpath)):
for j in range(len(newpath[0])):
if not close_enough(newpath[i][j], answer[i][j]):
print 'Error, at least one of your entries is not correct.'
return False
print "Test case correct!"
return True
# --------------
# Testing Instructions
#
# To test your code, call the solution_check function with
# two arguments. The first argument should be the result of your
# smooth function. The second should be the corresponding answer.
# For example, calling
#
# solution_check(smooth(testpath1), answer1)
#
# should return True if your answer is correct and False if
# it is not.
testpath1 = [[0, 0],
[1, 0],
[2, 0],
[3, 0],
[4, 0],
[5, 0],
[6, 0],
[6, 1],
[6, 2],
[6, 3],
[5, 3],
[4, 3],
[3, 3],
[2, 3],
[1, 3],
[0, 3],
[0, 2],
[0, 1]]
answer1 = [[0.5449300156668018, 0.47485226780102946],
[1.2230705677535505, 0.2046277687200752],
[2.079668890615267, 0.09810778721159963],
[3.0000020176660755, 0.07007646364781912],
[3.9203348821839112, 0.09810853832382399],
[4.7769324511170455, 0.20462917195702085],
[5.455071854686622, 0.4748541381544533],
[5.697264197153936, 1.1249625336275617],
[5.697263485026567, 1.8750401628534337],
[5.455069810373743, 2.5251482916876378],
[4.776929339068159, 2.795372759575895],
[3.92033110541304, 2.9018927284871063],
[2.999998066091118, 2.929924058932193],
[2.0796652780381826, 2.90189200881968],
[1.2230677654766597, 2.7953714133566603],
[0.544928391271399, 2.5251464933327794],
[0.3027360471605494, 1.875038145804603],
[0.302736726373967, 1.1249605602741133]]
## [0.000, 0.000] -> [0.545, 0.475]
## [1.000, 0.000] -> [1.223, 0.205]
## [2.000, 0.000] -> [2.080, 0.098]
## [3.000, 0.000] -> [3.000, 0.070]
## [4.000, 0.000] -> [3.920, 0.098]
## [5.000, 0.000] -> [4.777, 0.205]
## [6.000, 0.000] -> [5.455, 0.475]
## [6.000, 1.000] -> [5.697, 1.125]
## [6.000, 2.000] -> [5.697, 1.875]
## [6.000, 3.000] -> [5.455, 2.525]
## [5.000, 3.000] -> [4.777, 2.795]
## [4.000, 3.000] -> [3.920, 2.902]
## [3.000, 3.000] -> [3.000, 2.930]
## [2.000, 3.000] -> [2.080, 2.902]
## [1.000, 3.000] -> [1.223, 2.795]
## [0.000, 3.000] -> [0.545, 2.525]
## [0.000, 2.000] -> [0.303, 1.875]
## [0.000, 1.000] -> [0.303, 1.125]
testpath2 = [[1, 0], # Move in the shape of a plus sign
[2, 0],
[2, 1],
[3, 1],
[3, 2],
[2, 2],
[2, 3],
[1, 3],
[1, 2],
[0, 2],
[0, 1],
[1, 1]]
answer2 = [[1.239080543767428, 0.5047204351187283],
[1.7609243903912781, 0.5047216452560908],
[2.0915039821562416, 0.9085017167753027],
[2.495281862032503, 1.2390825203587184],
[2.4952805300504783, 1.7609262468826048],
[2.0915003641706296, 2.0915058211575475],
[1.7609195135622062, 2.4952837841027695],
[1.2390757942466555, 2.4952826072236918],
[0.9084962737918979, 2.091502621431358],
[0.5047183914625598, 1.7609219230352355],
[0.504719649257698, 1.2390782835562297],
[0.9084996902674257, 0.9084987462432871]]
## [1.000, 0.000] -> [1.239, 0.505]
## [2.000, 0.000] -> [1.761, 0.505]
## [2.000, 1.000] -> [2.092, 0.909]
## [3.000, 1.000] -> [2.495, 1.239]
## [3.000, 2.000] -> [2.495, 1.761]
## [2.000, 2.000] -> [2.092, 2.092]
## [2.000, 3.000] -> [1.761, 2.495]
## [1.000, 3.000] -> [1.239, 2.495]
## [1.000, 2.000] -> [0.908, 2.092]
## [0.000, 2.000] -> [0.505, 1.761]
## [0.000, 1.000] -> [0.505, 1.239]
## [1.000, 1.000] -> [0.908, 0.908]
solution_check(smooth(testpath1), answer1)
Raw
03_coconuts.py
#!/usr/bin/env python
#Write a function that takes the number of
#coconuts, n, as an argument and returns the
#number of coconuts after one is given to
#the monkey and one fifth are taken away.
def f(n):
return 4*(n-1)/5
print f(96.)
Raw
03_constrained_smoothing.py
#!/usr/bin/env python
# -------------
# User Instructions
#
# Now you will be incorporating fixed points into
# your smoother.
#
# You will need to use the equations from gradient
# descent AND the new equations presented in the
# previous lecture to implement smoothing with
# fixed points.
#
# Your function should return the newpath that it
# calculates.
#
# Feel free to use the provided solution_check function
# to test your code. You can find it at the bottom.
#
# --------------
# Testing Instructions
#
# To test your code, call the solution_check function with
# two arguments. The first argument should be the result of your
# smooth function. The second should be the corresponding answer.
# For example, calling
#
# solution_check(smooth(testpath1), answer1)
#
# should return True if your answer is correct and False if
# it is not.
from math import *
# Do not modify path inside your function.
path=[[0, 0], #fix
[1, 0],
[2, 0],
[3, 0],
[4, 0],
[5, 0],
[6, 0], #fix
[6, 1],
[6, 2],
[6, 3], #fix
[5, 3],
[4, 3],
[3, 3],
[2, 3],
[1, 3],
[0, 3], #fix
[0, 2],
[0, 1]]
# Do not modify fix inside your function
fix = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0]
######################## ENTER CODE BELOW HERE #########################
def smooth(path, fix, weight_data = 0.0, weight_smooth = 0.1, tolerance = 0.00001):
#
# Enter code here.
# The weight for each of the two new equations should be 0.5 * weight_smooth
#
newpath = [[0 for row in range(len(path[0]))] for col in range(len(path))]
for i in range(len(path)):
for j in range(len(path[0])):
newpath[i][j] = path[i][j]
change = tolerance
while change >= tolerance:
change = 0.0
for i in range(0, len(path)):
if fix[i]:
continue
for j in range(0, len(path[0])):
aux = newpath[i][j]
newpath[i][j] += weight_data * (path[i][j]-newpath[i][j])
newpath[i][j] += weight_smooth * (newpath[(i-1)%len(path)][j] \
+ newpath[(i+1)%len(path)][j] - 2.*newpath[i][j])
newpath[i][j] += 0.5 * weight_smooth * (2*newpath[(i-1)%len(path)][j] \
- newpath[(i-2)%len(path)][j] \
- newpath[i][j])
newpath[i][j] += 0.5 * weight_smooth * (2*newpath[(i+1)%len(path)][j] \
- newpath[(i+2)%len(path)][j] \
- newpath[i][j])
change += abs(aux - newpath[i][j])
return newpath
#thank you - EnTerr - for posting this on our discussion forum
## newpath = smooth(path)
## for i in range(len(path)):
## print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']'
# --------------------------------------------------
# check if two numbers are 'close enough,'used in
# solution_check function.
#
def close_enough(user_answer, true_answer, epsilon = 0.03):
if abs(user_answer - true_answer) > epsilon:
return False
return True
# --------------------------------------------------
# check your solution against our reference solution for
# a variety of test cases (given below)
#
def solution_check(newpath, answer):
if type(newpath) != type(answer):
print "Error. You do not return a list."
return False
if len(newpath) != len(answer):
print 'Error. Your newpath is not the correct length.'
return False
if len(newpath[0]) != len(answer[0]):
print 'Error. Your entries do not contain an (x, y) coordinate pair.'
return False
for i in range(len(newpath)):
for j in range(len(newpath[0])):
if not close_enough(newpath[i][j], answer[i][j]):
print 'Error, at least one of your entries is not correct.'
return False
print "Test case correct!"
return True
# --------------
# Testing Instructions
#
# To test your code, call the solution_check function with
# two arguments. The first argument should be the result of your
# smooth function. The second should be the corresponding answer.
# For example, calling
#
# solution_check(smooth(testpath1), answer1)
#
# should return True if your answer is correct and False if
# it is not.
testpath1=[[0, 0], #fix
[1, 0],
[2, 0],
[3, 0],
[4, 0],
[5, 0],
[6, 0], #fix
[6, 1],
[6, 2],
[6, 3], #fix
[5, 3],
[4, 3],
[3, 3],
[2, 3],
[1, 3],
[0, 3], #fix
[0, 2],
[0, 1]]
testfix1 = [1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0]
answer1 = [[0, 0],
[0.7938620981547201, -0.8311168821106101],
[1.8579052986461084, -1.3834788165869276],
[3.053905318597796, -1.5745863173084],
[4.23141390533387, -1.3784271816058231],
[5.250184859723701, -0.8264215958231558],
[6, 0],
[6.415150091996651, 0.9836951698796843],
[6.41942442687092, 2.019512290770163],
[6, 3],
[5.206131365604606, 3.831104483245191],
[4.142082497497067, 4.383455704596517],
[2.9460804122779813, 4.5745592975708105],
[1.768574219397359, 4.378404668718541],
[0.7498089205417316, 3.826409771585794],
[0, 3],
[-0.4151464728194156, 2.016311854977891],
[-0.4194207879552198, 0.9804948340550833]]
testpath2 = [[0, 0], # fix
[2, 0],
[4, 0], # fix
[4, 2],
[4, 4], # fix
[2, 4],
[0, 4], # fix
[0, 2]]
testfix2 = [1, 0, 1, 0, 1, 0, 1, 0]
answer2 = [[0, 0],
[2.0116767115496095, -0.7015439080661671],
[4, 0],
[4.701543905420104, 2.0116768147460418],
[4, 4],
[1.9883231877640861, 4.701543807525115],
[0, 4],
[-0.7015438099112995, 1.9883232808252207]]
solution_check(smooth(testpath2, testfix2), answer2)
Raw
03_online_slam.py
#!/usr/bin/env python
# ------------
# User Instructions
#
# In this problem you will implement a more manageable
# version of graph SLAM in 2 dimensions.
#
# Define a function, online_slam, that takes 5 inputs:
# data, N, num_landmarks, motion_noise, and
# measurement_noise--just as was done in the last
# programming assignment of unit 6. This function
# must return TWO matrices, mu and the final Omega.
#
# Just as with the quiz, your matrices should have x
# and y interlaced, so if there were two poses and 2
# landmarks, mu would look like:
#
# mu = matrix([[Px0],
# [Py0],
# [Px1],
# [Py1],
# [Lx0],
# [Ly0],
# [Lx1],
# [Ly1]])
#
# Enter your code at line 566.
# -----------
# Testing
#
# You have two methods for testing your code.
#
# 1) You can make your own data with the make_data
# function. Then you can run it through the
# provided slam routine and check to see that your
# online_slam function gives the same estimated
# final robot pose and landmark positions.
# 2) You can use the solution_check function at the
# bottom of this document to check your code
# for the two provided test cases. The grading
# will be almost identical to this function, so
# if you pass both test cases, you should be
# marked correct on the homework.
from math import sqrt, pi, sin, cos
import random
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# -----------
#
# defines matrix equality - returns true if corresponding elements
# in two matrices are within epsilon of each other.
#
def __eq__(self, other):
epsilon = 0.01
if self.dimx != other.dimx or self.dimy != other.dimy:
return False
for i in range(self.dimx):
for j in range(self.dimy):
if abs(self.value[i][j] - other.value[i][j]) > epsilon:
return False
return True
def __ne__(self, other):
return not (self == other)
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ------------------------------------------------
#
# this is the robot class
#
# our robot lives in x-y space, and its motion is
# pointed in a random direction. It moves on a straight line
# until is comes close to a wall at which point it turns
# away from the wall and continues to move.
#
# For measurements, it simply senses the x- and y-distance
# to landmarks. This is different from range and bearing as
# commonly studies in the literature, but this makes it much
# easier to implement the essentials of SLAM without
# cluttered math
#
class robot:
# --------
# init:
# creates robot and initializes location to 0, 0
#
def __init__(self, world_size = 100.0, measurement_range = 30.0,
motion_noise = 1.0, measurement_noise = 1.0):
self.measurement_noise = 0.0
self.world_size = world_size
self.measurement_range = measurement_range
self.x = world_size / 2.0
self.y = world_size / 2.0
self.motion_noise = motion_noise
self.measurement_noise = measurement_noise
self.landmarks = []
self.num_landmarks = 0
def rand(self):
return random.random() * 2.0 - 1.0
# --------
#
# make random landmarks located in the world
#
def make_landmarks(self, num_landmarks):
self.landmarks = []
for i in range(num_landmarks):
self.landmarks.append([round(random.random() * self.world_size),
round(random.random() * self.world_size)])
self.num_landmarks = num_landmarks
# --------
#
# move: attempts to move robot by dx, dy. If outside world
# boundary, then the move does nothing and instead returns failure
#
def move(self, dx, dy):
x = self.x + dx + self.rand() * self.motion_noise
y = self.y + dy + self.rand() * self.motion_noise
if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size:
return False
else:
self.x = x
self.y = y
return True
# --------
#
# sense: returns x- and y- distances to landmarks within visibility range
# because not all landmarks may be in this range, the list of measurements
# is of variable length. Set measurement_range to -1 if you want all
# landmarks to be visible at all times
#
def sense(self):
Z = []
for i in range(self.num_landmarks):
dx = self.landmarks[i][0] - self.x + self.rand() * self.measurement_noise
dy = self.landmarks[i][1] - self.y + self.rand() * self.measurement_noise
if self.measurement_range < 0.0 or abs(dx) + abs(dy) <= self.measurement_range:
Z.append([i, dx, dy])
return Z
# --------
#
# print robot location
#
def __repr__(self):
return 'Robot: [x=%.5f y=%.5f]' % (self.x, self.y)
# ######################################################################
# --------
# this routine makes the robot data
#
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
complete = False
while not complete:
data = []
# make robot and landmarks
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks(num_landmarks)
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N-1):
# sense
Z = r.sense()
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# memorize data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
print ' '
print 'Landmarks: ', r.landmarks
print r
return data
# ######################################################################
# --------------------------------
#
# full_slam - retains entire path and all landmarks
# Feel free to use this for comparison.
#
def slam(data, N, num_landmarks, motion_noise, measurement_noise):
# Set the dimension of the filter
dim = 2 * (N + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for k in range(len(data)):
# n is the index of the robot pose in the matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (N + measurement[i][0])
# update the information maxtrix/vector based on the measurement
for b in range(2):
Omega.value[n+b][n+b] += 1.0 / measurement_noise
Omega.value[m+b][m+b] += 1.0 / measurement_noise
Omega.value[n+b][m+b] += -1.0 / measurement_noise
Omega.value[m+b][n+b] += -1.0 / measurement_noise
Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise
Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise
# update the information maxtrix/vector based on the robot motion
for b in range(4):
Omega.value[n+b][n+b] += 1.0 / motion_noise
for b in range(2):
Omega.value[n+b ][n+b+2] += -1.0 / motion_noise
Omega.value[n+b+2][n+b ] += -1.0 / motion_noise
Xi.value[n+b ][0] += -motion[b] / motion_noise
Xi.value[n+b+2][0] += motion[b] / motion_noise
# compute best estimate
mu = Omega.inverse() * Xi
# return the result
return mu
# --------------------------------
#
# online_slam - retains all landmarks but only most recent robot pose
#
def online_slam(data, N, num_landmarks, motion_noise, measurement_noise):
#
#
# Enter your code here!
#
#
# Set the dimension of the filter
dim = 2 * (1 + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
# process the data
for k in range(len(data)):
measurement = data[k][0]
motion = data[k][1]
# n is the index of the robot pose in matrix/vector
n = 0
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (1 + measurement[i][0])
# update the information maxtrix/vector based on the measurement
for b in range(2):
Omega.value[n+b][n+b] += 1.0 / measurement_noise
Omega.value[m+b][m+b] += 1.0 / measurement_noise
Omega.value[n+b][m+b] += -1.0 / measurement_noise
Omega.value[m+b][n+b] += -1.0 / measurement_noise
Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise
Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise
expand_row = [0,1] + range(4, dim+2)
Omega = Omega.expand(dim+2, dim+2, expand_row, expand_row)
Xi = Xi.expand(dim+2, 1, expand_row, [0])
# update the information maxtrix/vector based on the robot motion
for b in range(4):
Omega.value[n+b][n+b] += 1.0 / motion_noise
for b in range(2):
Omega.value[n+b ][n+b+2] += -1.0 / motion_noise
Omega.value[n+b+2][n+b ] += -1.0 / motion_noise
Xi.value[n+b ][0] += -motion[b] / motion_noise
Xi.value[n+b+2][0] += motion[b] / motion_noise
B = Omega.take([0,1], [0,1])
A = Omega.take([0,1],range(2,dim+2))
OmegaP = Omega.take(range(2, dim+2), range(2,dim+2))
C = Xi.take([0,1], [0])
XiP = Xi.take(range(2,dim+2),[0])
Omega = OmegaP - (A.transpose() * B.inverse() * A)
Xi = XiP - (A.transpose() * B.inverse() * C)
# compute best estimate
mu = Omega.inverse() * Xi
return mu, Omega # make sure you return both of these matrices to be marked correct.
# --------------------------------
#
# print the result of SLAM, the robot pose(s) and the landmarks
#
def print_result(N, num_landmarks, result):
print
print 'Estimated Pose(s):'
for i in range(N):
print ' ['+ ', '.join('%.3f'%x for x in result.value[2*i]) + ', ' \
+ ', '.join('%.3f'%x for x in result.value[2*i+1]) +']'
print
print 'Estimated Landmarks:'
for i in range(num_landmarks):
print ' ['+ ', '.join('%.3f'%x for x in result.value[2*(N+i)]) + ', ' \
+ ', '.join('%.3f'%x for x in result.value[2*(N+i)+1]) +']'
# ------------------------------------------------------------------------
#
# Main routines
#
num_landmarks = 5 # number of landmarks
N = 20 # time steps
world_size = 100.0 # size of world
measurement_range = 50.0 # range at which we can sense landmarks
motion_noise = 2.0 # noise in robot motion
measurement_noise = 2.0 # noise in the measurements
distance = 20.0 # distance by which robot (intends to) move each iteratation
# Uncomment the following three lines to run the full slam routine.
#data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
#result = slam(data, N, num_landmarks, motion_noise, measurement_noise)
#print_result(N, num_landmarks, result)
# Uncomment the following three lines to run the online_slam routine.
#data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
#result = online_slam(data, N, num_landmarks, motion_noise, measurement_noise)
#print_result(1, num_landmarks, result[0])
##########################################################
# ------------
# TESTING
#
# Uncomment one of the test cases below to check that your
# online_slam function works as expected.
def solution_check(result, answer_mu, answer_omega):
if len(result) != 2:
print "Your function must return TWO matrices, mu and Omega"
return False
user_mu = result[0]
user_omega = result[1]
if user_mu.dimx == answer_omega.dimx and user_mu.dimy == answer_omega.dimy:
print "It looks like you returned your results in the wrong order. Make sure to return mu then Omega."
return False
if user_mu.dimx != answer_mu.dimx or user_mu.dimy != answer_mu.dimy:
print "Your mu matrix doesn't have the correct dimensions. Mu should be a", answer_mu.dimx, " x ", answer_mu.dimy, "matrix."
return False
else:
print "Mu has correct dimensions."
if user_omega.dimx != answer_omega.dimx or user_omega.dimy != answer_omega.dimy:
print "Your Omega matrix doesn't have the correct dimensions. Omega should be a", answer_omega.dimx, " x ", answer_omega.dimy, "matrix."
return False
else:
print "Omega has correct dimensions."
if user_mu != answer_mu:
print "Mu has incorrect entries."
return False
else:
print "Mu correct."
if user_omega != answer_omega:
print "Omega has incorrect entries."
return False
else:
print "Omega correct."
print "Test case passed!"
return True
# -----------
# Test Case 1
testdata1 = [[[[1, 21.796713239511305, 25.32184135169971], [2, 15.067410969755826, -27.599928007267906]], [16.4522379034509, -11.372065246394495]],
[[[1, 6.1286996178786755, 35.70844618389858], [2, -0.7470113490937167, -17.709326161950294]], [16.4522379034509, -11.372065246394495]],
[[[0, 16.305692184072235, -11.72765549112342], [2, -17.49244296888888, -5.371360408288514]], [16.4522379034509, -11.372065246394495]],
[[[0, -0.6443452578030207, -2.542378369361001], [2, -32.17857547483552, 6.778675958806988]], [-16.66697847355152, 11.054945886894709]]]
answer_mu1 = matrix([[81.63549976607898],
[27.175270706192254],
[98.09737507003692],
[14.556272940621195],
[71.97926631050574],
[75.07644206765099],
[65.30397603859097],
[22.150809430682695]])
answer_omega1 = matrix([[0.36603773584905663, 0.0, -0.169811320754717, 0.0, -0.011320754716981133, 0.0, -0.1811320754716981, 0.0],
[0.0, 0.36603773584905663, 0.0, -0.169811320754717, 0.0, -0.011320754716981133, 0.0, -0.1811320754716981],
[-0.169811320754717, 0.0, 0.6509433962264151, 0.0, -0.05660377358490567, 0.0, -0.40566037735849064, 0.0],
[0.0, -0.169811320754717, 0.0, 0.6509433962264151, 0.0, -0.05660377358490567, 0.0, -0.40566037735849064],
[-0.011320754716981133, 0.0, -0.05660377358490567, 0.0, 0.6962264150943396, 0.0, -0.360377358490566, 0.0],
[0.0, -0.011320754716981133, 0.0, -0.05660377358490567, 0.0, 0.6962264150943396, 0.0, -0.360377358490566],
[-0.1811320754716981, 0.0, -0.4056603773584906, 0.0, -0.360377358490566, 0.0, 1.2339622641509433, 0.0],
[0.0, -0.1811320754716981, 0.0, -0.4056603773584906, 0.0, -0.360377358490566, 0.0, 1.2339622641509433]])
#result = online_slam(testdata1, 5, 3, 2.0, 2.0)
#solution_check(result, answer_mu1, answer_omega1)
# -----------
# Test Case 2
testdata2 = [[[[0, 12.637647070797396, 17.45189715769647], [1, 10.432982633935133, -25.49437383412288]], [17.232472057089492, 10.150955955063045]],
[[[0, -4.104607680013634, 11.41471295488775], [1, -2.6421937245699176, -30.500310738397154]], [17.232472057089492, 10.150955955063045]],
[[[0, -27.157759429499166, -1.9907376178358271], [1, -23.19841267128686, -43.2248146183254]], [-17.10510363812527, 10.364141523975523]],
[[[0, -2.7880265859173763, -16.41914969572965], [1, -3.6771540967943794, -54.29943770172535]], [-17.10510363812527, 10.364141523975523]],
[[[0, 10.844236516370763, -27.19190207903398], [1, 14.728670653019343, -63.53743222490458]], [14.192077112147086, -14.09201714598981]]]
answer_mu2 = matrix([[63.37479912250136],
[78.17644539069596],
[61.33207502170053],
[67.10699675357239],
[62.57455560221361],
[27.042758786080363]])
answer_omega2 = matrix([[0.22871751620895048, 0.0, -0.11351536555795691, 0.0, -0.11351536555795691, 0.0],
[0.0, 0.22871751620895048, 0.0, -0.11351536555795691, 0.0, -0.11351536555795691],
[-0.11351536555795691, 0.0, 0.7867205207948973, 0.0, -0.46327947920510265, 0.0],
[0.0, -0.11351536555795691, 0.0, 0.7867205207948973, 0.0, -0.46327947920510265],
[-0.11351536555795691, 0.0, -0.46327947920510265, 0.0, 0.7867205207948973, 0.0],
[0.0, -0.11351536555795691, 0.0, -0.46327947920510265, 0.0, 0.7867205207948973]])
#result = online_slam(testdata2, 6, 2, 3.0, 4.0)
#solution_check(result, answer_mu2, answer_omega2)
#testdata0 = [
# [[[0, 10., 20.], [1, 30., 40.]], [0., 0.]],
# [[[0, 10., 20.], [1, 30., 40.]], [0., 0.]]
#]
#result = online_slam(testdata0, 1, 2, 1.0, 1.0)
#print result[0]
Raw
04_circular_motion.py
#!/usr/bin/env python
# -----------------
# USER INSTRUCTIONS
#
# Write a function in the class robot called move()
#
# that takes self and a motion vector (this
# motion vector contains a steering* angle and a
# distance) as input and returns an instance of the class
# robot with the appropriate x, y, and orientation
# for the given motion.
#
# *steering is defined in the video
# which accompanies this problem.
#
# For now, please do NOT add noise to your move function.
#
# Please do not modify anything except where indicated
# below.
#
# There are test cases which you are free to use at the
# bottom. If you uncomment them for testing, make sure you
# re-comment them before you submit.
from math import *
import random
# --------
#
# the "world" has 4 landmarks.
# the robot's initial coordinates are somewhere in the square
# represented by the landmarks.
#
# NOTE: Landmark coordinates are given in (y, x) form and NOT
# in the traditional (x, y) format!
landmarks = [[0.0, 100.0], [0.0, 0.0], [100.0, 0.0], [100.0, 100.0]] # position of 4 landmarks
world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel "out of bounds"
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.
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation
#
def __init__(self, length = 10.0):
self.x = random.random() * world_size # initial x position
self.y = random.random() * world_size # initial y position
self.orientation = random.random() * 2.0 * pi # initial orientation
self.length = length # length of robot
self.bearing_noise = 0.0 # initialize bearing noise to zero
self.steering_noise = 0.0 # initialize steering noise to zero
self.distance_noise = 0.0 # initialize distance noise to zero
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_b_noise, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.bearing_noise = float(new_b_noise)
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
############# ONLY ADD/MODIFY CODE BELOW HERE ###################
# --------
# move:
# move along a section of a circular path according to motion
#
def move(self, motion): # Do not change the name of this function
# ADD CODE HERE
steering, distance = motion
if abs(steering) > max_steering_angle:
raise ValueError('Exceeding max steering angle')
if distance < 0.0:
raise ValueError('Moving backwards is not valid')
res = robot()
res.length = self.length
res.bearing_noise = self.bearing_noise
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
turn = tan(steering2) * distance2 / res.length
if abs(turn) < 0.001:
# approximate by straingt line motion
res.x = self.x + distance2 * cos(self.orientation)
res.y = self.y + distance2 * sin(self.orientation)
res.orientation = self.orientation + turn
else:
radius = distance2 / turn
cx = self.x - sin(self.orientation)*radius
cy = self.y + cos(self.orientation)*radius
res.orientation = (self.orientation + turn) % (2*pi)
res.x = cx + radius * sin(res.orientation)
res.y = cy - radius * cos(res.orientation)
return res # make sure your move function returns an instance
# of the robot class with the correct coordinates.
############## ONLY ADD/MODIFY CODE ABOVE HERE ####################
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
## --------
## TEST CASE:
##
## 1) The following code should print:
## Robot: [x=0.0 y=0.0 orient=0.0]
## Robot: [x=10.0 y=0.0 orient=0.0]
## Robot: [x=19.861 y=1.4333 orient=0.2886]
## Robot: [x=39.034 y=7.1270 orient=0.2886]
##
##
##length = 20.
##bearing_noise = 0.0
##steering_noise = 0.0
##distance_noise = 0.0
##
##myrobot = robot(length)
##myrobot.set(0.0, 0.0, 0.0)
##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
##
##motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]
##
##T = len(motions)
##
##print 'Robot: ', myrobot
##for t in range(T):
## myrobot = myrobot.move(motions[t])
## print 'Robot: ', myrobot
##
##
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
## 2) The following code should print:
## Robot: [x=0.0 y=0.0 orient=0.0]
## Robot: [x=9.9828 y=0.5063 orient=0.1013]
## Robot: [x=19.863 y=2.0201 orient=0.2027]
## Robot: [x=29.539 y=4.5259 orient=0.3040]
## Robot: [x=38.913 y=7.9979 orient=0.4054]
## Robot: [x=47.887 y=12.400 orient=0.5067]
## Robot: [x=56.369 y=17.688 orient=0.6081]
## Robot: [x=64.273 y=23.807 orient=0.7094]
## Robot: [x=71.517 y=30.695 orient=0.8108]
## Robot: [x=78.027 y=38.280 orient=0.9121]
## Robot: [x=83.736 y=46.485 orient=1.0135]
##
##
##length = 20.
##bearing_noise = 0.0
##steering_noise = 0.0
##distance_noise = 0.0
##
##myrobot = robot(length)
##myrobot.set(0.0, 0.0, 0.0)
##myrobot.set_noise(bearing_noise, steering_noise, distance_noise)
##
##motions = [[0.2, 10.] for row in range(10)]
##
##T = len(motions)
##
##print 'Robot: ', myrobot
##for t in range(T):
## myrobot = myrobot.move(motions[t])
## print 'Robot: ', myrobot
## IMPORTANT: You may uncomment the test cases below to test your code.
## But when you submit this code, your test cases MUST be commented
## out. Our testing program provides its own code for testing your
## move function with randomized motion data.
Raw
04_coconuts.py
#!/usr/bin/env python
#Write a function that repeats the process of
#giving a coconut away and then taking one
#fifth of the remaining coconuts away.
def f(n):
return (n-1) / 5 * 4
def f6(n):
for _ in range(6):
n = f(n)
return n
print f6(96.)
Raw
04_localization_program.py
#!/usr/bin/env python
colors = [['red', 'green', 'green', 'red' , 'red'],
['red', 'red', 'green', 'red', 'red'],
['red', 'red', 'green', 'green', 'red'],
['red', 'red', 'red', 'red', 'red']]
measurements = ['green', 'green', 'green' ,'green', 'green']
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
sensor_right = 0.7
p_move = 0.8
def show(p):
for i in range(len(p)):
print p[i]
#DO NOT USE IMPORT
#ENTER CODE BELOW HERE
#ANY CODE ABOVE WILL CAUSE
#HOMEWORK TO BE GRADED
#INCORRECT
y_size = len(colors)
x_size = len(colors[0])
p = [ [ 1./(y_size*x_size) ] * x_size for _ in range(y_size)]
def sense(p, Z):
result = []
for y in range(y_size):
row = []
for x in range(x_size):
hit = (Z == colors[y][x])
if hit:
row.append(p[y][x] * sensor_right)
else:
row.append(p[y][x] * (1 - sensor_right))
result.append(row)
total_p = sum(sum(result[y]) for y in range(y_size))
for y in range(y_size):
for x in range(x_size):
result[y][x] = result[y][x] / total_p
return result
def move(p, U):
result = []
for y in range(y_size):
row = []
for x in range(x_size):
s = p_move * p[(y-U[0]) % y_size][(x-U[1]) % x_size]
s += (1-p_move) * p[y][x]
row.append(s)
result.append(row)
return result
for k in range(len(measurements)):
p = move(p, motions[k])
p = sense(p, measurements[k])
#Your probability array must be printed
#with the following code.
show(p)
Raw
04_racetrack_control.py
#!/usr/bin/env python
# --------------
# User Instructions
#
# Define a function cte in the robot class that will
# compute the crosstrack error for a robot on a
# racetrack with a shape as described in the video.
#
# You will need to base your error calculation on
# the robot's location on the track. Remember that
# the robot will be traveling to the right on the
# upper straight segment and to the left on the lower
# straight segment.
#
# --------------
# Grading Notes
#
# We will be testing your cte function directly by
# calling it with different robot locations and making
# sure that it returns the correct crosstrack error.
from math import *
import random
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 20.0):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# set_steering_drift:
# sets the systematical steering drift parameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# Execute motion
turn = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
############## ONLY ADD / MODIFY CODE BELOW THIS LINE ####################
def cte(self, radius):
#
#
# Add code here
#
#
if self.x <= radius:
cte = sqrt((self.x-radius) ** 2 + (self.y-radius)**2) - radius
elif radius <= self.x <= 3*radius and self.y>radius:
cte = self.y - 2*radius
elif self.x >= 3*radius:
cte = sqrt((self.x-3*radius) ** 2 + (self.y-radius)**2) - radius
elif radius <= self.x <= 3*radius and self.y<radius:
cte = - self.y
return cte
############## ONLY ADD / MODIFY CODE ABOVE THIS LINE ####################
# ------------------------------------------------------------------------
#
# run - does a single control run.
def run(params, radius, printflag = False):
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200
crosstrack_error = myrobot.cte(radius) # You need to define the cte function!
for i in range(N*2):
diff_crosstrack_error = - crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
- params[2] * int_crosstrack_error
myrobot = myrobot.move(steer, speed)
if i >= N:
err += crosstrack_error ** 2
if printflag:
print myrobot
return err / float(N)
radius = 25.0
params = [10.0, 15.0, 0]
err = run(params, radius, True)
print '\nFinal paramaeters: ', params, '\n ->', err
Raw
05_coconuts.py
#!/usr/bin/env python
#Write a program that will find the initial number
#of coconuts.
def f(n):
return (n-1) / 5.0 * 4
def f6(n):
for i in range(6):
n = f(n)
return n
def is_int(n):
return abs(n-int(n)) < 0.0000001
n = 1
while True:
n += 1
if is_int(f6(n)):
break
print n
Raw
05_path_smoothing.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Define a function smooth that takes a path as its input
# (with optional parameters for weight_data, weight_smooth)
# and returns a smooth path.
#
# Smoothing should be implemented by iteratively updating
# each entry in newpath until some desired level of accuracy
# is reached. The update should be done according to the
# gradient descent equations given in the previous video:
#
# If your function isn't submitting it is possible that the
# runtime is too long. Try sacrificing accuracy for speed.
# -----------
from math import *
# Don't modify path inside your function.
path = [[0, 0],
[0, 1],
[0, 2],
[1, 2],
[2, 2],
[3, 2],
[4, 2],
[4, 3],
[4, 4]]
# ------------------------------------------------
# smooth coordinates
#
def smooth(path, weight_data=0.5, weight_smooth=0.1, tolerance=0.000001):
# Make a deep copy of path into newpath
newpath = [[0 for row in range(len(path[0]))] for col in range(len(path))]
for i in range(len(path)):
for j in range(len(path[0])):
newpath[i][j] = path[i][j]
#### ENTER CODE BELOW THIS LINE ###
change = tolerance
while change >= tolerance:
change = 0.0
for i in range(1, len(path)-1):
for j in range(1, len(path[0])):
aux = newpath[i][j]
newpath[i][j] += weight_data * (path[i][j]-newpath[i][j])
newpath[i][j] += weight_smooth * (newpath[i-1][j] \
+ newpath[i+1][j] - 2.*newpath[i][j])
change += abs(aux - newpath[i][j])
return newpath # Leave this line for the grader!
# feel free to leave this and the following lines if you want to print.
newpath = smooth(path)
# thank you - EnTerr - for posting this on our discussion forum
for i in range(len(path)):
print '['+ ', '.join('%.3f'%x for x in path[i]) +'] -> ['+ ', '.join('%.3f'%x for x in newpath[i]) +']'
Raw
05_uniform_distribution.py
#!/usr/bin/env python
#Modify the empty list, p, so that it becomes a UNIFORM probability
#distribution over five grid cells, as expressed in a list of
#five probabilities.
p=[0.2] * 5
print p
Raw
06_generalized_uniform_distribution.py
#!/usr/bin/env python
#Modify your code to create probability vectors, p, of arbitrary
#size, n. Use n=5 to verify that your new solution matches
#the previous one.
p=[]
n=5
p = [1./n] * n
print p
Raw
06_localization_program.py
#!/usr/bin/env python
colors = [['green', 'green'],
['red', 'green']]
measurements = ['red',]
motions = [[-1,0]]
sensor_right = 0.8
p_move = 1
def show(p):
for i in range(len(p)):
print p[i]
#DO NOT USE IMPORT
#ENTER CODE BELOW HERE
#ANY CODE ABOVE WILL CAUSE
#HOMEWORK TO BE GRADED
#INCORRECT
y_size = len(colors)
x_size = len(colors[0])
p = [ [ 1./(y_size*x_size) ] * x_size for _ in range(y_size)]
def sense(p, Z):
result = []
for y in range(y_size):
row = []
for x in range(x_size):
hit = (Z == colors[y][x])
if hit:
row.append(p[y][x] * sensor_right)
else:
row.append(p[y][x] * (1 - sensor_right))
result.append(row)
total_p = sum(sum(result[y]) for y in range(y_size))
for y in range(y_size):
for x in range(x_size):
result[y][x] = result[y][x] / total_p
return result
def move(p, U):
result = []
for y in range(y_size):
row = []
for x in range(x_size):
s = 0
if y-U[0] >= y_size:
s = 0
else:
s = p[(y-U[0])][(x-U[1])]
row.append(s)
result.append(row)
#total_p = sum(sum(result[y]) for y in range(y_size))
#for y in range(y_size):
# for x in range(x_size):
# result[y][x] = result[y][x] / total_p
return result
#for k in range(len(measurements)):
# show(p)
# p = sense(p, measurements[k])
# show(p)
# p = move(p, motions[k])
# show(p)
p = [[0.7142, 0.2856],
[0, 0]]
p = sense(p, 'red')
show(p)
#Your probability array must be printed
#with the following code.
Raw
06_programming_exercise.py
#!/usr/bin/env python
# Fill in the matrices P, F, H, R and I at the bottom
#
# This question requires NO CODING, just fill in the
# matrices where indicated. Please do not delete or modify
# any provided code OR comments. Good luck!
from math import *
class matrix:
# implements basic operations of a matrix class
def __init__(self, value):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
def zero(self, dimx, dimy):
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0 for row in range(dimy)] for col in range(dimx)]
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1
def show(self):
for i in range(self.dimx):
print self.value[i]
print ' '
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimensions to add"
else:
# add if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimensions to subtract"
else:
# subtract if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# subtract if correct dimensions
res = matrix([[]])
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
def transpose(self):
# compute transpose
res = matrix([[]])
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
def Cholesky(self, ztol=1.0e-5):
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/tjj**2 - S/tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
return res
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
def __repr__(self):
return repr(self.value)
########################################
def filter(x, P):
for n in range(len(measurements)):
# prediction
x = (F * x) + u
P = F * P * F.transpose()
# measurement update
Z = matrix([measurements[n]])
y = Z.transpose() - (H * x)
S = H * P * H.transpose() + R
K = P * H.transpose() * S.inverse()
x = x + (K * y)
P = (I - (K * H)) * P
print 'x= '
x.show()
print 'P= '
P.show()
########################################
print "### 4-dimensional example ###"
measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
initial_xy = [4., 12.]
# measurements = [[1., 4.], [6., 0.], [11., -4.], [16., -8.]]
# initial_xy = [-4., 8.]
# measurements = [[1., 17.], [1., 15.], [1., 13.], [1., 11.]]
# initial_xy = [1., 19.]
dt = 0.1
x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]]) # initial state (location and velocity)
u = matrix([[0.], [0.], [0.], [0.]]) # external motion
#### DO NOT MODIFY ANYTHING ABOVE HERE ####
#### fill this in, remember to use the matrix() function!: ####
P = matrix([[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 1000., 0.],
[0., 0., 0., 1000.]]) # initial uncertainty
F = matrix([[1., 0., dt, 0.],
[0, 1., 0., dt],
[0, 0, 1., 0.],
[0, 0, 0., 1.,]])# next state function
H = matrix([[1., 0., 0., 0.],
[0., 1., 0., 0.]])# measurement function
R = matrix([[0.1, 0],
[0, 0.1]]) # measurement uncertainty
I = matrix([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) # identity matrix
###### DO NOT MODIFY ANYTHING HERE #######
filter(x, P)
Raw
06_segmented_cte.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Familiarize yourself with the code below. Most of it
# reproduces results that you have obtained at some
# point in this class. Once you understand the code,
# write a function, cte, in the run class that
# computes the crosstrack
# error for the case of a segmented path. You will
# need to include the equations shown in the video.
#
from math import *
import random
# don't change the noise paameters
steering_noise = 0.1
distance_noise = 0.03
measurement_noise = 0.3
class plan:
# --------
# init:
# creates an empty plan
#
def __init__(self, grid, init, goal, cost = 1):
self.cost = cost
self.grid = grid
self.init = init
self.goal = goal
self.make_heuristic(grid, goal, self.cost)
self.path = []
self.spath = []
# --------
#
# make heuristic function for a grid
def make_heuristic(self, grid, goal, cost):
self.heuristic = [[0 for row in range(len(grid[0]))]
for col in range(len(grid))]
for i in range(len(self.grid)):
for j in range(len(self.grid[0])):
self.heuristic[i][j] = abs(i - self.goal[0]) + \
abs(j - self.goal[1])
# ------------------------------------------------
#
# A* for searching a path to the goal
#
#
def astar(self):
if self.heuristic == []:
raise ValueError, "Heuristic must be defined to run A*"
# internal motion parameters
delta = [[-1, 0], # go up
[ 0, -1], # go left
[ 1, 0], # go down
[ 0, 1]] # do right
# open list elements are of the type: [f, g, h, x, y]
closed = [[0 for row in range(len(self.grid[0]))]
for col in range(len(self.grid))]
action = [[0 for row in range(len(self.grid[0]))]
for col in range(len(self.grid))]
closed[self.init[0]][self.init[1]] = 1
x = self.init[0]
y = self.init[1]
h = self.heuristic[x][y]
g = 0
f = g + h
open = [[f, g, h, x, y]]
found = False # flag that is set when search complete
resign = False # flag set if we can't find expand
count = 0
while not found and not resign:
# check if we still have elements on the open list
if len(open) == 0:
resign = True
print '###### Search terminated without success'
else:
# remove node from list
open.sort()
open.reverse()
next = open.pop()
x = next[3]
y = next[4]
g = next[1]
# check if we are done
if x == goal[0] and y == goal[1]:
found = True
# print '###### A* search successful'
else:
# expand winning element and add to new open list
for i in range(len(delta)):
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(self.grid) and y2 >= 0 \
and y2 < len(self.grid[0]):
if closed[x2][y2] == 0 and self.grid[x2][y2] == 0:
g2 = g + self.cost
h2 = self.heuristic[x2][y2]
f2 = g2 + h2
open.append([f2, g2, h2, x2, y2])
closed[x2][y2] = 1
action[x2][y2] = i
count += 1
# extract the path
invpath = []
x = self.goal[0]
y = self.goal[1]
invpath.append([x, y])
while x != self.init[0] or y != self.init[1]:
x2 = x - delta[action[x][y]][0]
y2 = y - delta[action[x][y]][1]
x = x2
y = y2
invpath.append([x, y])
self.path = []
for i in range(len(invpath)):
self.path.append(invpath[len(invpath) - 1 - i])
# ------------------------------------------------
#
# this is the smoothing function
#
def smooth(self, weight_data = 0.1, weight_smooth = 0.1,
tolerance = 0.000001):
if self.path == []:
raise ValueError, "Run A* first before smoothing path"
self.spath = [[0 for row in range(len(self.path[0]))] \
for col in range(len(self.path))]
for i in range(len(self.path)):
for j in range(len(self.path[0])):
self.spath[i][j] = self.path[i][j]
change = tolerance
while change >= tolerance:
change = 0.0
for i in range(1, len(self.path)-1):
for j in range(len(self.path[0])):
aux = self.spath[i][j]
self.spath[i][j] += weight_data * \
(self.path[i][j] - self.spath[i][j])
self.spath[i][j] += weight_smooth * \
(self.spath[i-1][j] + self.spath[i+1][j]
- (2.0 * self.spath[i][j]))
if i >= 2:
self.spath[i][j] += 0.5 * weight_smooth * \
(2.0 * self.spath[i-1][j] - self.spath[i-2][j]
- self.spath[i][j])
if i <= len(self.path) - 3:
self.spath[i][j] += 0.5 * weight_smooth * \
(2.0 * self.spath[i+1][j] - self.spath[i+2][j]
- self.spath[i][j])
change += abs(aux - self.spath[i][j])
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 0.5):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.measurement_noise = 0.0
self.num_collisions = 0
self.num_steps = 0
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise, new_m_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
self.measurement_noise = float(new_m_noise)
# --------
# check:
# checks of the robot pose collides with an obstacle, or
# is too far outside the plane
def check_collision(self, grid):
for i in range(len(grid)):
for j in range(len(grid[0])):
if grid[i][j] == 1:
dist = sqrt((self.x - float(i)) ** 2 +
(self.y - float(j)) ** 2)
if dist < 0.5:
self.num_collisions += 1
return False
return True
def check_goal(self, goal, threshold = 1.0):
dist = sqrt((float(goal[0]) - self.x) ** 2 + (float(goal[1]) - self.y) ** 2)
return dist < threshold
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, grid, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.measurement_noise = self.measurement_noise
res.num_collisions = self.num_collisions
res.num_steps = self.num_steps + 1
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# Execute motion
turn = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
# check for collision
# res.check_collision(grid)
return res
# --------
# sense:
#
def sense(self):
return [random.gauss(self.x, self.measurement_noise),
random.gauss(self.y, self.measurement_noise)]
# --------
# measurement_prob
# computes the probability of a measurement
#
def measurement_prob(self, measurement):
# compute errors
error_x = measurement[0] - self.x
error_y = measurement[1] - self.y
# calculate Gaussian
error = exp(- (error_x ** 2) / (self.measurement_noise ** 2) / 2.0) \
/ sqrt(2.0 * pi * (self.measurement_noise ** 2))
error *= exp(- (error_y ** 2) / (self.measurement_noise ** 2) / 2.0) \
/ sqrt(2.0 * pi * (self.measurement_noise ** 2))
return error
def __repr__(self):
# return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
return '[%.5f, %.5f]' % (self.x, self.y)
# ------------------------------------------------
#
# this is the particle filter class
#
class particles:
# --------
# init:
# creates particle set with given initial position
#
def __init__(self, x, y, theta,
steering_noise, distance_noise, measurement_noise, N = 100):
self.N = N
self.steering_noise = steering_noise
self.distance_noise = distance_noise
self.measurement_noise = measurement_noise
self.data = []
for i in range(self.N):
r = robot()
r.set(x, y, theta)
r.set_noise(steering_noise, distance_noise, measurement_noise)
self.data.append(r)
# --------
#
# extract position from a particle set
#
def get_position(self):
x = 0.0
y = 0.0
orientation = 0.0
for i in range(self.N):
x += self.data[i].x
y += self.data[i].y
# orientation is tricky because it is cyclic. By normalizing
# around the first particle we are somewhat more robust to
# the 0=2pi problem
orientation += (((self.data[i].orientation
- self.data[0].orientation + pi) % (2.0 * pi))
+ self.data[0].orientation - pi)
return [x / self.N, y / self.N, orientation / self.N]
# --------
#
# motion of the particles
#
def move(self, grid, steer, speed):
newdata = []
for i in range(self.N):
r = self.data[i].move(grid, steer, speed)
newdata.append(r)
self.data = newdata
# --------
#
# sensing and resampling
#
def sense(self, Z):
w = []
for i in range(self.N):
w.append(self.data[i].measurement_prob(Z))
# resampling (careful, this is using shallow copy)
p3 = []
index = int(random.random() * self.N)
beta = 0.0
mw = max(w)
for i in range(self.N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % self.N
p3.append(self.data[index])
self.data = p3
# --------
#
# run: runs control program for the robot
#
def run(grid, goal, spath, params, printflag = False, speed = 0.1, timeout = 1000):
myrobot = robot()
myrobot.set(0., 0., 0.)
myrobot.set_noise(steering_noise, distance_noise, measurement_noise)
filter = particles(myrobot.x, myrobot.y, myrobot.orientation,
steering_noise, distance_noise, measurement_noise)
cte = 0.0
err = 0.0
N = 0
index = 0 # index into the path
while not myrobot.check_goal(goal) and N < timeout:
diff_cte = - cte
# ----------------------------------------
# compute the CTE
# start with the present robot estimate
estimate = filter.get_position()
### ENTER CODE HERE
dx = spath[index+1][0] - spath[index][0]
dy = spath[index+1][1] - spath[index][1]
drx = estimate[0] - spath[index][0]
dry = estimate[1] - spath[index][1]
u = (drx * dx + dry * dy) / (dx * dx + dy * dy)
cte = (dry * dx - drx * dy) / (dx * dx + dy * dy)
if u > 1.0:
index += 1
# ----------------------------------------
diff_cte += cte
steer = - params[0] * cte - params[1] * diff_cte
myrobot = myrobot.move(grid, steer, speed)
filter.move(grid, steer, speed)
Z = myrobot.sense()
filter.sense(Z)
if not myrobot.check_collision(grid):
print '##### Collision ####'
err += (cte ** 2)
N += 1
if printflag:
print myrobot, cte, index, u
return [myrobot.check_goal(goal), myrobot.num_collisions, myrobot.num_steps]
# ------------------------------------------------
#
# this is our main routine
#
def main(grid, init, goal, steering_noise, distance_noise, measurement_noise,
weight_data, weight_smooth, p_gain, d_gain):
path = plan(grid, init, goal)
path.astar()
path.smooth(weight_data, weight_smooth)
return run(grid, goal, path.spath, [p_gain, d_gain])
# ------------------------------------------------
#
# input data and parameters
#
# grid format:
# 0 = navigable space
# 1 = occupied space
grid = [[0, 1, 0, 0, 0, 0],
[0, 1, 0, 1, 1, 0],
[0, 1, 0, 1, 0, 0],
[0, 0, 0, 1, 0, 1],
[0, 1, 0, 1, 0, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
steering_noise = 0.1
distance_noise = 0.03
measurement_noise = 0.3
weight_data = 0.1
weight_smooth = 0.2
p_gain = 2.0
d_gain = 6.0
print main(grid, init, goal, steering_noise, distance_noise, measurement_noise,
weight_data, weight_smooth, p_gain, d_gain)
def twiddle(init_params):
n_params = len(init_params)
dparams = [1.0 for row in range(n_params)]
params = [0.0 for row in range(n_params)]
K = 10
for i in range(n_params):
params[i] = init_params[i]
best_error = 0.0;
for k in range(K):
ret = main(grid, init, goal,
steering_noise, distance_noise, measurement_noise,
params[0], params[1], params[2], params[3])
if ret[0]:
best_error += ret[1] * 100 + ret[2]
else:
best_error += 99999
best_error = float(best_error) / float(k+1)
print best_error
n = 0
while sum(dparams) > 0.0000001:
for i in range(len(params)):
params[i] += dparams[i]
err = 0
for k in range(K):
ret = main(grid, init, goal,
steering_noise, distance_noise, measurement_noise,
params[0], params[1], params[2], params[3], best_error)
if ret[0]:
err += ret[1] * 100 + ret[2]
else:
err += 99999
print float(err) / float(k+1)
if err < best_error:
best_error = float(err) / float(k+1)
dparams[i] *= 1.1
else:
params[i] -= 2.0 * dparams[i]
err = 0
for k in range(K):
ret = main(grid, init, goal,
steering_noise, distance_noise, measurement_noise,
params[0], params[1], params[2], params[3], best_error)
if ret[0]:
err += ret[1] * 100 + ret[2]
else:
err += 99999
print float(err) / float(k+1)
if err < best_error:
best_error = float(err) / float(k+1)
dparams[i] *= 1.1
else:
params[i] += dparams[i]
dparams[i] *= 0.5
n += 1
print 'Twiddle #', n, params, ' -> ', best_error
print ' '
return params
#twiddle([weight_data, weight_smooth, p_gain, d_gain])
Raw
07_coconuts.py
#!/usr/bin/env python
#Write a program that outputs the number
#of coconuts we started with. Do not
#overthink this one! You should be able
#to answer this with one line of code!
print 5**6-4
Raw
09_moving_robot.py
#!/usr/bin/env python
# Make a robot called myrobot that starts at
# coordinates 30, 50 heading north (pi/2).
# Have your robot turn clockwise by pi/2, move
# 15 m, and sense. Then have it turn clockwise
# by pi/2 again, move 10 m, and sense again.
#
# Your program should print out the result of
# your two sense measurements.
#
# Don't modify the code below. Please enter
# your code at the bottom.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
myrobot = robot()
myrobot.set(30, 50, pi/2)
myrobot = myrobot.move(-pi/2, 15)
print myrobot.sense()
myrobot = myrobot.move(-pi/2, 10)
print myrobot.sense()
Raw
09_p_controller.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Implement a P controller by running 100 iterations
# of robot motion. The steering angle should be set
# by the parameter tau so that:
#
# steering = -tau * crosstrack_error
#
# Note that tau is called "param" in the function
# run to be completed below.
#
# Your code should print output that looks like
# the output shown in the video. That is, at each step:
# print myrobot, steering
#
# Only modify code at the bottom!
# ------------
from math import *
import random
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 20.0):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# set_steering_drift:
# sets the systematical steering drift parameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# Execute motion
turn = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
############## ADD / MODIFY CODE BELOW ####################
# ------------------------------------------------------------------------
#
# run - does a single control run
def run(param):
myrobot = robot()
myrobot.set(0.0, 1.0, 0.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
N = 100
#
# Add Code Here
#
for i in range(N):
crosstrack_error = myrobot.y
steer = - param * crosstrack_error
myrobot = myrobot.move(steer, speed)
print myrobot, steer
run(0.1) # call function with parameter tau of 0.1 and print results
Raw
10_add_noise.py
#!/usr/bin/env python
# Now add noise to your robot as follows:
# forward_noise = 5.0, turn_noise = 0.1,
# sense_noise = 5.0.
#
# Once again, your robot starts at 30, 50,
# heading north (pi/2), then turns clockwise
# by pi/2, moves 15 meters, senses,
# then turns clockwise by pi/2 again, moves
# 10 m, then senses again.
#
# Your program should print out the result of
# your two sense measurements.
#
# Don't modify the code below. Please enter
# your code at the bottom.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
myrobot = robot()
# enter code here
myrobot.set_noise(5.0, 0.1, 5.0)
myrobot.set(30.0, 50.0, pi/2)
myrobot = myrobot.move(-pi/2, 15.0)
print myrobot.sense()
myrobot = myrobot.move(-pi/2, 10.0)
print myrobot.sense()
Raw
10_phin_and_pmiss.py
#!/usr/bin/env python
#Write a code that outputs p after multiplying each entry
#by pHit or pMiss at the appropriate places. Remember that
#the red cells 1 and 2 are hits and the other green cells
#are misses
p=[0.2,0.2,0.2,0.2,0.2]
pHit = 0.6
pMiss = 0.2
#Enter code here
pHitRes = [0, 1, 1, 0, 0]
pHitFunc = [[pMiss, pHit][hit] for hit in pHitRes]
p = [pVal*pHitVal for pVal, pHitVal in zip(p, pHitFunc)]
Raw
11_pd_controller.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Implement a PD controller by running 100 iterations
# of robot motion. The steering angle should be set
# by the parameter tau so that:
#
# steering = -tau_p * CTE - tau_d * diff_CTE
# where differential crosstrack error (diff_CTE)
# is given by CTE(t) - CTE(t-1)
#
# Your code should print output that looks like
# the output shown in the video.
#
# Only modify code at the bottom!
# ------------
from math import *
import random
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 20.0):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# set_steering_drift:
# sets the systematical steering drift parameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# Execute motion
turn = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
############## ADD / MODIFY CODE BELOW ####################
# ------------------------------------------------------------------------
#
# run - does a single control run.
def run(param1, param2):
myrobot = robot()
myrobot.set(0.0, 1.0, 0.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
N = 100
#
# Enter code here
#
crosstrack_error = myrobot.y
for i in range(N):
diff_crosstrack_error = myrobot.y - crosstrack_error
crosstrack_error = myrobot.y
steer = - param1 * crosstrack_error \
- param2 * diff_crosstrack_error
myrobot = myrobot.move(steer, speed)
print myrobot, steer
print run(0.2, 3.0)# Call your function with parameters of 0.2 and 3.0 and print results
Raw
11_sum_of_probabilities.py
#!/usr/bin/env python
#Modify the program to find and print the sum of all
#the entries in the list p.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
pHit = 0.6
pMiss = 0.2
p[0]=p[0]*pMiss
p[1]=p[1]*pHit
p[2]=p[2]*pHit
p[3]=p[3]*pMiss
p[4]=p[4]*pMiss
print sum(p)
Raw
12_creating_particies.py
#!/usr/bin/env python
# Now we want to create particles,
# p[i] = robot(). In this assignment, write
# code that will assign 1000 such particles
# to a list.
#
# Your program should print out the length
# of your list (don't cheat by making an
# arbitrary list of 1000 elements!)
#
# Don't modify the code below. Please enter
# your code at the bottom.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
N = 1000
p = []
#enter code here
for _ in range(N):
p.append(robot())
print len(p)
Raw
12_sense_function.py
#!/usr/bin/env python
#Modify the code below so that the function sense, which
#takes p and Z as inputs, will output the NON-normalized
#probability distribution, q, after multiplying the entries
#in p by pHit or pMiss according to the color in the
#corresponding cell in world.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
Z = 'red'
pHit = 0.6
pMiss = 0.2
def sense(p, Z):
return [pVal*[pMiss, pHit][Z==worldVal] for pVal, worldVal in zip(p, world)]
print sense(p,Z)
Raw
13_maximize_gausian.py
#!/usr/bin/env python
#For this problem, you aren't writing any code.
#Instead, please just change the last argument
#in f() to maximize the output.
from math import *
def f(mu, sigma2, x):
return 1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)
print f(10.,4.,10.) #Change the 8. to something else!
Raw
13_normalized_sense_function.py
#!/usr/bin/env python
#Modify your code so that it normalizes the output for
#the function sense. This means that the entries in q
#should sum to one.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
Z = 'red'
pHit = 0.6
pMiss = 0.2
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
q = [qVal/sum(q) for qVal in q]
return q
print sense(p,Z)
Raw
13_robot_particles.py
#!/usr/bin/env python
# Now we want to simulate robot
# motion with our particles.
# Each particle should turn by 0.1
# and then move by 5.
#
#
# Don't modify the code below. Please enter
# your code at the bottom.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
N = 1000
p = []
for i in range(N):
x = robot()
x = x.move(0.1, 5.0)
p.append(x)
print p #PLEASE LEAVE THIS HERE FOR GRADING PURPOSES
Raw
14_importance_weight.py
#!/usr/bin/env python
# Now we want to give weight to our
# particles. This program will print a
# list of 1000 particle weights.
#
# Don't modify the code below. Please enter
# your code at the bottom.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
#insert code here!
for i in range(N):
w.append(p[i].measurement_prob(Z))
print w #Please print w for grading purposes.
Raw
14_pid_controller.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Implement a P controller by running 100 iterations
# of robot motion. The steering angle should be set
# by the parameter tau so that:
#
# steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE
#
# where the integrated crosstrack error (int_CTE) is
# the sum of all the previous crosstrack errors.
# This term works to cancel out steering drift.
#
# Your code should print a list that looks just like
# the list shown in the video.
#
# Only modify code at the bottom!
# ------------
from math import *
import random
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 20.0):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# set_steering_drift:
# sets the systematical steering drift parameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# Execute motion
turn = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
############## ADD / MODIFY CODE BELOW ####################
# ------------------------------------------------------------------------
#
# run - does a single control run.
def run(param1, param2, param3):
myrobot = robot()
myrobot.set(0.0, 1.0, 0.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
N = 100
myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias, this will be added in by the move function, you do not need to add it below!
#
# Enter code here
#
int_crosstrack_error = 0.
crosstrack_error = myrobot.y
for i in range(N):
diff_crosstrack_error = myrobot.y - crosstrack_error
crosstrack_error = myrobot.y
int_crosstrack_error += crosstrack_error
steer = - param1 * crosstrack_error \
- param2 * diff_crosstrack_error \
- param3 * int_crosstrack_error
myrobot = myrobot.move(steer, speed)
print myrobot, steer
print run(0.2, 3.0, 0.004)
# Call your function with parameters of (0.2, 3.0, and 0.004)
Raw
14_test_sense_function.py
#!/usr/bin/env python
#Try using your code with a measurement of 'green' and
#make sure the resulting probability distribution is correct.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
Z = 'green'
pHit = 0.6
pMiss = 0.2
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(p)):
q[i]=q[i]/s
return q
print sense(p, Z)
Raw
15_multiple_measurements.py
#!/usr/bin/env python
#Modify the code so that it updates the probability twice
#and gives the posterior distribution after both
#measurements are incorporated. Make sure that your code
#allows for any sequence of measurement of any length.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
#
#ADD YOUR CODE HERE
#
for Z in measurements:
p = sense(p, Z)
print p
Raw
15_twiddle.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Implement twiddle as shown in the previous two videos.
# Your accumulated error should be very small!
#
# Your twiddle function should RETURN the accumulated
# error. Try adjusting the parameters p and dp to make
# this error as small as possible.
#
# Try to get your error below 1.0e-10 with as few iterations
# as possible (too many iterations will cause a timeout).
# No cheating!
# ------------
from math import *
import random
# ------------------------------------------------
#
# this is the robot class
#
class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 20.0):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0
# --------
# set:
# sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)
# --------
# set_noise:
# sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)
# --------
# set_steering_drift:
# sets the systematical steering drift parameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift
# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):
if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0
# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift
# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)
# apply steering drift
steering2 += self.steering_drift
# Execute motion
turn = tan(steering2) * distance2 / res.length
if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)
return res
def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)
# ------------------------------------------------------------------------
#
# run - does a single control run.
def run(params):
myrobot = robot()
myrobot.set(0.0, 1.0, 0.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
N = 100
myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias, this will be added in by the move function, you do not need to add it below!
old_CTE = myrobot.y
int_error = 0
error = 0
for i in range(2*N):
CTE = myrobot.y
int_error += speed*cos(myrobot.orientation)*CTE
steering = -params[0]*CTE - params[1]*(CTE - old_CTE) - params[2]*int_error
old_CTE = myrobot.y
myrobot = myrobot.move(steering, speed)
if i > N:
error += CTE**2
#print 'error = ', error
return error
############## ADD CODE BELOW ####################
def twiddle(tol = 0.1):
# -------------
# Add code here
# -------------
n_params = 3
dparams = [1.0 for row in range(n_params)]
params = [0.0 for row in range(n_params)]
best_error = run(params)
n = 0
while sum(dparams) > tol:
for i in range(len(params)):
params[i] += dparams[i]
err = run(params)
if err < best_error:
best_error = err
dparams[i] *= 1.1
else:
params[i] -= 2.0 * dparams[i]
err = run(params)
if err < best_error:
best_error = err
dparams[i] *= 1.1
else:
params[i] += dparams[i]
dparams[i] *= 0.9
n += 1
print 'Twiddle #', n, params, ' -> ', best_error
print ' '
return params
return error # Your function only needs to return the computed error
# from runs 100-200.
params = twiddle()
err = run(params)
print '\nFinal parameters: ', params, '\n ->', err
Raw
17_move_function.py
#!/usr/bin/env python
#Program a function that returns a new distribution
#q, shifted to the right by U units. If U=0, q should
#be the same as p.
p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
def move(p, U):
#
#ADD CODE HERE
U = U % len(p)
return p[len(p)-U:] + p[:len(p)-U]
print move(p, 1)
Raw
17_omega_and_xi.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Write a function, doit, that takes as its input an
# initial robot position, move1, and move2. This
# function should compute the Omega and Xi matrices
# discussed in lecture and should RETURN the mu vector
# (which is the product of Omega.inverse() and Xi).
#
# Please enter your code at the bottom.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy = 0):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ######################################################################
# ######################################################################
"""
For the following example, you would call doit(-3, 5, 3):
3 robot positions
initially: -3
moves by 5
moves by 3
which should return a mu of:
[[-3.0],
[2.0],
[5.0]]
"""
def doit(initial_pos, move1, move2):
#
#
# Add your code here.
#
#
Omega = matrix([[1., 0., 0.],
[0., 0., 0.],
[0., 0., 0.]])
Xi = matrix([[initial_pos], [0.], [0.]])
Omega += matrix([[ 1., -1., 0.],
[-1., 1., 0.],
[ 0., 0., 0.]])
Xi += matrix([[-move1], [move1], [0.]])
Omega += matrix([[0., 0., 0.],
[0., 1., -1.],
[0., -1., 1.]])
Xi += matrix([[0.], [-move2], [move2]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Result: ')
return mu
doit(-3, 5, 3)
Raw
19_expand.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Modify your doit function to incorporate 3
# distance measurements to a landmark(Z0, Z1, Z2).
# You should use the provided expand function to
# allow your Omega and Xi matrices to accomodate
# the new information.
#
# Each landmark measurement should modify 4
# values in your Omega matrix and 2 in your
# Xi vector.
#
# Add your code at line 356.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy = 0):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ######################################################################
# ######################################################################
"""
For the following example, you would call doit(-3, 5, 3, 10, 5, 2):
3 robot positions
initially: -3 (measure landmark to be 10 away)
moves by 5 (measure landmark to be 5 away)
moves by 3 (measure landmark to be 2 away)
which should return a mu of:
[[-3.0],
[2.0],
[5.0],
[7.0]]
"""
def doit(initial_pos, move1, move2, Z0, Z1, Z2):
Omega = matrix([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]])
Xi = matrix([[initial_pos],
[0.0],
[0.0]])
Omega += matrix([[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[0.0, 0.0, 0.0]])
Xi += matrix([[-move1],
[move1],
[0.0]])
Omega += matrix([[0.0, 0.0, 0.0],
[0.0, 1.0, -1.0],
[0.0, -1.0, 1.0]])
Xi += matrix([[0.0],
[-move2],
[move2]])
#
#
# Add your code here.
#
#
Omega = Omega.expand(4, 4, [0, 1, 2], [0, 1, 2])
Xi = Xi.expand(4, 1, [0, 1, 2], [0])
Omega += matrix([[ 1., 0., 0., -1.],
[ 0., 0., 0., 0.],
[ 0., 0., 0., 0.],
[-1., 0., 0., 1.]])
Xi += matrix([[-Z0], [0.], [0.], [Z0]])
Omega += matrix([[0., 0., 0., 0.],
[0., 1., 0., -1.],
[0., 0., 0., 0.],
[0., -1., 0., 1.]])
Xi += matrix([[0.], [-Z1], [0.], [Z1]])
Omega += matrix([[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 1., -1.],
[0., 0., -1., 1.]])
Xi += matrix([[0.], [0.], [-Z2], [Z2]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Mu: ')
return mu
doit(-3, 5, 3, 10, 5, 2)
Raw
19_new_particles.py
#!/usr/bin/env python
# In this exercise, try to write a program that
# will resample particles according to their weights.
# Particles with higher weights should be sampled
# more frequently (in proportion to their weight).
# Don't modify anything below. Please scroll to the
# bottom to enter your code.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i in range(N):
w.append(p[i].measurement_prob(Z))
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
# You should make sure that p3 contains a list with particles
# resampled according to their weights.
# Also, DO NOT MODIFY p.
p3 = []
def weighted_choice(choices):
total = sum(w for c,w in choices)
r = random.uniform(0, total)
upto = 0
for c, w in choices:
if upto+w > r:
return c
upto += w
assert False, "Shouldn't get here"
for i in range(N):
p3.append(weighted_choice(zip(p,w)))
Raw
20_resampling_wheel.py
#!/usr/bin/env python
# In this exercise, you should implement the
# resampler shown in the previous video.
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i in range(N):
w.append(p[i].measurement_prob(Z))
p3 = []
index = int(random.random()*N)
beta = 0
mw = max(w)
for _ in range(N):
beta += random.random()*2*mw
while beta > w[index]:
beta -= w[index]
index = (index+1) % N
p3.append(p[index])
p = p3
print p #please leave this print statement here for grading!
Raw
21_confinent_measurements.py
#!/usr/bin/env python
# -----------
# User Instructions
#
# Modify the previous code to adjust for a highly
# confident last measurement. Do this by adding a
# factor of 5 into your Omega and Xi matrices
# as described in the video.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy = 0):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ######################################################################
# ######################################################################
# ######################################################################
# Including the 5 times multiplier, your returned mu should now be:
#
# [[-3.0],
# [2.179],
# [5.714],
# [6.821]]
############## MODIFY CODE BELOW ##################
def doit(initial_pos, move1, move2, Z0, Z1, Z2):
Omega = matrix([[1.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0]])
Xi = matrix([[initial_pos],
[0.0],
[0.0]])
Omega += matrix([[1.0, -1.0, 0.0],
[-1.0, 1.0, 0.0],
[0.0, 0.0, 0.0]])
Xi += matrix([[-move1],
[move1],
[0.0]])
Omega += matrix([[0.0, 0.0, 0.0],
[0.0, 1.0, -1.0],
[0.0, -1.0, 1.0]])
Xi += matrix([[0.0],
[-move2],
[move2]])
Omega = Omega.expand(4, 4, [0, 1, 2], [0, 1, 2])
Xi = Xi.expand(4, 1, [0, 1, 2], [0])
Omega += matrix([[1.0, 0.0, 0.0, -1.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
[-1.0, 0.0, 0.0, 1.0]])
Xi += matrix([[-Z0],
[0.0],
[0.0],
[Z0]])
Omega += matrix([[0.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, -1.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 1.0]])
Xi += matrix([[0.0],
[-Z1],
[0.0],
[Z1]])
Omega += matrix([[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 5.0, -5.0],
[0.0, 0.0, -5.0, 5.0]])
Xi += matrix([[0.0],
[0.0],
[-5*Z2],
[5*Z2]])
Omega.show('Omega: ')
Xi.show('Xi: ')
mu = Omega.inverse() * Xi
mu.show('Mu: ')
return mu
doit(-3, 5, 3, 10, 5, 1)
Raw
21_inexact_move_function.py
#!/usr/bin/env python
#Modify the move function to accommodate the added
#probabilities of overshooting or undershooting
#the intended destination.
p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
def move(p, U):
q = [0] * len(p)
for i in range(len(p)):
q[(i-1)%len(p)] += p[(i-U)%len(p)] * pUndershoot
q[i] += p[(i-U)%len(p)] * pExact
q[(i+1)%len(p)] += p[(i-U)%len(p)] * pOvershoot
return q
print move(p, 1)
Raw
22_implementing_slam.py
#!/usr/bin/env python
# ------------
# User Instructions
#
# In this problem you will implement SLAM in a 2 dimensional
# world. Please define a function, slam, which takes five
# parameters as input and returns the vector mu. This vector
# should have x, y coordinates interlaced, so for example,
# if there were 2 poses and 2 landmarks, mu would look like:
#
# mu = matrix([[Px0],
# [Py0],
# [Px1],
# [Py1],
# [Lx0],
# [Ly0],
# [Lx1],
# [Ly1]])
#
# data - This is the data that is generated with the included
# make_data function. You can also use test_data to
# make sure your function gives the correct result.
#
# N - The number of time steps.
#
# num_landmarks - The number of landmarks.
#
# motion_noise - The noise associated with motion. The update
# strength for motion should be 1.0 / motion_noise.
#
# measurement_noise - The noise associated with measurement.
# The update strength for measurement should be
# 1.0 / measurement_noise.
#
#
# Enter your code at line 509
# --------------
# Testing
#
# Uncomment the test cases at the bottom of this document.
# Your output should be identical to the given results.
from math import *
import random
#===============================================================
#
# SLAM in a rectolinear world (we avoid non-linearities)
#
#
#===============================================================
# ------------------------------------------------
#
# this is the matrix class
# we use it because it makes it easier to collect constraints in GraphSLAM
# and to calculate solutions (albeit inefficiently)
#
class matrix:
# implements basic operations of a matrix class
# ------------
#
# initialization - can be called with an initial matrix
#
def __init__(self, value = [[]]):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
# ------------
#
# makes matrix of a certain size and sets each element to zero
#
def zero(self, dimx, dimy):
if dimy == 0:
dimy = dimx
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0.0 for row in range(dimy)] for col in range(dimx)]
# ------------
#
# makes matrix of a certain (square) size and turns matrix into identity matrix
#
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0.0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1.0
# ------------
#
# prints out values of matrix
#
def show(self, txt = ''):
for i in range(len(self.value)):
print txt + '['+ ', '.join('%.3f'%x for x in self.value[i]) + ']'
print ' '
# ------------
#
# defines elmement-wise matrix addition. Both matrices must be of equal dimensions
#
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to add"
else:
# add if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
# ------------
#
# defines elmement-wise matrix subtraction. Both matrices must be of equal dimensions
#
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimx != other.dimx:
raise ValueError, "Matrices must be of equal dimension to subtract"
else:
# subtract if correct dimensions
res = matrix()
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
# ------------
#
# defines multiplication. Both matrices must be of fitting dimensions
#
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# multiply if correct dimensions
res = matrix()
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
# ------------
#
# returns a matrix transpose
#
def transpose(self):
# compute transpose
res = matrix()
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[ 1, 2, 3, 4, 5],
# [ 6, 7, 8, 9, 10],
# [11, 12, 13, 14, 15]])
#
# l.take([0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 3, 4],
# [11, 13, 14]]
#
#
# take is used to remove rows and columns from existing matrices
# list1/list2 define a sequence of rows/columns that shall be taken
# is no list2 is provided, then list2 is set to list1 (good for
# symmetric matrices)
#
def take(self, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in take()"
res = matrix()
res.zero(len(list1), len(list2))
for i in range(len(list1)):
for j in range(len(list2)):
res.value[i][j] = self.value[list1[i]][list2[j]]
return res
# ------------
#
# creates a new matrix from the existing matrix elements.
#
# Example:
# l = matrix([[1, 2, 3],
# [4, 5, 6]])
#
# l.expand(3, 5, [0, 2], [0, 2, 3])
#
# results in:
#
# [[1, 0, 2, 3, 0],
# [0, 0, 0, 0, 0],
# [4, 0, 5, 6, 0]]
#
# expand is used to introduce new rows and columns into an existing matrix
# list1/list2 are the new indexes of row/columns in which the matrix
# elements are being mapped. Elements for rows and columns
# that are not listed in list1/list2
# will be initialized by 0.0.
#
def expand(self, dimx, dimy, list1, list2 = []):
if list2 == []:
list2 = list1
if len(list1) > self.dimx or len(list2) > self.dimy:
raise ValueError, "list invalid in expand()"
res = matrix()
res.zero(dimx, dimy)
for i in range(len(list1)):
for j in range(len(list2)):
res.value[list1[i]][list2[j]] = self.value[i][j]
return res
# ------------
#
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
#
def Cholesky(self, ztol= 1.0e-5):
res = matrix()
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(i)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
# ------------
#
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
# This code is based on http://adorio-research.org/wordpress/?p=4560
#
def CholeskyInverse(self):
res = matrix()
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/ tjj**2 - S/ tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = \
-sum([self.value[i][k]*res.value[k][j] for k in \
range(i+1,self.dimx)])/self.value[i][i]
return res
# ------------
#
# comutes and returns the inverse of a square matrix
#
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
# ------------
#
# prints matrix (needs work!)
#
def __repr__(self):
return repr(self.value)
# ------------------------------------------------
#
# this is the robot class
#
# our robot lives in x-y space, and its motion is
# pointed in a random direction. It moves on a straight line
# until is comes close to a wall at which point it turns
# away from the wall and continues to move.
#
# For measurements, it simply senses the x- and y-distance
# to landmarks. This is different from range and bearing as
# commonly studied in the literature, but this makes it much
# easier to implement the essentials of SLAM without
# cluttered math
#
class robot:
# --------
# init:
# creates robot and initializes location to 0, 0
#
def __init__(self, world_size = 100.0, measurement_range = 30.0,
motion_noise = 1.0, measurement_noise = 1.0):
self.measurement_noise = 0.0
self.world_size = world_size
self.measurement_range = measurement_range
self.x = world_size / 2.0
self.y = world_size / 2.0
self.motion_noise = motion_noise
self.measurement_noise = measurement_noise
self.landmarks = []
self.num_landmarks = 0
def rand(self):
return random.random() * 2.0 - 1.0
# --------
#
# make random landmarks located in the world
#
def make_landmarks(self, num_landmarks):
self.landmarks = []
for i in range(num_landmarks):
self.landmarks.append([round(random.random() * self.world_size),
round(random.random() * self.world_size)])
self.num_landmarks = num_landmarks
# --------
#
# move: attempts to move robot by dx, dy. If outside world
# boundary, then the move does nothing and instead returns failure
#
def move(self, dx, dy):
x = self.x + dx + self.rand() * self.motion_noise
y = self.y + dy + self.rand() * self.motion_noise
if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size:
return False
else:
self.x = x
self.y = y
return True
# --------
#
# sense: returns x- and y- distances to landmarks within visibility range
# because not all landmarks may be in this range, the list of measurements
# is of variable length. Set measurement_range to -1 if you want all
# landmarks to be visible at all times
#
def sense(self):
Z = []
for i in range(self.num_landmarks):
dx = self.landmarks[i][0] - self.x + self.rand() * self.measurement_noise
dy = self.landmarks[i][1] - self.y + self.rand() * self.measurement_noise
if self.measurement_range < 0.0 or abs(dx) + abs(dy) <= self.measurement_range:
Z.append([i, dx, dy])
return Z
# --------
#
# print robot location
#
def __repr__(self):
return 'Robot: [x=%.5f y=%.5f]' % (self.x, self.y)
######################################################
# --------
# this routine makes the robot data
#
def make_data(N, num_landmarks, world_size, measurement_range, motion_noise,
measurement_noise, distance):
complete = False
while not complete:
data = []
# make robot and landmarks
r = robot(world_size, measurement_range, motion_noise, measurement_noise)
r.make_landmarks(num_landmarks)
seen = [False for row in range(num_landmarks)]
# guess an initial motion
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
for k in range(N-1):
# sense
Z = r.sense()
# check off all landmarks that were observed
for i in range(len(Z)):
seen[Z[i][0]] = True
# move
while not r.move(dx, dy):
# if we'd be leaving the robot world, pick instead a new direction
orientation = random.random() * 2.0 * pi
dx = cos(orientation) * distance
dy = sin(orientation) * distance
# memorize data
data.append([Z, [dx, dy]])
# we are done when all landmarks were observed; otherwise re-run
complete = (sum(seen) == num_landmarks)
print ' '
print 'Landmarks: ', r.landmarks
print r
return data
####################################################
# --------------------------------
#
# print the result of SLAM, the robot pose(s) and the landmarks
#
def print_result(N, num_landmarks, result):
print
print 'Estimated Pose(s):'
for i in range(N):
print ' ['+ ', '.join('%.3f'%x for x in result.value[2*i]) + ', ' \
+ ', '.join('%.3f'%x for x in result.value[2*i+1]) +']'
print
print 'Estimated Landmarks:'
for i in range(num_landmarks):
print ' ['+ ', '.join('%.3f'%x for x in result.value[2*(N+i)]) + ', ' \
+ ', '.join('%.3f'%x for x in result.value[2*(N+i)+1]) +']'
# --------------------------------
#
# slam - retains entire path and all landmarks
#
############## ENTER YOUR CODE BELOW HERE ###################
def slam(data, N, num_landmarks, motion_noise, measurement_noise):
#
#
# Add your code here!
#
#
dim = 2 * (N + num_landmarks)
Omega = matrix()
Omega.zero(dim, dim)
Omega.value[0][0] = 1.0
Omega.value[1][1] = 1.0
Xi = matrix()
Xi.zero(dim, 1)
Xi.value[0][0] = world_size / 2.0
Xi.value[1][0] = world_size / 2.0
for k in range(len(data)):
# n is the index of the robot pose in matrix/vector
n = k * 2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in matrix/vector
m = 2 * (N + measurement[i][0])
# update the information matrix/vector based on the measurement
for b in range(2):
Omega.value[n+b][n+b] += 1.0 / measurement_noise
Omega.value[m+b][m+b] += 1.0 / measurement_noise
Omega.value[n+b][m+b] += -1.0 / measurement_noise
Omega.value[m+b][n+b] += 1.0 / measurement_noise
Xi.value[n+b][0] += -measurement[i][1+b] / measurement_noise
Xi.value[m+b][0] += measurement[i][1+b] / measurement_noise
# update the information matrix/vector based on the motion
for b in range(4):
Omega.value[n+b][n+b] += 1.0 / motion_noise
for b in range(2):
Omega.value[n+b ][n+b+2] += -1.0 / motion_noise
Omega.value[n+b+2][n+b ] += -1.0 / motion_noise
Xi.value[n+b ][0] += -motion[b] / motion_noise
Xi.value[n+b+2][0] += motion[b] / motion_noise
# compute best estimate
mu = Omega.inverse() * Xi
return mu # Make sure you return mu for grading!
############### ENTER YOUR CODE ABOVE HERE ###################
# ------------------------------------------------------------------------
# ------------------------------------------------------------------------
# ------------------------------------------------------------------------
#
# Main routines
#
num_landmarks = 5 # number of landmarks
N = 20 # time steps
world_size = 100.0 # size of world
measurement_range = 50.0 # range at which we can sense landmarks
motion_noise = 2.0 # noise in robot motion
measurement_noise = 2.0 # noise in the measurements
distance = 20.0 # distance by which robot (intends to) move each iteratation
data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
result = slam(data, N, num_landmarks, motion_noise, measurement_noise)
print_result(N, num_landmarks, result)
# -------------
# Testing
#
# Uncomment one of the test cases below to compare your results to
# the results shown for Test Case 1 and Test Case 2.
test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]]
test_data2 = [[[[0, 26.543274387283322, -6.262538160312672], [3, 9.937396825799755, -9.128540360867689]], [18.92765331253674, -6.460955043986683]], [[[0, 7.706544739722961, -3.758467215445748], [1, 17.03954411948937, 31.705489938553438], [3, -11.61731288777497, -6.64964096716416]], [18.92765331253674, -6.460955043986683]], [[[0, -12.35130507136378, 2.585119104239249], [1, -2.563534536165313, 38.22159657838369], [3, -26.961236804740935, -0.4802312626141525]], [-11.167066095509824, 16.592065417497455]], [[[0, 1.4138633151721272, -13.912454837810632], [1, 8.087721200818589, 20.51845934354381], [3, -17.091723454402302, -16.521500551709707], [4, -7.414211721400232, 38.09191602674439]], [-11.167066095509824, 16.592065417497455]], [[[0, 12.886743222179561, -28.703968411636318], [1, 21.660953298391387, 3.4912891084614914], [3, -6.401401414569506, -32.321583037341625], [4, 5.034079343639034, 23.102207946092893]], [-11.167066095509824, 16.592065417497455]], [[[1, 31.126317672358578, -10.036784369535214], [2, -38.70878528420893, 7.4987265861424595], [4, 17.977218575473767, 6.150889254289742]], [-6.595520680493778, -18.88118393939265]], [[[1, 41.82460922922086, 7.847527392202475], [3, 15.711709540417502, -30.34633659912818]], [-6.595520680493778, -18.88118393939265]], [[[0, 40.18454208294434, -6.710999804403755], [3, 23.019508919299156, -10.12110867290604]], [-6.595520680493778, -18.88118393939265]], [[[3, 27.18579315312821, 8.067219022708391]], [-6.595520680493778, -18.88118393939265]], [[], [11.492663265706092, 16.36822198838621]], [[[3, 24.57154567653098, 13.461499960708197]], [11.492663265706092, 16.36822198838621]], [[[0, 31.61945290413707, 0.4272295085799329], [3, 16.97392299158991, -5.274596836133088]], [11.492663265706092, 16.36822198838621]], [[[0, 22.407381798735177, -18.03500068379259], [1, 29.642444125196995, 17.3794951934614], [3, 4.7969752441371645, -21.07505361639969], [4, 14.726069092569372, 32.75999422300078]], [11.492663265706092, 16.36822198838621]], [[[0, 10.705527984670137, -34.589764174299596], [1, 18.58772336795603, -0.20109708164787765], [3, -4.839806195049413, -39.92208742305105], [4, 4.18824810165454, 14.146847823548889]], [11.492663265706092, 16.36822198838621]], [[[1, 5.878492140223764, -19.955352450942357], [4, -7.059505455306587, -0.9740849280550585]], [19.628527845173146, 3.83678180657467]], [[[1, -11.150789592446378, -22.736641053247872], [4, -28.832815721158255, -3.9462962046291388]], [-19.841703647091965, 2.5113335861604362]], [[[1, 8.64427397916182, -20.286336970889053], [4, -5.036917727942285, -6.311739993868336]], [-5.946642674882207, -19.09548221169787]], [[[0, 7.151866679283043, -39.56103232616369], [1, 16.01535401373368, -3.780995345194027], [4, -3.04801331832137, 13.697362774960865]], [-5.946642674882207, -19.09548221169787]], [[[0, 12.872879480504395, -19.707592098123207], [1, 22.236710716903136, 16.331770792606406], [3, -4.841206109583004, -21.24604435851242], [4, 4.27111163223552, 32.25309748614184]], [-5.946642674882207, -19.09548221169787]]]
## Test Case 1
##
## Estimated Pose(s):
## [49.999, 49.999]
## [37.971, 33.650]
## [26.183, 18.153]
## [13.743, 2.114]
## [28.095, 16.781]
## [42.383, 30.900]
## [55.829, 44.494]
## [70.855, 59.697]
## [85.695, 75.540]
## [74.010, 92.431]
## [53.543, 96.451]
## [34.523, 100.078]
## [48.621, 83.951]
## [60.195, 68.105]
## [73.776, 52.932]
## [87.130, 38.536]
## [80.301, 20.506]
## [72.797, 2.943]
## [55.244, 13.253]
## [37.414, 22.315]
##
## Estimated Landmarks:
## [82.954, 13.537]
## [70.493, 74.139]
## [36.738, 61.279]
## [18.696, 66.057]
## [20.633, 16.873]
## Test Case 2
##
## Estimated Pose(s):
## [49.999, 49.999]
## [69.180, 45.664]
## [87.742, 39.702]
## [76.269, 56.309]
## [64.316, 72.174]
## [52.256, 88.151]
## [44.058, 69.399]
## [37.001, 49.916]
## [30.923, 30.953]
## [23.507, 11.417]
## [34.179, 27.131]
## [44.154, 43.844]
## [54.805, 60.919]
## [65.697, 78.544]
## [77.467, 95.624]
## [96.801, 98.819]
## [75.956, 99.969]
## [70.199, 81.179]
## [64.053, 61.721]
## [58.106, 42.626]
##
## Estimated Landmarks:
## [76.778, 42.885]
## [85.064, 77.436]
## [13.546, 95.649]
## [59.448, 39.593]
## [69.262, 94.238]
### Uncomment the following three lines for test case 1 ###
#result = slam(test_data1, 20, 5, 2.0, 2.0)
#print_result(20, 5, result)
#print result
### Uncomment the following three lines for test case 2 ###
#result = slam(test_data2, 20, 5, 2.0, 2.0)
#print_result(20, 5, result)
#print result
Raw
22_new_mean_variance.py
#!/usr/bin/env python
# Write a program to update your mean and variance
# when given the mean and variance of your belief
# and the mean and variance of your measurement.
# This program will update the parameters of your
# belief function.
def update(mean1, var1, mean2, var2):
new_mean = (mean1*var2 + mean2*var1)/(var1+var2)
new_var = 1/(1/var1 + 1/var2)
return [new_mean, new_var]
print update(10.,8.,13., 2.)
Raw
22_orientation.py
#!/usr/bin/env python
# In this exercise, write a program that will
# run your previous code twice.
# Please only modify the indicated area below!
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i in range(N):
w.append(p[i].measurement_prob(Z))
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
p = []
for i in range(N):
x = robot()
x.set_noise(0.05, 0.05, 5.0)
p.append(x)
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i in range(N):
w.append(p[i].measurement_prob(Z))
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
print p #Leave this print statement for grading purposes!
Raw
23_error.py
#!/usr/bin/env python
# In this exercise, please run your previous code twice.
# Please only modify the indicated area below!
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 1000
T = 10 #Leave this as 10 for grading purposes.
p = []
for i in range(N):
r = robot()
r.set_noise(0.05, 0.05, 5.0)
p.append(r)
for t in range(T):
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
p2 = []
for i in range(N):
p2.append(p[i].move(0.1, 5.0))
p = p2
w = []
for i in range(N):
w.append(p[i].measurement_prob(Z))
p3 = []
index = int(random.random() * N)
beta = 0.0
mw = max(w)
for i in range(N):
beta += random.random() * 2.0 * mw
while beta > w[index]:
beta -= w[index]
index = (index + 1) % N
p3.append(p[index])
p = p3
#enter code here, make sure that you output 10 print statements.
print eval(myrobot, p)
Raw
23_move_twice.py
#!/usr/bin/env python
#Write code that makes the robot move twice and then prints
#out the resulting distribution, starting with the initial
#distribution p = [0, 1, 0, 0, 0]
p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
def move(p, U):
q = []
for i in range(len(p)):
s = pExact * p[(i-U) % len(p)]
s = s + pOvershoot * p[(i-U-1) % len(p)]
s = s + pUndershoot * p[(i-U+1) % len(p)]
q.append(s)
return q
#
# ADD CODE HERE
#
print move(move(p,1), 1)
Raw
24_move_1000.py
#!/usr/bin/env python
#write code that moves 1000 times and then prints the
#resulting probability distribution.
p=[0, 1, 0, 0, 0]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
def move(p, U):
q = []
for i in range(len(p)):
s = pExact * p[(i-U) % len(p)]
s = s + pOvershoot * p[(i-U-1) % len(p)]
s = s + pUndershoot * p[(i-U+1) % len(p)]
q.append(s)
return q
#
# ADD CODE HERE
#
for _ in range(1000):
p = move(p, 1)
print p
Raw
24_predict_function.py
#!/usr/bin/env python
# Write a program that will predict your new mean
# and variance given the mean and variance of your
# prior belief and the mean and variance of your
# motion.
def update(mean1, var1, mean2, var2):
new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
new_var = 1/(1/var1 + 1/var2)
return [new_mean, new_var]
def predict(mean1, var1, mean2, var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
print predict(10., 4., 12., 4.)
Raw
25_kalman_filter_code.py
#!/usr/bin/env python
# Write a program that will iteratively update and
# predict based on the location measurements
# and inferred motions shown below.
def update(mean1, var1, mean2, var2):
new_mean = (var2 * mean1 + var1 * mean2) / (var1 + var2)
new_var = 1/(1/var1 + 1/var2)
return [new_mean, new_var]
def predict(mean1, var1, mean2, var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
measurements = [5., 6., 7., 9., 10.]
motion = [1., 1., 2., 1., 1.]
measurement_sig = 4.
motion_sig = 2.
mu = 0
sig = 10000
#Please print out ONLY the final values of the mean
#and the variance in a list [mu, sig].
# Insert code here
for ms, mt in zip(measurements, motion):
mu, sig = update(mu, sig, ms, measurement_sig)
mu, sig = predict(mu, sig, mt, motion_sig)
print [mu, sig]
Raw
25_sense_and_move.py
#!/usr/bin/env python
#Given the list motions=[1,1] which means the robot
#moves right and then right again, compute the posterior
#distribution if the robot first senses red, then moves
#right one, then senses green, then moves right again,
#starting with a uniform prior distribution.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'green']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
def move(p, U):
q = []
for i in range(len(p)):
s = pExact * p[(i-U) % len(p)]
s = s + pOvershoot * p[(i-U-1) % len(p)]
s = s + pUndershoot * p[(i-U+1) % len(p)]
q.append(s)
return q
for k in range(len(motions)):
p = sense(p, measurements[k])
p = move(p, motions[k])
print p
Raw
26_sense_and_move.py
#!/usr/bin/env python
#Modify the previous code so that the robot senses red twice.
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
measurements = ['red', 'red']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
for i in range(len(q)):
q[i] = q[i] / s
return q
def move(p, U):
q = []
for i in range(len(p)):
s = pExact * p[(i-U) % len(p)]
s = s + pOvershoot * p[(i-U-1) % len(p)]
s = s + pUndershoot * p[(i-U+1) % len(p)]
q.append(s)
return q
for k in range(len(measurements)):
p = sense(p, measurements[k])
p = move(p, motions[k])
print p
Raw
32_kalman_matrices.py
#!/usr/bin/env python
# Write a function 'filter' that implements a multi-
# dimensional Kalman Filter for the example given
from math import *
class matrix:
# implements basic operations of a matrix class
def __init__(self, value):
self.value = value
self.dimx = len(value)
self.dimy = len(value[0])
if value == [[]]:
self.dimx = 0
def zero(self, dimx, dimy):
# check if valid dimensions
if dimx < 1 or dimy < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dimx
self.dimy = dimy
self.value = [[0 for row in range(dimy)] for col in range(dimx)]
def identity(self, dim):
# check if valid dimension
if dim < 1:
raise ValueError, "Invalid size of matrix"
else:
self.dimx = dim
self.dimy = dim
self.value = [[0 for row in range(dim)] for col in range(dim)]
for i in range(dim):
self.value[i][i] = 1
def show(self):
for i in range(self.dimx):
print self.value[i]
print ' '
def __add__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimensions to add"
else:
# add if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] + other.value[i][j]
return res
def __sub__(self, other):
# check if correct dimensions
if self.dimx != other.dimx or self.dimy != other.dimy:
raise ValueError, "Matrices must be of equal dimensions to subtract"
else:
# subtract if correct dimensions
res = matrix([[]])
res.zero(self.dimx, self.dimy)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[i][j] = self.value[i][j] - other.value[i][j]
return res
def __mul__(self, other):
# check if correct dimensions
if self.dimy != other.dimx:
raise ValueError, "Matrices must be m*n and n*p to multiply"
else:
# subtract if correct dimensions
res = matrix([[]])
res.zero(self.dimx, other.dimy)
for i in range(self.dimx):
for j in range(other.dimy):
for k in range(self.dimy):
res.value[i][j] += self.value[i][k] * other.value[k][j]
return res
def transpose(self):
# compute transpose
res = matrix([[]])
res.zero(self.dimy, self.dimx)
for i in range(self.dimx):
for j in range(self.dimy):
res.value[j][i] = self.value[i][j]
return res
# Thanks to Ernesto P. Adorio for use of Cholesky and CholeskyInverse functions
def Cholesky(self, ztol=1.0e-5):
# Computes the upper triangular Cholesky factorization of
# a positive definite matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
for i in range(self.dimx):
S = sum([(res.value[k][i])**2 for k in range(i)])
d = self.value[i][i] - S
if abs(d) < ztol:
res.value[i][i] = 0.0
else:
if d < 0.0:
raise ValueError, "Matrix not positive-definite"
res.value[i][i] = sqrt(d)
for j in range(i+1, self.dimx):
S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
if abs(S) < ztol:
S = 0.0
res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
return res
def CholeskyInverse(self):
# Computes inverse of matrix given its Cholesky upper Triangular
# decomposition of matrix.
res = matrix([[]])
res.zero(self.dimx, self.dimx)
# Backward step for inverse.
for j in reversed(range(self.dimx)):
tjj = self.value[j][j]
S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
res.value[j][j] = 1.0/tjj**2 - S/tjj
for i in reversed(range(j)):
res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
return res
def inverse(self):
aux = self.Cholesky()
res = aux.CholeskyInverse()
return res
def __repr__(self):
return repr(self.value)
########################################
# Implement the filter function below
def filter(x, P):
for n in range(len(measurements)):
# measurement update
y = matrix([[measurements[n]]]) - H*x
s = H * P * H.transpose() + R
k = P * H.transpose() * s.inverse()
x = x + k*y
P = (I - k*H) * P
# prediction
x = F*x + u
P = F * P * F.transpose()
print 'x= '
x.show()
print 'P= '
P.show()
########################################
measurements = [1, 2, 3]
x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
filter(x, P)
Raw
README
Udacity cs373 code
Raw
q5_particle_filters.py
#!/usr/bin/env python
import random
cnt, total = 0, 0
for a in range(0,13):
for b in range(0,13):
for c in range(0,13):
for d in range(0,13):
if a+b+c+d==12:
total += 1
if a and b and c and d:
cnt +=1
print cnt, total, 1.8*cnt/total
for _ in range(100000):
x = [0, 0, 0, 0]
for _ in range(12):
x[random.randint(0,3)] += 1
total += 1
if all(a>0 for a in x):
cnt += 1
print cnt, total, 1.0*cnt/total
Raw
q5_robot.py
#!/usr/bin/env python
# In this exercise, please run your previous code twice.
# Please only modify the indicated area below!
from math import *
import random
landmarks = [[20.0, 20.0], [80.0, 80.0], [20.0, 80.0], [80.0, 20.0]]
world_size = 100.0
class robot:
def __init__(self):
self.x = random.random() * world_size
self.y = random.random() * world_size
self.orientation = random.random() * 2.0 * pi
self.forward_noise = 0.0;
self.turn_noise = 0.0;
self.sense_noise = 0.0;
def set(self, new_x, new_y, new_orientation):
if new_x < 0 or new_x >= world_size:
raise ValueError, 'X coordinate out of bound'
if new_y < 0 or new_y >= world_size:
raise ValueError, 'Y coordinate out of bound'
if new_orientation < 0 or new_orientation >= 2 * pi:
raise ValueError, 'Orientation must be in [0..2pi]'
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation)
def set_noise(self, new_f_noise, new_t_noise, new_s_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.forward_noise = float(new_f_noise);
self.turn_noise = float(new_t_noise);
self.sense_noise = float(new_s_noise);
def sense(self):
Z = []
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
dist += random.gauss(0.0, self.sense_noise)
Z.append(dist)
return Z
def move(self, turn, forward):
if forward < 0:
raise ValueError, 'Robot cant move backwards'
# turn, and add randomness to the turning command
orientation = self.orientation + float(turn) + random.gauss(0.0, self.turn_noise)
orientation %= 2 * pi
# move, and add randomness to the motion command
dist = float(forward) + random.gauss(0.0, self.forward_noise)
x = self.x + (cos(orientation) * dist)
y = self.y + (sin(orientation) * dist)
x %= world_size # cyclic truncate
y %= world_size
# set particle
res = robot()
res.set(x, y, orientation)
res.set_noise(self.forward_noise, self.turn_noise, self.sense_noise)
return res
def Gaussian(self, mu, sigma, x):
# calculates the probability of x for 1-dim Gaussian with mean mu and var. sigma
return exp(- ((mu - x) ** 2) / (sigma ** 2) / 2.0) / sqrt(2.0 * pi * (sigma ** 2))
def measurement_prob(self, measurement):
# calculates how likely a measurement should be
prob = 1.0;
for i in range(len(landmarks)):
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])
return prob
def __repr__(self):
return '[x=%.6s y=%.6s orient=%.6s]' % (str(self.x), str(self.y), str(self.orientation))
def eval(r, p):
sum = 0.0;
for i in range(len(p)): # calculate mean error
dx = (p[i].x - r.x + (world_size/2.0)) % world_size - (world_size/2.0)
dy = (p[i].y - r.y + (world_size/2.0)) % world_size - (world_size/2.0)
err = sqrt(dx * dx + dy * dy)
sum += err
return sum / float(len(p))
#myrobot = robot()
#myrobot.set_noise(5.0, 0.1, 5.0)
#myrobot.set(30.0, 50.0, pi/2)
#myrobot = myrobot.move(-pi/2, 15.0)
#print myrobot.sense()
#myrobot = myrobot.move(-pi/2, 10.0)
#print myrobot.sense()
#### DON'T MODIFY ANYTHING ABOVE HERE! ENTER/MODIFY CODE BELOW ####
myrobot = robot()
myrobot = myrobot.move(0.1, 5.0)
Z = myrobot.sense()
N = 12
Raw
q7_programming.py
#!/usr/bin/env python
# -------------------
# Background Information
#
# In this problem, you will build a planner that makes a robot
# car's lane decisions. On a highway, the left lane (in the US)
# generally has a higher traffic speed than the right line.
#
# In this problem, a 2 lane highway of length 5 could be
# represented as:
#
# road = [[80, 80, 80, 80, 80],
# [60, 60, 60, 60, 60]]
#
# In this case, the left lane has an average speed of 80 km/h and
# the right lane has a speed of 60 km/h. We can use a 0 to indicate
# an obstacle in the road.
#
# To get to a location as quickly as possible, we usually
# want to be in the left lane. But there is a cost associated
# with changing lanes. This means that for short trips, it is
# sometimes optimal to stay in the right lane.
#
# -------------------
# User Instructions
#
# Design a planner (any kind you like, so long as it works).
# This planner should be a function named plan() that takes
# as input four parameters: road, lane_change_cost, init, and
# goal. See parameter info below.
#
# Your function should RETURN the final cost to reach the
# goal from the start point (which should match with our answer).
# You may include print statements to show the optimum policy,
# though this is not necessary for grading.
#
# Your solution must work for a variety of roads and lane
# change costs.
#
# Add your code at line 92.
#
# --------------------
# Parameter Info
#
# road - A grid of values. Each value represents the speed associated
# with that cell. A value of 0 means the cell in non-navigable.
# The cost for traveling in a cell must be (1.0 / speed).
#
# lane_change_cost - The cost associated with changing lanes.
#
# init - The starting point for your car. This will always be somewhere
# in the right (bottom) lane to simulate a highway on-ramp.
#
# goal - The destination. This will always be in the right lane to
# simulate a highway exit-ramp.
#
# --------------------
# Testing
#
# You may use our test function below, solution_check
# to test your code for a variety of input parameters.
#
# You may also use the build_road function to build
# your own roads to test your function with.
import random
# ------------------------------------------
# build_road - Makes a road according to your specified length and
# lane_speeds. lane_speeds is a list of speeds for the lanes (listed
# from left lane to right). You can also include random obstacles.
#
def build_road(length, lane_speeds, print_flag = False, obstacles = False, obstacle_prob = 0.05):
num_lanes = len(lane_speeds)
road = [[lane_speeds[i] for dist in range(length)] for i in range(len(lane_speeds))]
if obstacles:
for x in range(len(road)):
for y in range(len(road[0])):
if random.random() < obstacle_prob:
road[x][y] = 0
if print_flag:
for lane in road:
print '[' + ', '.join('%5.3f' % speed for speed in lane) + ']'
return road
# ------------------------------------------
# plan - Returns cost to get from init to goal on road given a
# lane_change_cost.
#
def plan(road, lane_change_cost, init, goal): # Don't change the name of this function!
#
#
# Insert Code Here
#
#
lanes_count = len(road)
costs = [100000 for _ in range(lanes_count)]
costs[init[0]] = 1.0/road[init[0]][init[1]]
for road_pos in range(init[1]+1, goal[1]):
new_costs = [0 for _ in range(lanes_count)]
for lane in range(lanes_count):
new_cost = []
if not road[lane][road_pos]:
new_costs[lane] = 1000000
else:
if road[lane][road_pos]:
new_cost.append(1.0/road[lane][road_pos]+costs[lane])
if lane-1 >= 0:
new_cost.append(1.0/road[lane][road_pos]+costs[lane-1]+lane_change_cost)
if lane+1 < lanes_count:
new_cost.append(1.0/road[lane][road_pos]+costs[lane+1]+lane_change_cost)
else:
new_cost.append(100000)
new_costs[lane] = min(new_cost) if new_cost else 100000000
costs = new_costs
possible_costs = []
if road[goal[0]][goal[1]]:
possible_costs.append(costs[goal[0]])
if goal[0]-1 >= 0:
possible_costs.append(costs[goal[0]-1]+ lane_change_cost)
if goal[0]+1 < lanes_count:
possible_costs.append(costs[goal[0]+1] + lane_change_cost)
cost = min(possible_costs)
return cost
################# TESTING ##################
# ------------------------------------------
# solution check - Checks your path function using
# data from list called test[]. Uncomment the call
# to solution_check at the bottom to test your code.
#
def solution_check(test, epsilon = 0.00001):
answer_list = []
for i in range(len(test[0])):
user_cost = plan(test[0][i], test[1][i], test[2][i], test[3][i])
true_cost = test[4][i]
if abs(user_cost - true_cost) < epsilon:
answer_list.append(1)
else:
answer_list.append(0)
correct_answers = 0
print
for i in range(len(answer_list)):
if answer_list[i] == 1:
print 'Test case', i+1, 'passed!'
correct_answers += 1
else:
print 'Test case', i+1, 'failed.'
if correct_answers == len(answer_list):
print "\nYou passed all test cases!"
return True
else:
print "\nYou passed", correct_answers, "of", len(answer_list), "test cases. Try to get them all!"
return False
# Test Case 1 (FAST left lane)
test_road1 = build_road(8, [100, 10, 1])
lane_change_cost1 = 1.0 / 1000.0
test_init1 = [len(test_road1) - 1, 0]
test_goal1 = [len(test_road1) - 1, len(test_road1[0]) - 1]
true_cost1 = 1.244
# Test Case 2 (more realistic road)
test_road2 = build_road(14, [80, 60, 40, 20])
lane_change_cost2 = 1.0 / 100.0
test_init2 = [len(test_road2) - 1, 0]
test_goal2 = [len(test_road2) - 1, len(test_road2[0]) - 1]
true_cost2 = 0.293333333333
# Test Case 3 (Obstacles included)
test_road3 = [[50, 50, 50, 50, 50, 40, 0, 40, 50, 50, 50, 50, 50, 50, 50], # left lane: 50 km/h
[40, 40, 40, 40, 40, 30, 20, 30, 40, 40, 40, 40, 40, 40, 40],
[30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]] # right lane: 30 km/h
lane_change_cost3 = 1.0 / 500.0
test_init3 = [len(test_road3) - 1, 0]
test_goal3 = [len(test_road3) - 1, len(test_road3[0]) - 1]
true_cost3 = 0.355333333333
# Test Case 4 (Slalom)
test_road4 = [[50, 50, 50, 50, 50, 40, 0, 40, 50, 50, 0, 50, 50, 50, 50], # left lane: 50 km/h
[40, 40, 40, 40, 0, 30, 20, 30, 0, 40, 40, 40, 40, 40, 40],
[30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]] # right lane: 30 km/h
lane_change_cost4 = 1.0 / 65.0
test_init4 = [len(test_road4) - 1, 0]
test_goal4 = [len(test_road4) - 1, len(test_road4[0]) - 1]
true_cost4 = 0.450641025641
testing_suite = [[test_road1, test_road2, test_road3, test_road4],
[lane_change_cost1, lane_change_cost2, lane_change_cost3, lane_change_cost4],
[test_init1, test_init2, test_init3, test_init4],
[test_goal1, test_goal2, test_goal3, test_goal4],
[true_cost1, true_cost2, true_cost3, true_cost4]]
solution_check(testing_suite) #UNCOMMENT THIS LINE TO TEST YOUR CODE
@WolsYang
Copy link

Hi kmmbvnr,
I have a question at 06_segmented_cte.py 353-356
Why is sqrt(2.0 * pi * (self.measurement_noise ** 2)) at here?
I think in gaussian, here should be sqrt(2.0 * pi *) (self.measurement_noise ** 2)
Sorry to bother you, but I can't find the answer.

@kmmbvnr
Copy link
Author

kmmbvnr commented Apr 17, 2022

@WolsYang I'm afraid I would not be able to figure out, since it's over 10 years passed

@WolsYang
Copy link

@kmmbvnr Never mind : ), thank you for your reply.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment