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)