This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# 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(): |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
def get_poses_landmarks(mu, N): | |
# create a list of poses | |
poses = [] | |
for i in range(N): | |
poses.append((mu[2*i].item(), mu[2*i+1].item())) | |
# create a list of landmarks | |
landmarks = [] | |
for i in range(num_landmarks): | |
landmarks.append((mu[2*(N+i)].item(), mu[2*(N+i)+1].item())) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
## slam takes in 6 arguments and returns mu, | |
## mu is the entire path traversed by a robot (all x,y poses) *and* all landmarks locations | |
def slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise): | |
## TODO: Use your initilization to create constraint matrices, omega and xi | |
omega, xi = initialize_constraints(N, num_landmarks, world_size) | |
## TODO: Iterate through each time step in the data | |
for time_step in range(len(data)): | |
## get all the motion and measurement data as you iterate through each time step |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
from helpers import make_data | |
# your implementation of slam should work with the following inputs | |
# feel free to change these input values and see how it responds! | |
# world parameters | |
num_landmarks = 5 # number of landmarks | |
N = 20 # time steps |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
def initialize_constraints(N, num_landmarks, world_size): | |
''' This function takes in a number of time steps N, number of landmarks, and a world_size, | |
and returns initialized constraint matrices, omega and xi.''' | |
middle_of_the_world = world_size / 2 | |
## Recommended: Define and store the size (rows/cols) of the constraint matrix in a variable | |
rows, cols = 2*(N + num_landmarks), 2*(N + num_landmarks) | |
## TODO: Define the constraint matrix, Omega, with two initial "strength" values |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
# define omega and xi as in the example | |
omega = np.array([[1,0,0], | |
[-1,1,0], | |
[0,-1,1]]) | |
xi = np.array([[-3], | |
[5], | |
[3]]) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
class robot: | |
#move function | |
def move(self, dx, dy): | |
x = self.x + dx + self.rand() * self.motion_noise | |
y = self.y + dy + self.rand() * self.motion_noise | |
if x < 0.0 or x > self.world_size or y < 0.0 or y > self.world_size: | |
return False |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
## Print out and display the final, resulting Gaussian | |
# set the parameters equal to the output of the Kalman filter result | |
mu = mu | |
sigma2 = sig | |
# define a range of x values | |
x_axis = np.arange(-20, 20, 0.1) | |
# create a corresponding list of gaussian values | |
g = [] |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# measurements for mu and motions, U | |
measurements = [5., 6., 7., 9., 10.] | |
motions = [1., 1., 2., 1., 1.] | |
# initial parameters | |
measurement_sig = 4. | |
motion_sig = 2. | |
mu = 0. | |
sig = 10000. #0000000001 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
def predict(mean1, var1, mean2, var2): | |
''' This function takes in two means and two squared variance terms, | |
and returns updated gaussian parameters, after motion.''' | |
# Calculate the new parameters | |
new_mean = mean1 + mean2 | |
new_var = var1 + var2 | |
return [new_mean, new_var] |
NewerOlder