课程2：Problem Set 1
课程2：Problem Set 1
2. 练习：Localization
平面环境中机器人的一般有3个坐标轴: (x,y,orientation) orientation 是面向的角度。
而立体空间中，飞行机器人，一般有6个坐标轴：（x,y,z, roll,pitch,yaw）
状态变量从3变化为6，使用基于直方图的定位，对于内存使用量来说，是指数变化的？
3. 练习：Bayes’ Rule
P(F)=0.001 失火的概率
B = 邻居说“失火了”
P(邻居撒谎的概率)=0.1
当邻居说“失火了”时，
真正起火的概率为 P1(F|B) = 0.0009
没有起火的概率为 P1(~F|B) = 0.0999
归一化后，P(F|B)=0.0089
P(~F|B)=0.991
4. 练习：Localization Program
二维移动的例子，代码如下：
# -*- coding: utf-8 -*-
# The function localize takes the following arguments:
#
# colors:
# 2D list, each entryeither 'R' (for red cell) or 'G' (for green cell)
#
# measurements:
# list of measurementstaken by the robot, each entry either 'R' or 'G'
#
# motions:
# list of actions takenby the robot, each entry of the form [dy,dx],
# where dx refers tothe change in the x-direction (positive meaning
# movement to theright) and dy refers to the change in the y-direction
# (positive meaningmovement downward)
# NOTE: the *first*coordinate is change in y; the *second* coordinate is
# change in x
#
# sensor_right:
# float between 0 and1, giving the probability that any given
# measurement iscorrect; the probability that the measurement is
# incorrect is1-sensor_right
#
# p_move:
# float between 0 and1, giving the probability that any given movement
# command takes place;the probability that the movement command fails
# (and the robotremains still) is 1-p_move; the robot will NOT overshoot
# its destination inthis exercise
#
# The function should RETURN (not just show or print) a 2D list (ofthe same
# dimensions as colors) that gives the probabilities that the robotoccupies
# each cell in the world.
#
# Compute the probabilities by assuming the robot initially has auniform
# probability of being in any cell.
#
# Also assume that at each step, the robot:
# 1) first makes a movement,
# 2) then takes a measurement.
#
# Motion:
# [0,0] - stay
# [0,1] - right
# [0,-1] - left
# [1,0] - down
# [-1,0] - up
def localize(colors,measurements,motions,sensor_right,p_move):
# initializes p to auniform distribution over a grid of the same dimensions as colors
pinit = 1.0 / float(len(colors))/ float(len(colors[0]))
p = [[pinit for row inrange(len(colors[0]))] for col in range(len(colors))]
# >>> Insert yourcode here <<<
if len(motions) !=len(measurements):
print("localize()meet a exception ! the size of motions and measurements should be equal.")
raise
#show(p)
for m inrange(len(motions)):
print('----enter forloop index {} move is [{}][{}]'.format(m,motions[m][0], motions[m][1]))
p = move(p,motions[m], p_move)
print('-------------------------begin to sense()')
p = sense(p, colors,measurements[m], sensor_right)
#show(p) # for debug
return p
def move(p, movements, p_move):
print('move() invoked')
q = [[0 for col inrange(len(p[0]))] for row in range(len(p))]
newMove = [0,0]
newMove[0] = movements[0]% len(p)
newMove[1] = movements[1]% len(p[0])
for r in range(len(p)) :
for c inrange(len(p[r])) :
# not move,just copy the p list
if newMove[0]== 0 and newMove[1] == 0:
q[r][c] =p[r][c]
continue
# move thefirst dimention, when move, do not change dimen
mx = r
if newMove[0]> 0 : # down
mx =(r-newMove[0])%len(p)
print("[{0}][{1}] move down\n".format(r,c))
elifnewMove[0] < 0 : # up
mx =(r+newMove[0])%len(p)
print("[{0}][{1}] move up\n".format(r,c))
# move thesecond dimention, when move, do not change dimen
my = c
if newMove[1]> 0 : # right
my =(c-newMove[1])%len(p[r])
print("[{0}][{1}] move right\n".format(r,c))
elifnewMove[1] < 0 : # left
my =(c+newMove[1])%len(p[r])
print("[{0}][{1}] move left\n".format(r,c))
q[r][c] = p[mx][my] * p_move + p[r][c] *(1-p_move)
#if q[r][c] !=0:
# print('qv = {}, qh = {}'.format(qv, qh))
#show(p)
#print('p---move to----q')
#show(q)
total = 0
for r in range(len(q)) :
total += sum(q[r])
for r in range(len(q)) :
for c inrange(len(q[r])) :
q[r][c] =q[r][c] / total
return q
def sense(p, colors, measureVal, sensor_right):
print('sense() invoked')
q = [[0 for col inrange(len(p[0]))] for row in range(len(p))]
for r in range(len(p)) :
for c inrange(len(p[r])) :
# sense
if measureVal ==colors[r][c] :
qrc = p[r][c]* sensor_right
print("sense [{0}][{1}] measure {2}\n".format(r,c,measureVal))
else :
qrc = p[r][c]* (1 - sensor_right)
q[r][c] = qrc
total = 0
for r in range(len(q)) :
total += sum(q[r])
for r in range(len(q)) :
for c inrange(len(q[r])) :
q[r][c] = q[r][c]/ total
return q
def show(p):
rows = ['[' +','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]
print('[' + ',\n'.join(rows) + ']')
#############################################################
# For the following test case, your output should be
# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],
# [0.00715, 0.01017, 0.08696,0.07988, 0.00935],
# [0.00739, 0.00894, 0.11272,0.35350, 0.04065],
# [0.00910, 0.00715, 0.01434,0.04313, 0.03642]]
# (within a tolerance of +/- 0.001 for each entry)
colors = [['R','G','G','R','R'],
['R','R','G','R','R'],
['R','R','G','G','R'],
['R','R','R','R','R']]
measurements = ['G','G','G','G','G']
motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]
p = localize(colors,measurements,motions,sensor_right = 0.7, p_move= 0.8)
show(p) # displays your answer
# the result is :
# [[0.01106,0.02464,0.06800,0.04472,0.02465],
# [0.00715,0.01017,0.08697,0.07988,0.00935],
# [0.00740,0.00894,0.11273,0.35351,0.04066],
# [0.00911,0.00715,0.01435,0.04313,0.03643]]
#
#
#
# test 1
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'G'],
# ['G', 'G', 'G']]
#measurements = ['R']
#motions = [[0,0]]
#sensor_right = 1.0
#p_move = 1.0
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass
# test 2
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'R'],
# ['G', 'G', 'G']]
#measurements = ['R']
#motions = [[0,0]]
#sensor_right = 1.0
#p_move = 1.0
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass
# test 3
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'R'],
# ['G', 'G', 'G']]
#measurements = ['R']
#motions = [[0,0]]
#sensor_right = 0.8
#p_move = 1.0
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass
# test 4
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'R'],
# ['G', 'G', 'G']]
#measurements = ['R', 'R']
#motions = [[0,0], [0,1]]
#sensor_right = 0.8
#p_move = 1.0
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass
# test 5
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'R'],
# ['G', 'G', 'G']]
#measurements = ['R', 'R']
#motions = [[0,0], [0,1]]
#sensor_right = 1.0
#p_move = 1.0
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass
# test 6
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'R'],
# ['G', 'G', 'G']]
#measurements = ['R', 'R']
#motions = [[0,0], [0,1]]
#sensor_right = 0.8
#p_move = 0.5
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass
# test 7
#colors = [['G', 'G', 'G'],
# ['G', 'R', 'R'],
# ['G', 'G', 'G']]
#measurements = ['R', 'R']
#motions = [[0,0], [0,1]]
#sensor_right = 1.0
#p_move = 0.5
#p = localize(colors,measurements,motions,sensor_right,p_move)
#show(p) # test pass其实，move()中应该可以不用归一化，因为sense()中每次会进行归一化，而且sense()是每次最后调用的方法。