def calculate_similarity_and_assign_weight(self):
for particle in self.particles:
error_front = abs(self.robot.features[0] - particle.features[0])
error_left = abs(self.robot.features[1] - particle.features[1])
error_right = abs(self.robot.features[2] - particle.features[2])
total = (error_front + error_left + error_right) / 3.0
similarity1 = w_gauss(error_front, 0, sigma2=1000)
similarity2 = w_gauss(error_left, 0, sigma2=1000)
similarity3 = w_gauss(error_right, 0, sigma2=1000)
similarity = [similarity1, similarity2, similarity3]
similarity_other = w_gauss(total, 0, sigma2=20000)
value_to_use = similarity_other
if value_to_use > 0.5:
particle.w = value_to_use
else:
particle.w = 0
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 w_gauss2d(self, r1, r2, sigma2=500):
return w_gauss(r1[0], r2[0], sigma2), w_gauss(r1[1], r2[1], sigma2)