# In[12]:
# import the helper function
from helpers import display_world
# Display the final world!
# define figure size
plt.rcParams["figure.figsize"] = (20, 20)
# check if poses has been created
if 'poses' in locals():
# print out the last pose
print('Last pose: ', poses[-1])
# display the last position of the robot *and* the landmark positions
display_world(int(world_size), poses[-1], landmarks)
# ### Question: How far away is your final pose (as estimated by `slam`) compared to the *true* final pose? Why do you think these poses are different?
#
# You can find the true value of the final pose in one of the first cells where `make_data` was called. You may also want to look at the true landmark locations and compare them to those that were estimated by `slam`. Ask yourself: what do you think would happen if we moved and sensed more (increased N)? Or if we had lower/higher noise parameters.
# **Answer**:
#
# Due to the noise value which is randomly generated, the value estimated by the slam is deviated more to that of true values.
#
# If we increase N, then the final pose value will vary more from the true value.
#
# If we lower the noise, then we are reducing the randomness in the system and also its impact on the motion and measurements will be really less and we can get accurate locations easily, on the other hand if we increase the noise we will increase the randomness in the system and also its impact on the motion and measurement will be more and we have to adjust the motion and measured with their respective noises to caluculate the locations
# ## Testing
#
def main1():
print("started")
print("-------")
world_size = 10.0 # size of world (square)
measurement_range = 5.0 # range at which we can sense landmarks
motion_noise = 0.2 # noise in robot motion
measurement_noise = 0.2 # noise in the measurements
# instantiate a robot, r
r = Robot(world_size, measurement_range, motion_noise, measurement_noise)
# print out the location of r
print(r)
# define figure size
plt.rcParams["figure.figsize"] = (5, 5)
# call display_world and display the robot in it's grid world
print(r)
display_world(int(world_size), [r.x, r.y])
# choose values of dx and dy (negative works, too)
dx = 1
dy = 2
r.move(dx, dy)
# print out the exact location
print(r)
# display the world after movement, not that this is the same call as before
# the robot tracks its own movement
display_world(int(world_size), [r.x, r.y])
# create any number of landmarks
num_landmarks = 3
r.make_landmarks(num_landmarks)
# print out our robot's exact location
print(r)
# display the world including these landmarks
display_world(int(world_size), [r.x, r.y], r.landmarks)
# print the locations of the landmarks
print('Landmark locations [x,y]: ', r.landmarks)
# try to sense any surrounding landmarks
measurements = r.sense()
# this will print out an empty list if `sense` has not been implemented
print(measurements)
data = []
# after a robot first senses, then moves (one time step)
# that data is appended like so:
data.append([measurements, [dx, dy]])
# for our example movement and measurement
print(data)
# in this example, we have only created one time step (0)
time_step = 0
# so you can access robot measurements:
print('Measurements: ', data[time_step][0])
# and its motion for a given time step:
print('Motion: ', data[time_step][1])
def main():
# world parameters
num_landmarks = 5 # number of landmarks
N = 20 # time steps
world_size = 100.0 # size of world (square)
# robot parameters
measurement_range = 50.0 # range at which we can sense landmarks
motion_noise = 2.0 # noise in robot motion
measurement_noise = 2.0 # noise in the measurements
distance = 20.0 # distance by which robot (intends to) move each iteratation
# make_data instantiates a robot, AND generates random landmarks for a given world size and number of landmarks
data = make_data(N, num_landmarks, world_size, measurement_range,
motion_noise, measurement_noise, distance)
# print out some stats about the data
time_step = 0
print('Example measurements: \n', data[time_step][0])
print('\n')
print('Example motion: \n', data[time_step][1])
# define a small N and world_size (small for ease of visualization)
N_test = 5
num_landmarks_test = 2
small_world = 10
# initialize the constraints
initial_omega, initial_xi = initialize_constraints(N_test,
num_landmarks_test,
small_world)
plt.rcParams["figure.figsize"] = (10, 7)
# display omega (need to convert omega to a 2x2 matrix for the heatmap to show)
display_omega = reformat_omega(initial_omega)
sns.heatmap(display_omega, cmap='Blues', annot=True, linewidths=.5)
#plt.show()
# define figure size
plt.rcParams["figure.figsize"] = (1, 7)
# display xi
sns.heatmap(DataFrame(initial_xi),
cmap='Oranges',
annot=True,
linewidths=.5)
#plt.show()
## TODO: Complete the code to implement SLAM
# call your implementation of slam, passing in the necessary parameters
mu = slam(data, N, num_landmarks, world_size, motion_noise,
measurement_noise)
# print out the resulting landmarks and poses
if (mu is not None):
# get the lists of poses and landmarks
# and print them out
poses, landmarks = get_poses_landmarks(mu, N, num_landmarks)
print_all(poses, landmarks)
# Display the final world!
# define figure size
plt.rcParams["figure.figsize"] = (20, 20)
# check if poses has been created
if 'poses' in locals():
# print out the last pose
print('Last pose: ', poses[-1])
# display the last position of the robot *and* the landmark positions
display_world(int(world_size), poses[-1], landmarks)
print("*** THE END ***")
# # In the given example, we can see/print out that the robot is in the middle of the 10x10 world at (x, y) = (5.0, 5.0), which is exactly what we expect! # # However, it's kind of hard to imagine this robot in the center of a world, without visualizing the grid itself, and so in the next cell we provide a helper visualization function, `display_world`, that will display a grid world in a plot and draw a red `o` at the location of our robot, `r`. The details of how this function wors can be found in the `helpers.py` file in the home directory; you do not have to change anything in this `helpers.py` file. # In[14]: # import helper function from helpers import display_world # define figure size plt.rcParams["figure.figsize"] = (5, 5) # call display_world and display the robot in it's grid world print(r) display_world(int(world_size), [r.x, r.y]) # ## Movement # # Now you can really picture where the robot is in the world! Next, let's call the robot's `move` function. We'll ask it to move some distance `(dx, dy)` and we'll see that this motion is not perfect by the placement of our robot `o` and by the printed out position of `r`. # # Try changing the values of `dx` and `dy` and/or running this cell multiple times; see how the robot moves and how the uncertainty in robot motion accumulates over multiple movements. # # #### For a `dx` = 1, does the robot move *exactly* one spot to the right? What about `dx` = -1? What happens if you try to move the robot past the boundaries of the world? # In[15]: # choose values of dx and dy (negative works, too) dx = 1 dy = 2 r.move(dx, dy)
