def __init__(self, grid, blur, p_hit, start_pos=None):
self.grid = grid
self.beliefs = localizer.initialize_beliefs(self.grid)
self.height = len(grid)
self.width = len(grid[0])
self.blur = blur
self.p_hit = p_hit
self.p_miss = 1.0
self.incorrect_sense_probability = self.p_miss / (p_hit + self.p_miss)
self.colors = self.get_colors()
self.num_colors = len(self.colors)
if not start_pos:
self.true_pose = (self.height / 2, self.width / 2)
else:
self.true_pose = start_pos
self.prev_pose = self.true_pose
self.prepare_visualizer()
def __init__(self, grid, blur, p_hit,start_pos=None): self.grid = grid self.beliefs = localizer.initialize_beliefs(self.grid)
del(self.Y[:])
del(self.P[:])
R = 'r'
G = 'g'
blur = 0.05
p_hit = 200.0
grid = [
[R,G,G,G,R],
[G,G,R,G,R],
[G,R,G,G,G],
[R,R,G,R,G],
[R,G,R,G,R],
]
localizer.initialize_beliefs(grid)
true_pose = None
X = []
Y = []
P = []
for y, row in enumerate(localizer.initialize_beliefs(grid)):
for x, belief in enumerate(row):
X.append(x)
Y.append(5-y-1)
P.append(5000.0 * belief)
plt.figure()
plt.scatter(X,Y,s=P,color="blue")
plt.scatter([true_pose[1]], [height-true_pose[0]-1], color='red', marker="*", s=200)
plt.show()
