-
Notifications
You must be signed in to change notification settings - Fork 0
/
localization_2D_move_measure.py
107 lines (97 loc) · 3.72 KB
/
localization_2D_move_measure.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
# -*- coding: utf-8 -*-
"""
Created on Tue Mar 17 16:38:35 2020
@author: cenic
"""
# The function localize takes the following arguments:
#
# colors:
# 2D list, each entry either 'R' (for red cell) or 'G' (for green cell)
#
# measurements:
# list of measurements taken by the robot, each entry either 'R' or 'G'
#
# motions:
# list of actions taken by the robot, each entry of the form [dy,dx],
# where dx refers to the change in the x-direction (positive meaning
# movement to the right) and dy refers to the change in the y-direction
# (positive meaning movement downward)
# NOTE: the *first* coordinate is change in y; the *second* coordinate is
# change in x
#
# sensor_right:
# float between 0 and 1, giving the probability that any given
# measurement is correct; the probability that the measurement is
# incorrect is 1-sensor_right
#
# p_move:
# float between 0 and 1, giving the probability that any given movement
# command takes place; the probability that the movement command fails
# (and the robot remains still) is 1-p_move; the robot will NOT overshoot
# its destination in this exercise
#
# The function should RETURN (not just show or print) a 2D list (of the same
# dimensions as colors) that gives the probabilities that the robot occupies
# each cell in the world.
#
# Compute the probabilities by assuming the robot initially has a uniform
# 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 a uniform distribution over a grid of the same dimensions as colors
pinit = 1.0 / float(len(colors)) / float(len(colors[0]))
p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))]
# >>> Insert your code here <<<
def move(p, motion, p_stay):
q=[x[:] for x in p ]
U = motion[0]
V = motion[1]
for i in range(len(p)):
for j in range(len(p[i])):
q[i][j] = p_stay * p[i][j]
q[i][j] += (1-p_stay) * p[(i-U) % len(p)][(j-V) % len(p[i])]
return q
def sense(p, colors, measurement, sensor_right):
q = p
suma=0
for i in range(len(p)):
for j in range(len(p[i])):
hit = colors[i][j]==measurement
q[i][j] = p[i][j] * (hit * sensor_right + (1-hit) * (1-sensor_right))
suma+=q[i][j]
for i in range(len(p)):
for j in range(len(p[i])):
q[i][j]/=suma
return q
for i in range(len(measurements)):
p = move(p, motions[i], 1-p_move)
p = sense(p, colors,measurements[i], sensor_right)
return p
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