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