コード例 #1
0
    def test_transition_matrix_3x3(self):
        t = grid.build_transition_matrix(3, 3)

        # move in the direction of the heading
        for index in ((16, 4), (28, 16), (11, 7), (19, 15), (29, 33)):
            self.assertEqual(t[index], 0.7)

        # corner moving to another heading p=0.3
        self.assertEqual(t[1, 14], 0.3)
        self.assertEqual(t[11, 22], 0.3)
        self.assertEqual(t[10, 7], 0.3)
        self.assertEqual(t[2, 5], 0.3)
        self.assertEqual(t[25, 12], 0.3)

        # corner moving to another heading p=0.5
        self.assertEqual(t[27, 12], 0.5)
        self.assertEqual(t[26, 12], 0.5)
        self.assertEqual(t[9, 7], 0.5)
        self.assertEqual(t[9, 22], 0.5)

        # middle square to another square p=0.1
        self.assertAlmostEqual(t[16, 15], 0.1)
        self.assertAlmostEqual(t[17, 15], 0.1)
        self.assertAlmostEqual(t[17, 30], 0.1)

        # test some edge movements p=0.15
        self.assertAlmostEqual(t[23, 8], 0.15)
        self.assertAlmostEqual(t[13, 0], 0.15)
        self.assertAlmostEqual(t[13, 26], 0.15)

        self.assertAlmostEqual(np.sum(t), 36)
コード例 #2
0
def automated_run():
    import matplotlib.pyplot as plt

    fig = plt.figure(figsize=(10, 7))
    navg = 20
    nsteps = 10

    for size in (2, 2), (3, 3), (4, 4), (5, 5), (10, 10):

        avg_distances = np.zeros(shape=(nsteps+1,))

        for n in range(navg):

            distances = list()
            none_values = list()

            the_T_matrix = build_transition_matrix(*size)
            the_filter = FilterState(transition=the_T_matrix)
            the_sensor = Sensor()
            the_grid = Grid(*size)
            the_robot = Robot(the_grid, the_T_matrix)

            # get the manhattan distance at the start
            filter_est = the_grid.index_to_pose(the_filter.belief_state)
            pos_est = (filter_est[0], filter_est[1])
            distances.append(manhattan(the_robot.get_position(), pos_est))

            for i in range(nsteps):

                # take a step then approximate etc.
                the_robot.step()
                sensor_value = the_sensor.get_position(the_robot)
                if sensor_value is None:
                    none_values.append(i)  # keep track of where None was returned

                obs = the_sensor.get_obs_matrix(sensor_value, size)
                the_filter.forward(obs)

                filter_est = the_grid.index_to_pose(the_filter.belief_state)
                pos_est = (filter_est[0], filter_est[1])
                distances.append(manhattan(the_robot.get_position(), pos_est))

            avg_distances += np.array(distances)

        avg_distances /= navg
        base_line, = plt.plot(avg_distances, label="Grid size {}".format(size))

        # for point in none_values:
        #     plt.scatter(point, distances[point], marker='o',
        #                 color=base_line.get_color(), s=40)

    plt.legend()
    plt.xlim(0, nsteps)
    plt.ylim(0,)
    plt.ylabel("Manhattan distance")
    plt.xlabel("Steps")
    plt.title("Manhattan distance from true position and inferred position \n"
              "from the hidden Markov model (average over %s runs)" % navg)
    fig.savefig("automated_run.png")
    plt.show()
コード例 #3
0
    def test_transition_matrix_3x3(self):
        t = grid.build_transition_matrix(3, 3)

        # move in the direction of the heading
        for index in ((16, 4), (28, 16), (11, 7), (19, 15), (29, 33)):
            self.assertEqual(t[index], 0.7)

        # corner moving to another heading p=0.3
        self.assertEqual(t[1, 14], 0.3)
        self.assertEqual(t[11, 22], 0.3)
        self.assertEqual(t[10, 7], 0.3)
        self.assertEqual(t[2, 5], 0.3)
        self.assertEqual(t[25, 12], 0.3)

        # corner moving to another heading p=0.5
        self.assertEqual(t[27, 12], 0.5)
        self.assertEqual(t[26, 12], 0.5)
        self.assertEqual(t[9, 7], 0.5)
        self.assertEqual(t[9, 22], 0.5)

        # middle square to another square p=0.1
        self.assertAlmostEqual(t[16, 15], 0.1)
        self.assertAlmostEqual(t[17, 15], 0.1)
        self.assertAlmostEqual(t[17, 30], 0.1)

        # test some edge movements p=0.15
        self.assertAlmostEqual(t[23, 8], 0.15)
        self.assertAlmostEqual(t[13, 0], 0.15)
        self.assertAlmostEqual(t[13, 26], 0.15)

        self.assertAlmostEqual(np.sum(t), 36)
コード例 #4
0
    def test_surrounding(self):
        size = (4, 4)

        trans_mat = grid.build_transition_matrix(*size)
        rob = robot.Robot(grid.Grid(4, 4), trans_mat)
        sens = robot.Sensor()

        # print('\n')
        for i in range(10000):
            p = sens.get_position(rob)
コード例 #5
0
    def test_step(self):
        size = (4, 4)

        trans_mat = grid.build_transition_matrix(*size)
        robo = robot.Robot(grid.Grid(4, 4), trans_mat)

        # Test that the robot moves somewhat according to the rules
        for i in range(100):
            fst_pose = robo.get_pose()
            robo.step()
            snd_pose = robo.get_pose()
            self.assertNotEqual(fst_pose, snd_pose)
            if snd_pose[2] == Heading.EAST:
                self.assert_pose_east_of(snd_pose, fst_pose)
            elif snd_pose[2] == Heading.NORTH:
                self.assert_pose_north_of(snd_pose, fst_pose)
            elif snd_pose[2] == Heading.SOUTH:
                self.assert_pose_south_of(snd_pose, fst_pose),
            elif snd_pose[2] == Heading.WEST:
                self.assert_pose_west_of(snd_pose, fst_pose)
コード例 #6
0
def main():
    parser = argparse.ArgumentParser(description='Robot localisation with HMM')
    parser.add_argument(
        '-r', '--rows',
        type=int,
        help='the number of rows on the grid, default is 4',
        default=4)
    parser.add_argument(
        '-c', '--columns',
        type=int,
        help='the number of columns on the grid, default is 4',
        default=4)
    args = parser.parse_args()

    # Initialise the program
    size = (args.rows, args.columns)
    the_T_matrix = build_transition_matrix(*size)
    the_filter = FilterState(transition=the_T_matrix)
    the_sensor = Sensor()
    the_grid = Grid(*size)
    the_robot = Robot(the_grid, the_T_matrix)
    sensor_value = None
    obs = None

    print(help_text())
    print("Grid size is {} x {}".format(size[0], size[1]))
    print(the_robot)
    print("The sensor says: {}".format(sensor_value))
    filter_est = the_grid.index_to_pose(the_filter.belief_state)
    pos_est = (filter_est[0], filter_est[1])
    print("The HMM filter thinks the robot is at {}".format(filter_est))
    print("The Manhattan distance is: {}".format(
        manhattan(the_robot.get_position(), pos_est)))
    np.set_printoptions(linewidth=1000)

    # Main loop
    while True:
        user_command = str(input('> '))
        if user_command.upper() == 'QUIT' or user_command.upper() == 'Q':
            break
        elif user_command.upper() == 'HELP':
            print(help_text())
        elif user_command.upper() == 'SHOW T':
            print(the_T_matrix)
        elif user_command.upper() == 'SHOW F':
            print(the_filter.belief_matrix)
        elif user_command.upper() == 'SHOW O':
            print(obs)
        elif not user_command:
            # take a step then approximate etc.
            the_robot.step()
            sensor_value = the_sensor.get_position(the_robot)
            obs = the_sensor.get_obs_matrix(sensor_value, size)
            the_filter.forward(obs)

            print(the_robot)
            print("The sensor says: {}".format(sensor_value))
            filter_est = the_grid.index_to_pose(the_filter.belief_state)
            pos_est = (filter_est[0], filter_est[1])
            print("The HMM filter thinks the robot is at {}".format(filter_est))
            print("The Manhattan distance is: {}".format(
                manhattan(the_robot.get_position(), pos_est)))

        else:
            print("Unknown command!")
コード例 #7
0
def main():
    parser = argparse.ArgumentParser(description='Robot localisation with HMM')
    parser.add_argument('-r',
                        '--rows',
                        type=int,
                        help='the number of rows on the grid, default is 4',
                        default=4)
    parser.add_argument('-c',
                        '--columns',
                        type=int,
                        help='the number of columns on the grid, default is 4',
                        default=4)
    args = parser.parse_args()

    # Initialise the program
    size = (args.rows, args.columns)
    the_T_matrix = build_transition_matrix(*size)
    the_filter = FilterState(transition=the_T_matrix)
    the_sensor = Sensor()
    the_grid = Grid(*size)
    the_robot = Robot(the_grid, the_T_matrix)
    sensor_value = None
    obs = None

    print(help_text())
    print("Grid size is {} x {}".format(size[0], size[1]))
    print(the_robot)
    print("The sensor says: {}".format(sensor_value))
    filter_est = the_grid.index_to_pose(the_filter.belief_state)
    pos_est = (filter_est[0], filter_est[1])
    print("The HMM filter thinks the robot is at {}".format(filter_est))
    print("The Manhattan distance is: {}".format(
        manhattan(the_robot.get_position(), pos_est)))
    np.set_printoptions(linewidth=1000)

    # Main loop
    while True:
        user_command = str(input('> '))
        if user_command.upper() == 'QUIT' or user_command.upper() == 'Q':
            break
        elif user_command.upper() == 'HELP':
            print(help_text())
        elif user_command.upper() == 'SHOW T':
            print(the_T_matrix)
        elif user_command.upper() == 'SHOW F':
            print(the_filter.belief_matrix)
        elif user_command.upper() == 'SHOW O':
            print(obs)
        elif not user_command:
            # take a step then approximate etc.
            the_robot.step()
            sensor_value = the_sensor.get_position(the_robot)
            obs = the_sensor.get_obs_matrix(sensor_value, size)
            the_filter.forward(obs)

            print(the_robot)
            print("The sensor says: {}".format(sensor_value))
            filter_est = the_grid.index_to_pose(the_filter.belief_state)
            pos_est = (filter_est[0], filter_est[1])
            print(
                "The HMM filter thinks the robot is at {}".format(filter_est))
            print("The Manhattan distance is: {}".format(
                manhattan(the_robot.get_position(), pos_est)))

        else:
            print("Unknown command!")
コード例 #8
0
def automated_run():
    import matplotlib.pyplot as plt

    fig = plt.figure(figsize=(10, 7))
    navg = 20
    nsteps = 10

    for size in (2, 2), (3, 3), (4, 4), (5, 5), (10, 10):

        avg_distances = np.zeros(shape=(nsteps + 1, ))

        for n in range(navg):

            distances = list()
            none_values = list()

            the_T_matrix = build_transition_matrix(*size)
            the_filter = FilterState(transition=the_T_matrix)
            the_sensor = Sensor()
            the_grid = Grid(*size)
            the_robot = Robot(the_grid, the_T_matrix)

            # get the manhattan distance at the start
            filter_est = the_grid.index_to_pose(the_filter.belief_state)
            pos_est = (filter_est[0], filter_est[1])
            distances.append(manhattan(the_robot.get_position(), pos_est))

            for i in range(nsteps):

                # take a step then approximate etc.
                the_robot.step()
                sensor_value = the_sensor.get_position(the_robot)
                if sensor_value is None:
                    none_values.append(
                        i)  # keep track of where None was returned

                obs = the_sensor.get_obs_matrix(sensor_value, size)
                the_filter.forward(obs)

                filter_est = the_grid.index_to_pose(the_filter.belief_state)
                pos_est = (filter_est[0], filter_est[1])
                distances.append(manhattan(the_robot.get_position(), pos_est))

            avg_distances += np.array(distances)

        avg_distances /= navg
        base_line, = plt.plot(avg_distances, label="Grid size {}".format(size))

        # for point in none_values:
        #     plt.scatter(point, distances[point], marker='o',
        #                 color=base_line.get_color(), s=40)

    plt.legend()
    plt.xlim(0, nsteps)
    plt.ylim(0, )
    plt.ylabel("Manhattan distance")
    plt.xlabel("Steps")
    plt.title("Manhattan distance from true position and inferred position \n"
              "from the hidden Markov model (average over %s runs)" % navg)
    fig.savefig("automated_run.png")
    plt.show()