main_slam.py

## 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
        measurement = data[time_step][0]
        motion = data[time_step][1]
        
        dx = motion[0]         # distance to be moved along x in this time_step
        dy = motion[1]         # distance to be moved along y in this time_step
        
        #Consider the robot moves from (x0,y0) to (x1,y1) in this time_step
        
        #even numbered columns of omega correspond to x values
        x0 = (time_step * 2)   #x0 = 0,2,4,...
        x1 = x0 + 2            #x1 = 2,4,6,...
        
        #odd numbered columns of omega correspond to y values
        y0 = x0 + 1            #y0 = 1,3,5,...
        y1 = y0 + 2            #y1 = 3,5,7,...
        
        actual_m_noise = 1.0/measurement_noise
        actual_n_noise = 1.0/motion_noise
    ## TODO: update the constraint matrix/vector(omega/xi) to account for all *measurements*
    ## this should be a series of additions that take into account the measurement noise
        for landmark in measurement:
            lM = landmark[0]            # landmark id
            dx_lM = landmark[1]         # separation along x from current position
            dy_lM = landmark[2]         # separation along y from current position
            
            L_x0 = (N*2) + (lM*2)       # even-numbered columns have x values of landmarks
            L_y0 = L_x0 + 1             # odd-numbered columns have y values of landmarks

            # update omega values corresponding to measurement between x0 and Lx0
            omega[x0][x0] += actual_m_noise
            omega[L_x0][L_x0] += actual_m_noise
            omega[x0][L_x0] += -actual_m_noise
            omega[L_x0][x0] += -actual_m_noise
            
            # update omega values corresponding to measurement between y0 and Ly0
            omega[y0][y0] += actual_m_noise
            omega[L_y0][L_y0] += actual_m_noise
            omega[y0][L_y0] += -actual_m_noise
            omega[L_y0][y0] += -actual_m_noise
            
            # update xi values corresponding to measurement between x0 and Lx0
            xi[x0]  -= dx_lM/measurement_noise
            xi[L_x0]  += dx_lM/measurement_noise
            
            # update xi values corresponding to measurement between y0 and Ly0
            xi[y0]  -= dy_lM/measurement_noise
            xi[L_y0] += dy_lM/measurement_noise
            
            
        ## TODO: update the constraint matrix/vector(omega/xi) to account for all *motion* from from (x0,y0) to (x1,y1) and motion noise
        omega[x0][x0] += actual_n_noise
        omega[x1][x1] += actual_n_noise
        omega[x0][x1] += -actual_n_noise
        omega[x1][x0] += -actual_n_noise
        
        omega[y0][y0] += actual_n_noise
        omega[y1][y1] += actual_n_noise
        omega[y0][y1] += -actual_n_noise
        omega[y1][y0] += -actual_n_noise
        
        xi[x0] -= dx/motion_noise
        xi[y0] -= dy/motion_noise
        
        xi[x1] += dx/motion_noise
        xi[y1] += dy/motion_noise
    
    ## TODO: After iterating through all the data
    ## Compute the best estimate of poses and landmark positions
    ## using the formula, omega_inverse * Xi
    inverse_of_omega = np.linalg.inv(np.matrix(omega))
    mu = inverse_of_omega * xi
    
    return mu