-
Notifications
You must be signed in to change notification settings - Fork 0
/
localizer.py
53 lines (45 loc) · 1.44 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
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): # color ~ Z , beliefs ~ p , grid ~ world
new_beliefs = []
#
# TODO - implement this in part 2
#
height = len(grid)
width = len(grid[0])
sum1 =0
for i in range(height):
row = []
for j in range(width):
hit = (color==grid[i][j])
row.append(beliefs[i][j]*(hit*p_hit + (1-hit)*p_miss))
sum1 = sum1 + beliefs[i][j]*(hit*p_hit + (1-hit)*p_miss)
new_beliefs.append(row)
for i in range(height):
row = []
for j in range(width):
new_beliefs[i][j] = new_beliefs[i][j]/sum1
return new_beliefs
def move(dy, dx, beliefs, blurring):
height = len(beliefs)
width = len(beliefs[0])#gives cols
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)