-
Notifications
You must be signed in to change notification settings - Fork 0
/
localizer.py
54 lines (44 loc) · 1.41 KB
/
localizer.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
import pdb
from helpers import normalize, blur
def initialize_beliefs(grid):
height = len(grid)
width = len(grid[0])
area = height * width
belief_per_cell = 1.0 / area
beliefs = []
for i in range(height):
row = []
for j in range(width):
row.append(belief_per_cell)
beliefs.append(row)
return beliefs
def sense(color, grid, beliefs, p_hit, p_miss):
new_beliefs = []
#
# TODO - implement this in part 2
#
height = len(grid)
width = len(grid[0])
for i in range(height):
row = []
for j in range(width):
hit = (color==grid[i][j])
q = ( (hit * p_hit * beliefs[i][j]) + ((1-hit) * p_miss * beliefs[i][j]))
row.append(q)
new_beliefs.append(row)
s = sum(map(sum, new_beliefs))
for i in range(height):
for j in range(width):
new_beliefs[i][j] = new_beliefs[i][j] / s
return new_beliefs
def move(dy, dx, beliefs, blurring):
height = len(beliefs)
width = len(beliefs[0])
new_G = [[0.0 for i in range(width)] for j in range(height)]
for i, row in enumerate(beliefs):
for j, cell in enumerate(row):
new_i = (i + dy ) % height
new_j = (j + dx ) % width
#pdb.set_trace()
new_G[int(new_i)][int(new_j)] = cell
return blur(new_G, blurring)