def test_callback_call(): pf = ParticleFilter(func, 100) pf.add_callback("at_start_of_run", callback) with pytest.raises(NotImplementedError): pf.callback("at_start_of_run") pf.add_callback("at_start_of_run") pf.callback("at_start_of_run")
def test_creation_errors(): with pytest.raises(TypeError): _ = ParticleFilter() with pytest.raises(TypeError): _ = ParticleFilter(func([1]), 100) with pytest.raises(TypeError): _ = ParticleFilter(func, 100, boundaries=20)
def test_calculate_procreation_rates_1(): pf = ParticleFilter(func, 100) x = np.random.rand(10, 2) y = np.arange(10) pf.set_seed(x, y) rates = pf._calculate_procreation_rates(y, 10000) rates[:-1] = rates[:-1] - rates[1:] positive = np.sum(rates[:-1] > 0) assert positive == len(y) - 1
def __init__(self, world, width=1000, height=800): self.width = width self.height = height self.master = Tk() self.canvas = Canvas(self.master, width=width, height=height) canvas = self.canvas canvas.pack() self.world = world world.display(canvas) self.localizer = ParticleFilter(N=3000, width=width, height=height) localizer = self.localizer localizer.display(canvas)
def test_run_iteration(): pf = ParticleFilter(func, 10, initial_width=0.00001, boundaries=np.array([[0, 1], [0, 1]]), kill_controller=get_kill_controller(0.2, False)) x = np.random.rand(100, 2) y = np.arange(100) pf.set_seed(x, y) pf.initialise_run() pf.run_iteration() pf.end_run()
def test_callback_errors(): pf = ParticleFilter(func, 100) with pytest.raises(TypeError): pf.add_callback("at_start_of_iteration", "callback") with pytest.raises(ValueError): pf.add_callback("at_start_of_iteration", incorrect_callback) with pytest.raises(ValueError): pf.add_callback("at_some_undefined_point", callback)
def test_end_iteration_with_graveyard(tmp_path): pf = ParticleFilter(func, 100, initial_width=0.00001, boundaries=np.array([[0, 1], [0, 1]]), kill_controller=get_kill_controller(0.2, False)) graveyard = str(tmp_path) + "/graveyard.csv" x = np.random.rand(100, 2) y = np.arange(100) pf.set_seed(x, y) pf.initialise_run(graveyard) pf.run_iteration() pf.end_run() graveyard = np.genfromtxt(graveyard, delimiter=',', skip_header=1) assert graveyard.shape == (200, 5)
def test_initialise_run(): pf = ParticleFilter(func, 100) pf.add_callback("at_start_of_run", callback) pf.iteration = 100 assert pf.iteration == 100 with pytest.raises(NotImplementedError): pf.initialise_run() assert pf.iteration == 0
def test_calculate_procreation_rates_2(): pf = ParticleFilter(func, 100) x = np.random.rand(10, 2) y = np.ones(10) pf.set_seed(x, y) rates = pf._calculate_procreation_rates(y, 10000) assert len(np.unique(rates)) == 1 assert rates[0] > 0 rates = pf._calculate_procreation_rates(y, 13) assert len(np.unique(rates)) == 2 assert np.amax(rates) - np.amin(rates) == 1 assert np.sum(rates) == 13
def test_function_errors(): _ = ParticleFilter(func, 100) with pytest.raises(TypeError): _ = ParticleFilter("func", 100) with pytest.raises(ValueError): _ = ParticleFilter(callback, 100)
def test_sampler_errors(): pf = ParticleFilter(func, 100, boundaries=np.array([[0, 1], [0, 1]])) with pytest.raises(TypeError): pf.sample_seed(10, "uniform_sampler") with pytest.raises(ValueError): pf.sample_seed(10, callback)
def test_sampler(): pf = ParticleFilter(func, 100, boundaries=np.array([[0, 1], [0, 1]])) pf.validate_emptiness() pf.sample_seed(10, uniform_sampler) with pytest.raises(Exception): pf.validate_emptiness()
def next_particle(state, prop_param): """ Sample new state from Gaussian around new expected location. prop_param: expected change in feet """ expected_state = state + prop_param * 4 stdev = prop_param * 4 # guesstimate of noise # stdev = prop_param * 30 # larger variance to test limits return min(max(int(np.round(np.random.normal(expected_state, stdev))), 0), 65) n_particles = int(input('Number of particles: ')) pf = ParticleFilter(p_particle, next_particle, np.random.choice(66, size=n_particles)) all_states = list(range(66)) viz = ParticleFilterVisualization(all_states, get_true_obs, pf.particles, y_particle=120) with open('longboard_episode_2.csv') as csvfile: reader = csv.reader(csvfile) prev_pos = 5 for row_idx, row in enumerate(reader): if row_idx % 100 == 0: # sample every 100 row to avoid tiny transitions obs, pos, _ = map(float, row) print('Particles: {}'.format(np.round(sorted(pf.particles), 2))) print('=========\n')
acc_s[:, 0] = [0, 0, 9.81] head_enc = 0 # Heading given by the encoders head_from_enc = np.zeros(N) xh = np.zeros((4, N)) # Orientation state estimate xh[0, :] = 1 Ph = np.identity(4) * 1 # Initial state covariance heading = np.zeros( (2, N)) # Heading and inclination from estimated orientation euler = np.zeros((3, N)) # Orientation estimation in Euler angles 'zyx' time_orientation = np.zeros( N) # Execution time for the orientation estimation algorithm # Position Estimation x_pf = np.zeros((3, N)) # State Estimate x_pf[:, 0] = [0, 0, 0] pf = ParticleFilter(x_pf[:, 0], 200) # Class particle filter vel_enc = np.zeros(N) # Velocity given by encoder measurements theta_dot = np.zeros(N) # Pitch angular velocity theta_2dot = np.zeros(N) # Pitch angular acceleration time_position = np.zeros(N) # Execution time for the particle filter # Slipping Detection slipping_one = np.zeros(N) slipping_two = np.zeros(N) real_slip = np.zeros(N) # Calibration x2_cal = np.zeros((6, 1000)) x2_cal[:, 0] = robot.state acceleration_cal = np.zeros((3, 1000)) acceleration_cal[:, 0] = [0, 0, 9.81]
return np.exp(-(get_true_obs(state) - obs) ** 2 / (2 * variance)) \ if 0 < state < 100 else 0 def next_particle(state, prop_param): """ Sample new state from Gaussian around new expected location. prop_param: predicted change in state """ expected_state = state + prop_param return np.random.normal(expected_state, abs(prop_param)**0.5) n_particles = int(input('Number of particles: ')) pf = ParticleFilter(p_particle, next_particle, 100 * np.random.random(n_particles)) plot_states = np.linspace(0, 100, 201) true_state = 10 viz = ParticleFilterVisualization(plot_states, get_true_obs, pf.particles, y_particle=700, true_state=true_state) while True: obs = np.random.normal(get_true_obs(true_state), 10) print('True state: {}'.format(true_state)) print('Particles: {}'.format(np.round(sorted(pf.particles), 2))) print('=========\n')
def test_creation(): pf = ParticleFilter(func, 100) pf.validate_emptiness()
def test_run_iteration_with_graveyard(tmp_path): pf = ParticleFilter(func, 10, initial_width=0.00001, boundaries=np.array([[0, 1], [0, 1]]), kill_controller=get_kill_controller(0.2, False)) graveyard = str(tmp_path) + "/graveyard.csv" x = np.random.rand(100, 2) y = np.arange(100) pf.set_seed(x, y) pf.initialise_run(graveyard) pf.run_iteration() assert os.path.exists(graveyard) os.remove(graveyard) assert not os.path.exists(graveyard) pf.change_graveyard(None) pf.run_iteration() assert not os.path.exists(graveyard) pf.change_graveyard(graveyard) pf.run_iteration() assert os.path.exists(graveyard) pf.end_run()
def setUp(self): self.particle_filter = ParticleFilter( '../../data/map/synthetic_small.dat')
def test_creation_width(): pf = ParticleFilter(func, 100, initial_width=1.0) assert pf.initial_width == 1.0 assert pf.width == 1.0
class Simulator: robot = None def __init__(self, world, width=1000, height=800): self.width = width self.height = height self.master = Tk() self.canvas = Canvas(self.master, width=width, height=height) canvas = self.canvas canvas.pack() self.world = world world.display(canvas) self.localizer = ParticleFilter(N=3000, width=width, height=height) localizer = self.localizer localizer.display(canvas) def interactive(self): """Start interactive mode (doesn't return)""" print "Click anywhere on the canvas to place the robot" def callback(event): print "@", event.x, ",", event.y self.place_robot(event.x, event.y) self.canvas.bind("<Button-1>", callback) mainloop() def explore(self, x, y, moves, delay=0.5): self.place_robot(x, y) for (x, y) in moves: self.move_robot(x, y) time.sleep(delay) def measurement_probabilty(self, particle, Z): loss = self.world.binary_loss(particle, Z) if loss: particle.color = "blue" else: particle. color = "gray" return loss def move_robot(self, rotation, distance): robot = self.robot canvas = self.canvas localizer = self.localizer if not robot: raise ValueError("Need to place robot in simulator before moving it") original_x = robot.x original_y = robot.y robot.move(rotation, distance) if robot.color and not robot.color == "None": canvas.create_line(original_x, original_y, robot.x, robot.y) Z = self.world.surface(robot.x, robot.y) self.localizer.erase(canvas) localizer.update(rotation, distance, lambda particle: self.measurement_probabilty(particle, Z)) localizer.display(canvas) robot.display(canvas) self.master.update() print "Sense:", Z def place_robot(self, x=None, y=None, bearing=None, color="green"): """Move the robot to the given position on the canvas""" if not self.robot: land = self.world.terrain[0] if not x: x = random.randint(land[0], land[2]) if not y: y = random.randint(land[1], land[3]) self.robot = Robot(x, y) self.robot.display_noise = 0.0 self.robot.color = color self.robot.size = 5 robot = self.robot if not bearing: bearing = atan2((y - robot.y), (x - robot.x)) rotation = bearing - robot.orientation distance = sqrt((robot.x - x) ** 2 + (robot.y - y) ** 2) self.move_robot(rotation, distance) return self.robot
def test_callbacks(): pf = ParticleFilter(func, 100) pf.add_callback('at_start_of_iteration', callback) assert pf._callbacks['at_start_of_iteration'] == [callback] pf.add_callback('at_start_of_iteration') assert pf._callbacks['at_start_of_iteration'] == []
class Simulator: robot = None def __init__(self, world, width=1000, height=800): self.width = width self.height = height self.master = Tk() self.canvas = Canvas(self.master, width=width, height=height) canvas = self.canvas canvas.pack() self.world = world world.display(canvas) self.localizer = ParticleFilter(N=3000, width=width, height=height) localizer = self.localizer localizer.display(canvas) def interactive(self): """Start interactive mode (doesn't return)""" print "Click anywhere on the canvas to place the robot" def callback(event): print "@", event.x, ",", event.y self.place_robot(event.x, event.y) self.canvas.bind("<Button-1>", callback) mainloop() def explore(self, x, y, moves, delay=0.5): self.place_robot(x, y) for (x, y) in moves: self.move_robot(x, y) time.sleep(delay) def measurement_probabilty(self, particle, Z): loss = self.world.binary_loss(particle, Z) if loss: particle.color = "blue" else: particle.color = "gray" return loss def move_robot(self, rotation, distance): robot = self.robot canvas = self.canvas localizer = self.localizer if not robot: raise ValueError( "Need to place robot in simulator before moving it") original_x = robot.x original_y = robot.y robot.move(rotation, distance) if robot.color and not robot.color == "None": canvas.create_line(original_x, original_y, robot.x, robot.y) Z = self.world.surface(robot.x, robot.y) self.localizer.erase(canvas) localizer.update( rotation, distance, lambda particle: self.measurement_probabilty(particle, Z)) localizer.display(canvas) robot.display(canvas) self.master.update() print "Sense:", Z def place_robot(self, x=None, y=None, bearing=None, color="green"): """Move the robot to the given position on the canvas""" if not self.robot: land = self.world.terrain[0] if not x: x = random.randint(land[0], land[2]) if not y: y = random.randint(land[1], land[3]) self.robot = Robot(x, y) self.robot.display_noise = 0.0 self.robot.color = color self.robot.size = 5 robot = self.robot if not bearing: bearing = atan2((y - robot.y), (x - robot.x)) rotation = bearing - robot.orientation distance = sqrt((robot.x - x)**2 + (robot.y - y)**2) self.move_robot(rotation, distance) return self.robot
def external_init_particle_filter(img): middle_pt = Point(img.width / 2, img.height / 2) # NOTE: square crops a bit of image but should be ok box = Box(middle_pt, img.height / 2) particle_filter = ParticleFilter(box) return particle_filter