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 __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_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
