def gaussian_distance_two_matrices(self, sigma2=100):
uvs = [get_uv_robot_to_robot(self.robots[0], r1) for r1 in self.robots][1:]
for particle in self.particles:
uv = get_uv_robot_to_robot(self.robots[0], particle)
weights = [self.w_gauss2d(uv, uvelem) for uvelem in uvs]
weights = [w for w in weights if w[0] > 0 and w[1] > 0]
if len(weights) > 0:
particle.w = max(max(weights))
else:
particle.w = 0
def __init__(self):
PARTICLE_COUNT = 2500
self.robots = []
self.robots.append(RealRobot("Goalie", 0, -4500, 90))
self.robots.append(RealRobot("Fieldie1", 0, -900, 180))
self.robots.append(RealRobot("Fieldie2", -1200, -1500, 180))
self.robots.append(RealRobot("Fieldie3", 1200, -2400, 180))
self.ball = Particle(0, 0, 0)
print get_uv_robot_to_robot(self.robots[0], self.robots[1])
print get_uv_robot_to_robot(self.robots[0], self.robots[2])
print get_uv_robot_to_robot(self.robots[0], self.robots[3])
self.particles = [Particle(random.randint(-3000, 3000), random.randint(-4500, 4500), random.randint(0,360)) for i in range(PARTICLE_COUNT)]
for i in range(200):
self.filter_particles_based_on_measurement()
self.normalize_particles()
self.particles = self.make_new_particle_cloud(PARTICLE_COUNT)
self.ball.x += random.randint(0, 10)
self.ball.y += random.randint(0, 10)
if i % 20 == 0:
particles_x = [p.x for p in self.particles]
particles_y = [p.y for p in self.particles]
plot(particles_x, particles_y, 'ro')
plot([r.x for r in self.robots], [r.y for r in self.robots], "bs")
xlim([-3000, 3000])
ylim([-4500, 4500])
show()