Ejemplo n.º 1
0
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")
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
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
Ejemplo n.º 4
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)
Ejemplo n.º 5
0
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()
Ejemplo n.º 6
0
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)
Ejemplo n.º 7
0
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)
Ejemplo n.º 8
0
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
Ejemplo n.º 9
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)
Ejemplo n.º 10
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
Ejemplo n.º 11
0
def test_function_errors():
    _ = ParticleFilter(func, 100)
    with pytest.raises(TypeError):
        _ = ParticleFilter("func", 100)
    with pytest.raises(ValueError):
        _ = ParticleFilter(callback, 100)
Ejemplo n.º 12
0
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)
Ejemplo n.º 13
0
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()
Ejemplo n.º 14
0
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')
Ejemplo n.º 15
0
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')
Ejemplo n.º 17
0
def test_creation():
    pf = ParticleFilter(func, 100)
    pf.validate_emptiness()
Ejemplo n.º 18
0
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()
Ejemplo n.º 19
0
 def setUp(self):
     self.particle_filter = ParticleFilter(
         '../../data/map/synthetic_small.dat')
Ejemplo n.º 20
0
def test_creation_width():
    pf = ParticleFilter(func, 100, initial_width=1.0)
    assert pf.initial_width == 1.0
    assert pf.width == 1.0
Ejemplo n.º 21
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
Ejemplo n.º 22
0
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'] == []
Ejemplo n.º 23
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
Ejemplo n.º 24
0
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