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.

## Discussions

## Become a Hackaday.io Member

Create an account to leave a comment. Already have an account? Log In.