Lesson 1 Localization
蒙特卡洛机器人定位模型
sense 贝叶斯模型
move 全概率公式
localization练习
1 # The function localize takes the following arguments: 2 # 3 # colors: 4 # 2D list, each entry either 'R' (for red cell) or 'G' (for green cell) 5 # 6 # measurements: 7 # list of measurements taken by the robot, each entry either 'R' or 'G' 8 # 9 # motions: 10 # list of actions taken by the robot, each entry of the form [dy,dx], 11 # where dx refers to the change in the x-direction (positive meaning 12 # movement to the right) and dy refers to the change in the y-direction 13 # (positive meaning movement downward) 14 # NOTE: the *first* coordinate is change in y; the *second* coordinate is 15 # change in x 16 # 17 # sensor_right: 18 # float between 0 and 1, giving the probability that any given 19 # measurement is correct; the probability that the measurement is 20 # incorrect is 1-sensor_right 21 # 22 # p_move: 23 # float between 0 and 1, giving the probability that any given movement 24 # command takes place; the probability that the movement command fails 25 # (and the robot remains still) is 1-p_move; the robot will NOT overshoot 26 # its destination in this exercise 27 # 28 # The function should RETURN (not just show or print) a 2D list (of the same 29 # dimensions as colors) that gives the probabilities that the robot occupies 30 # each cell in the world. 31 # 32 # Compute the probabilities by assuming the robot initially has a uniform 33 # probability of being in any cell. 34 # 35 # Also assume that at each step, the robot: 36 # 1) first makes a movement, 37 # 2) then takes a measurement. 38 # 39 # Motion: 40 # [0,0] - stay 41 # [0,1] - right 42 # [0,-1] - left 43 # [1,0] - down 44 # [-1,0] - up 45 def sense(p,colors,measurement,sensor_right): 46 q=[] 47 for row in range(len(colors)): 48 temp=[] 49 for col in range(len(colors[0])): 50 hit = (measurement == colors[row][col]) 51 temp.append(p[row][col] * (hit * sensor_right + (1-hit) * (1-sensor_right))) 52 q.append(temp) 53 s=0 54 for row in range(len(q)): 55 for col in range(len(q[0])): 56 s += q[row][col] 57 for row in range(len(p)): 58 for col in range(len(q[0])): 59 q[row][col] = q[row][col]/s 60 return q 61 62 def move(p, motion, p_move): 63 q = [] 64 for row in range(len(colors)): 65 temp=[] 66 for col in range(len(colors[0])): 67 s = p_move * p[(row - motion[0]) % len(colors)][(col - motion[1]) % len(colors[0])] 68 s += (1-p_move) * p[row][col] 69 temp.append(s) 70 q.append(temp) 71 return q 72 73 def localize(colors,measurements,motions,sensor_right,p_move): 74 # initializes p to a uniform distribution over a grid of the same dimensions as colors 75 pinit = 1.0 / float(len(colors)) / float(len(colors[0])) 76 p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))] 77 78 # >>> Insert your code here <<< 79 80 for k in range(len(motions)): 81 p = move(p, motions[k],p_move) 82 p = sense(p,colors,measurements[k],sensor_right) 83 84 return p 85 86 def show(p): 87 rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p] 88 print '[' + ', '.join(rows) + ']' 89 90 ############################################################# 91 # For the following test case, your output should be 92 # [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465], 93 # [0.00715, 0.01017, 0.08696, 0.07988, 0.00935], 94 # [0.00739, 0.00894, 0.11272, 0.35350, 0.04065], 95 # [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]] 96 # (within a tolerance of +/- 0.001 for each entry) 97 98 colors = [['R','G','G','R','R'], 99 ['R','R','G','R','R'], 100 ['R','R','G','G','R'], 101 ['R','R','R','R','R']] 102 measurements = ['G','G','G','G','G'] 103 motions = [[0,0],[0,1],[1,0],[1,0],[0,1]] 104 p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8) 105 show(p) # displays your answer
simultaneous adj.同时的