In order to do SLAM compute, you need to update the values in the 2D Ω matrix and ξ vector, to account for motion and measurement constraints in the x and y directions.

## Optimized implementation of Graph SLAM.

## 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):
    
    coefficients = [1, -1, -1, 1]
    
    # initialize the constraints
    initial_omega_1, initial_xi_1, initial_omega_2, initial_xi_2 = \\
                                   initialize_constraints(N, num_landmarks, world_size)
    
    ## get all the motion and measurement data as you iterate
    for i in range(len(data)):
        
        landmarks = data[i][0]  # measurement
        motion = data[i][1]  # motion
        
        # setting measurement constraints
        for landmark in landmarks:
            
            # calculate indices in the same order as coefficients (to meaningfully add)
            index1 = [i, i, N+landmark[0], N+landmark[0]]
            index2 = [i, N+landmark[0], i, N+landmark[0]]
            
            # dx update
            initial_omega_1[index1, index2] = initial_omega_1[index1, index2] + \\
                              np.divide(coefficients, measurement_noise)
            initial_xi_1[[i, N+landmark[0]]] = initial_xi_1[[i, N+landmark[0]]] + \\
                np.divide([-landmark[1], landmark[1]], measurement_noise)
            
            # dy update
            initial_omega_2[index1, index2] = initial_omega_2[index1, index2] + \\
                                        np.divide(coefficients, measurement_noise)
            initial_xi_2[[i, N+landmark[0]]] = initial_xi_2[[i, N+landmark[0]]] + \\
                          np.divide([-landmark[2], landmark[2]], measurement_noise)
        
        index1 = [i, i, i+1, i+1]
        index2 = [i, i+1, i, i+1]
        
        # dx update
        initial_omega_1[index1, index2] = initial_omega_1[index1, index2] + \\
                                       np.divide(coefficients, motion_noise)
        initial_xi_1[[i, i+1]] = initial_xi_1[[i, i+1]] + \\
                                np.divide([-motion[0], motion[0]], motion_noise)
        
        # dy update
        initial_omega_2[index1, index2] = initial_omega_2[index1, index2] + \\
                                       np.divide(coefficients, motion_noise)
        initial_xi_2[[i, i+1]] = initial_xi_2[[i, i+1]] + 
                            np.divide([-motion[1], motion[1]], motion_noise)
        
    ## To update the constraint matrix/vector to account for all measurements, 
    # measurement noise, motion and motion noise. Compute best estimate of poses
    # and landmark positions using the formula, omega_inverse * Xi
    mu_1 = np.linalg.inv(np.matrix(initial_omega_1)) * \\
                    np.expand_dims(initial_xi_1, 0).transpose()
    mu_2 = np.linalg.inv(np.matrix(initial_omega_2)) * \\
                    np.expand_dims(initial_xi_2, 0).transpose()
    
    mu = []
    for i in range(len(mu_1)):
        mu.extend((mu_1[i], mu_2[i]))
    
    return mu

 The complete source code and results of Custom SLAM implementation can be found in the IPython notebook here.