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()
