def filter_particles_based_on_measurement(self):
# Based on Robot 0 get all distances to other robots
distances = [get_robot_distance(self.robots[0], self.ball)]
# Iterate over every particle (possible other robot)
for i in range(len(self.particles)):
p = self.particles[i]
# Calculate the Gaussian based on the distance of the particle to the measurements
gaussians = [w_gauss(d, get_robot_distance(self.robots[0], p), sigma2=500) for d in distances]
# Assign the new weight to the particle
self.particles[i].w = max(gaussians)
def robot_distance_matrix(self):
d = [[int(get_robot_distance(r1, r2)) for r1 in self.robots] for r2 in self.robots]
return d
