def uncertainMove(D): global particles rm.move_forward(interface, motors, D) # robot move sigma_e = np.sqrt(D) * stdev_e sigma_f = np.sqrt(D) * stdev_f new_particles = [] for p in particles: # update particles e = random.gauss(0, sigma_e) f = random.gauss(0, sigma_f) theta = radians(p[2]) new_particles.append((p[0] + (D + e) * cos(theta), p[1] + (D + e) * sin(theta), p[2] + f, p[3])) particles = new_particles
def uncertainMove(D): global particles rm.move_forward(interface,D) # robot move sigma_e = np.sqrt(D) * stdev_e sigma_f = np.sqrt(D) * stdev_f new_particles = [] for p in particles.data: # update particles e = random.gauss(0,sigma_e) f = random.gauss(0,sigma_f) theta = radians(p[2]) new_particles.append((p[0]+(D+e)*cos(theta), p[1]+(D+e)*sin(theta), p[2]+f, p[3])) particles.data = new_particles sonar_reading = rm.sonar_measurement(interface) particles.update(sonar_reading) particles.draw()