og_omega_xi.py

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
    omega = np.zeros(shape = (rows, cols))
    ## for the initial x, y location of our robot
    #omega = [0]
    
    omega[0][0], omega[1][1] = 1,1
    
    ## TODO: Define the constraint *vector*, xi
    ## you can assume that the robot starts out in the middle of the world with 100% confidence
    #xi = [0]
    xi = np.zeros(shape = (rows, 1))
    xi[0][0] = middle_of_the_world
    xi[1][0] = middle_of_the_world
    
    return omega, xi