def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
#0-----associate/match-----[alpha]-----ambiguous-----[beta]-----new_landmark-----
#pdb.set_trace()
alpha = chi2.ppf(0.95, df=2)
beta = chi2.ppf(0.999, df=2)
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
measurements = np.array(measurements)[:, 0:2]
zhat = np.zeros([ekf_state["num_landmarks"], 2])
S = np.zeros([ekf_state["num_landmarks"], 2, 2])
Q = np.diag(
np.array([
sigmas['range'] * sigmas['range'],
sigmas['bearing'] * sigmas['bearing']
]))
for j in range(ekf_state["num_landmarks"]):
zhat[j], H = laser_measurement_model(ekf_state, j)
S[j] = np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T
M = alpha * np.ones((measurements.shape[0],
ekf_state["num_landmarks"] + measurements.shape[0]))
for i in range(measurements.shape[0]):
residuals = measurements[i] - zhat
for j in range(ekf_state["num_landmarks"]):
mahalanobis_dist = np.matmul(
residuals[j], np.matmul(np.linalg.inv(S[j]), residuals[j].T))
M[i, j] = mahalanobis_dist
matches = slam_utils.solve_cost_matrix_heuristic(np.copy(M))
matches.sort()
assoc = list(range(measurements.shape[0]))
for k in range(measurements.shape[0]):
if (matches[k][1] >= ekf_state['num_landmarks']):
if (np.amin(M[k, 0:ekf_state['num_landmarks']]) >
beta): #new landmark
assoc[matches[k][0]] = -1
else: #ambiguous
assoc[matches[k][0]] = -2
else: #matched
assoc[matches[k][0]] = matches[k][1]
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
m = len(measurements)
n = ekf_state['num_landmarks']
if m > n:
M = np.full((m, n + m), 6.1)
else:
M = np.zeros((m, n))
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
sigma = ekf_state['P']
for i in range(m):
z = np.zeros((2, 1))
z[0, 0] = measurements[i][0]
z[1, 0] = measurements[i][1]
for j in range(n):
zhat, H = laser_measurement_model(ekf_state, j)
r = z - zhat
S = np.matmul(H, np.matmul(sigma, H.T)) + Q
M[i, j] = np.matmul(r.T, np.matmul(np.linalg.inv(S), r))
pairs = slam_utils.solve_cost_matrix_heuristic(np.copy(M))
assoc = [0] * len(measurements)
for i, j in pairs:
if j >= n:
min_element = np.amin(M[i, 0:n - 1])
#13.8
if (min_element > 18.2): #chi(0.9999)
assoc[i] = -1
else:
assoc[i] = -2
else:
if M[i, j] > 6:
if M[i, j] > 18.2:
assoc[i] = -1
else:
assoc[i] = -2
else:
assoc[i] = j
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
Q = np.eye(2, 2)
Q[0][0] = sigmas['range']**2
Q[1][1] = sigmas['bearing']**2
xv, yv, phi = ekf_state['x'][:3]
landmark_state = ekf_state['x'][3:]
landmarks = np.zeros((2, ekf_state['num_landmarks']))
landmarks[0:] = landmark_state[::2]
landmarks[1:] = landmark_state[1::2]
landmark_rb = np.zeros((2, ekf_state['num_landmarks']))
landmark_rb[0, :] = np.sqrt((landmarks[0, :] - xv)**2 +
(landmarks[1, :] - yv)**2)
landmark_rb[1, :] = np.arctan2(landmarks[1, :] - yv,
landmarks[0, :] - xv) - phi + np.pi / 2
M = np.zeros((len(measurements), ekf_state['num_landmarks']))
# r = measurements - landmark_rb
# H = np.zeros((2, 3+2*ekf_state['num_landmarks']))
r = np.zeros((2, ))
for i in range(len(measurements)):
for j in range(ekf_state['num_landmarks']):
zhat, H = laser_measurement_model(ekf_state, j)
Sinv = np.linalg.inv(
np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q)
r[0] = measurements[i][0] - zhat[0]
r[1] = measurements[i][1] - zhat[1]
dist = np.matmul(np.matmul(r.T, Sinv), r)
M[i, j] = dist
assoc = [-2] * len(measurements)
padding = np.zeros(
(len(measurements), len(measurements))) + chi2.ppf(0.95, df=2)
cost = np.hstack((M, padding))
result = slam_utils.solve_cost_matrix_heuristic(cost.copy())
for index, (i, j) in enumerate(result):
if j < ekf_state['num_landmarks']:
assoc[i] = j
elif np.min(M[i, :ekf_state['num_landmarks']]) > chi2.ppf(0.99, df=2):
assoc[i] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
trees = measurements
append_M = np.ones(
(len(measurements), len(measurements))) * chi2.ppf(0.95, df=2)
measurements = [(tree[0], tree[1]) for tree in trees]
assoc = -1 * np.ones((len(measurements), ))
Q = np.eye(2, 2)
Q[0][0] = sigmas['range'] * sigmas['range']
Q[1][1] = sigmas['bearing'] * sigmas['bearing']
M = np.zeros((len(measurements), ekf_state['num_landmarks']))
for i in range(len(measurements)):
m = np.asarray(measurements[i])
for j in range(ekf_state['num_landmarks']):
z_hat, H = laser_measurement_model(ekf_state, j)
each_r = (m - z_hat).T
S = np.matmul(np.matmul(H, ekf_state['P']), H.T) + Q.T
mahal = np.matmul(np.matmul(each_r.T, np.linalg.inv(S)), each_r)
M[i][j] = mahal
M = np.hstack((M, append_M))
result = slam_utils.solve_cost_matrix_heuristic(M.copy())
assoc = [-2] * len(measurements)
for (i, j) in result:
if j < ekf_state['num_landmarks']:
assoc[i] = j
else:
if np.min(M[i, :ekf_state['num_landmarks']]) > chi2.ppf(0.99,
df=2):
assoc[i] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
# print(ekf_state["num_landmarks"])
P = ekf_state['P']
R = np.diag([sigmas['range']**2, sigmas['bearing']**2])
A = np.full((len(measurements), len(measurements)), chi2.ppf(0.96, df=2))
cost_mat = np.full((len(measurements), ekf_state['num_landmarks']),
chi2.ppf(0.96, df=2))
for k in range(0, len(measurements)):
for j in range(0, ekf_state['num_landmarks']):
z_hat, H = laser_measurement_model(ekf_state, j)
# print(measurements[k][0:2])
r = np.array(np.array(measurements[k][0:2]) - np.array(z_hat))
S_inv = np.linalg.inv(
np.matmul(np.matmul(H, P), np.transpose(H)) + R)
MD = np.matmul(np.matmul(np.transpose(r), S_inv), r)
cost_mat[k, j] = MD
cost_mat_conc = np.concatenate((cost_mat, A), axis=1)
temp1 = np.copy(cost_mat)
results = slam_utils.solve_cost_matrix_heuristic(temp1)
assoc = np.zeros(len(measurements), dtype=np.int32)
for k in range(0, len(results)):
# print(cost_mat[results[k][0],results[k][1]])
if cost_mat_conc[results[k][0], results[k][1]] > chi2.ppf(0.99, df=2):
assoc[results[k][0]] = -1
elif cost_mat_conc[results[k][0], results[k][1]] >= chi2.ppf(0.95,
df=2):
assoc[results[k][0]] = -2
else:
assoc[results[k][0]] = results[k][1]
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
# assoc = np.zeros((len(measurements),1))
# assoc = np.asarray([-1])
return [-1 for m in measurements]
###
# Implement this function.
###
Qt = np.diag([sigmas['range']**2, sigmas['bearing']**2])
zk = np.asarray(measurements)
n_landmarks = ekf_state['num_landmarks']
n_measurements = zk.shape[0]
M = np.zeros((n_measurements, n_landmarks))
# Thresholds for classifying as New or Ambiguous Landmarks
alpha = chi2.ppf(0.95, 2)
beta = chi2.ppf(0.99, 2)
for k in range(n_landmarks):
zhat, H = laser_measurement_model(ekf_state, k)
S = np.matmul(H, np.matmul(ekf_state['P'], H.T)) + Qt
Sinv = slam_utils.invert_2x2_matrix(S)
innovation = zk[:, :2] - zhat.T
M[:, k] = np.sum(innovation.T * np.matmul(Sinv, innovation.T), axis=0)
# Augmented Matrix with Cost Matrix
pairs = slam_utils.solve_cost_matrix_heuristic(
np.hstack((M, alpha * np.ones((n_measurements, n_measurements)))))
pairs.sort()
pairs = np.asarray(pairs)
assoc = pairs[:, 1]
assoc = np.where(assoc >= n_landmarks, -1, assoc)
for i in range(assoc.shape[0]):
if assoc[i] == -1 and np.any(M[i, :] < beta):
assoc[i] = -2
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
assoc = [-1 for m in measurements]
n = ekf_state['num_landmarks']
m = len(measurements)
M = np.zeros([m, n])
Q = np.eye(2)
Q[0, 0] = sigmas['range']**2
Q[1, 1] = sigmas['bearing']**2
for j in range(n):
zhat, H = laser_measurement_model(ekf_state, j)
S = multi_dot([H, ekf_state['P'], H.T]) + Q
for i, item in enumerate(measurements):
r = np.array([item[0], item[1]]).reshape((2, 1)) - zhat
M[i, j] = multi_dot([r.T, np.linalg.inv(S), r])
alpha = chi2.ppf(0.95, df=2)
K = alpha * np.ones([m, m])
Mk = np.hstack([M, K])
result = slam_utils.solve_cost_matrix_heuristic(Mk)
beta = chi2.ppf(0.99, df=2)
for i, j in result:
if j < n:
assoc[i] = j
else:
assoc[i] = -2
if min(M[i, :]) > beta:
assoc[i] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
P = ekf_state["P"].copy()
n = ekf_state['num_landmarks']
m = len(measurements)
cost_matrix = np.zeros([m, n + 1])
cost_matrix[:, n] = chi2.ppf(0.99, 2)
R = np.diag([sigmas['range']**2, sigmas['bearing']**2])
for i in range(n):
zhat, H = laser_measurement_model(ekf_state, i)
S = np.dot(np.dot(H, P), H.transpose()) + R
inv_S = slam_utils.invert_2x2_matrix(S)
for j in range(m):
update = np.asarray(measurements[j][0:2]) - zhat.flatten()
cost_matrix[j, i] = np.dot(np.dot(update.transpose(), inv_S),
update)
result = slam_utils.solve_cost_matrix_heuristic(cost_matrix)
assoc = [0] * m
for i, j in result:
if j < ekf_state["num_landmarks"]:
assoc[i] = j
else:
if min(cost_matrix[i, 0:]) < chi2.ppf(0.99, 2):
assoc[i] = -2
else:
assoc[i] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
return [-1 for m in measurements]
n_lmark = ekf_state['num_landmarks']
n_scans = len(measurements)
M = np.zeros((n_scans, n_lmark))
Q_t = np.array([[sigmas['range']**2, 0], [0, sigmas['bearing']**2]])
alpha = chi2.ppf(0.95, 2)
beta = chi2.ppf(0.99, 2)
A = alpha*np.ones((n_scans, n_scans))
for i in range(n_lmark):
zhat, H = laser_measurement_model(ekf_state, i)
S = np.matmul(H, np.matmul(ekf_state['P'],H.T)) + Q_t
Sinv = slam_utils.invert_2x2_matrix(S)
for j in range(n_scans):
temp_z = measurements[j][:2]
res = temp_z - np.squeeze(zhat)
M[j, i] = np.matmul(res.T, np.matmul(Sinv, res))
M_new = np.hstack((M, A))
pairs = slam_utils.solve_cost_matrix_heuristic(M_new)
pairs.sort()
pairs = list(map(lambda x:(x[0],-1) if x[1]>=n_lmark else (x[0],x[1]),pairs))
assoc = list(map(lambda x:x[1],pairs))
for i in range(len(assoc)):
if assoc[i] == -1:
for j in range(M.shape[1]):
if M[i, j] < beta:
assoc[i] = -2
break
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurement
return [-1 for m in measurements]
R = np.array([[sigmas['range'] ** 2, 0], [0, sigmas['bearing'] ** 2]])
m = ekf_state['num_landmarks']
measurement_size = len(measurements)
# To hold the cost. rows are old measurements and columns are new ones
M = np.zeros((measurement_size, m))
# todo vectorize this
measurements_np = np.array(measurements)[:, :2]
for j in range(m):
curr_pos, H = laser_measurement_model(ekf_state, j)
S = np.matmul(np.matmul(H, ekf_state['P']), H.T) + R
for i in range(measurement_size):
# x_L, y_L = slam_utils.tree_to_global_xy(measurements_np[np.newaxis, i], ekf_state)
# x, y = slam_utils.tree_to_global_xy(curr_pos[np.newaxis,:], ekf_state)
# M[i,j] = np.sqrt((x_L - x)**2 + (y_L - y)**2)
r = measurements_np[i] - curr_pos
M[i, j] = np.matmul(np.matmul(r.T, np.linalg.inv(S)), r)
matches = slam_utils.solve_cost_matrix_heuristic(M.copy())
assoc = [-1] * measurement_size
for i in matches:
if M[i] > 15:
assoc[i[0]] = -1
elif M[i] > chi2.ppf(.995, df=2):
assoc[i[0]] = -2
else:
assoc[i[0]] = i[1]
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
# print('assoc')
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
else:
B = 5.991 * np.ones((len(measurements), len(measurements)))
M = np.zeros((len(measurements), ekf_state['num_landmarks']))
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
P = ekf_state['P']
Zm = np.array(measurements)[:, 0:2]
for i in range(ekf_state['num_landmarks']):
zhat, H = laser_measurement_model(ekf_state, i)
Sinv = np.linalg.inv(np.matmul(np.matmul(H, P), H.T) + Q.T)
r = Zm - zhat
M[:, i] = mahalanobisDist(r, Sinv)
Mpad = np.concatenate((M, B), axis=1)
C = slam_utils.solve_cost_matrix_heuristic(Mpad)
assoc = [-2] * len(measurements)
for c in C:
if c[1] < ekf_state['num_landmarks']:
assoc[c[0]] = c[1]
elif np.min(M[c[0], :]) > 9.21: # 9.21
assoc[c[0]] = -1
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
cor = ekf_state['P']
num_obs = len(measurements)
measurements = np.array(measurements)[:, :2]
assoc = np.full([
num_obs,
], -2)
num_mark = ekf_state['num_landmarks']
M = np.zeros([num_obs, num_mark])
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
for i in range(num_mark):
zhat, H = laser_measurement_model(ekf_state, i)
S = H.dot(cor).dot(H.T) + Q.T
for j in range(num_obs):
r = (measurements[j, :] - zhat).reshape([2, 1])
M[j, i] = r.T.dot(slam_utils.invert_2x2_matrix(S)).dot(r)
if num_mark >= num_obs:
pos = np.array(slam_utils.solve_cost_matrix_heuristic(M.copy()))
posx = pos[:, 0]
posy = pos[:, 1]
value = M[posx, posy]
ind_update = np.where(value >= 9.2103)
ind_discard = np.where((value > 5.9915) & (value < 9.2103))
pos[ind_update, 1] = -1
pos[ind_discard, 1] = -2
assoc[pos[:, 0]] = pos[:, 1]
else:
A1 = np.full([num_obs, num_obs], 5.99)
A2 = np.full([num_obs, num_obs], 9.21)
M_exp1 = np.concatenate([M, A1], axis=1)
M_exp2 = np.concatenate([M, A2], axis=1)
pos1 = np.array(slam_utils.solve_cost_matrix_heuristic(M_exp1.copy()))
pos2 = np.array(slam_utils.solve_cost_matrix_heuristic(M_exp2.copy()))
posx1 = pos1[:, 0]
posy1 = pos1[:, 1]
posx2 = pos2[:, 0]
posy2 = pos2[:, 1]
assoc[posx1[posy1 < num_mark]] = posy1[posy1 < num_mark]
assoc[posx2[posy2 >= num_mark]] = -1
# assoc=slam.compute_data_association(ekf_state, measurements, sigmas, params)
return assoc
import numpy as np import slam_utils m = np.array([[1, 2, 4], [6, 7, 8], [6, 3, 9], [6, 3, 9], [6, 3, 9]]) print(np.argsort(m.min(axis=1))) a = slam_utils.solve_cost_matrix_heuristic(m) print(m)
def compute_data_association(ekf_state, measurements, sigmas, params):
"""
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
:param ekf_state:
:param measurements:
:param sigmas:
:param params:
:return:
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
"""
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for _ in measurements]
range_noise = sigmas['range']
bearing_noise = sigmas['bearing']
Q = np.diag((range_noise**2, bearing_noise**2))
n_measurements = len(measurements)
n_landmarks = ekf_state["num_landmarks"]
P = ekf_state['P'].copy()
assoc = [-2 for _ in range(n_measurements)]
M = np.zeros((n_measurements, n_landmarks))
for i in range(n_landmarks):
z_t, H = laser_measurement_model(ekf_state, i)
for j in range(n_measurements):
z = np.asarray(measurements[j][0:2]).reshape((2, 1))
r = z - z_t
S = np.dot(np.dot(H, P), H.T) + Q
d = np.dot(np.dot(r.T, slam_utils.invert_2x2_matrix(S)), r)
M[j, i] = d
# threshold for ambiguity
alpha = chi2.ppf(0.95, df=2)
A = alpha * np.ones((n_measurements, n_measurements))
M_new = np.concatenate((M, A), axis=1)
result = slam_utils.solve_cost_matrix_heuristic(M_new)
result = np.asarray(result)
update_index = result[:, 1] < n_landmarks
result_update = result[update_index]
for i in range(len(result_update)):
assoc[result_update[i, 0]] = result_update[i, 1]
remain_index = ~update_index
for i in range(len(M)):
if (remain_index[i]
and np.min(M[result[i, 0], :]) >= chi2.ppf(0.9999999, df=2)):
assoc[result[i, 0]] = -1
elif (remain_index[i]
and np.min(M[result[i, 0], :]) < chi2.ppf(0.9999999, df=2)):
assoc[result[i, 0]] = -2
return assoc
def compute_data_association(ekf_state, measurements, sigmas, params):
'''
Computes measurement data association.
Given a robot and map state and a set of (range,bearing) measurements,
this function should compute a good data association, or a mapping from
measurements to landmarks.
Returns an array 'assoc' such that:
assoc[i] == j if measurement i is determined to be an observation of landmark j,
assoc[i] == -1 if measurement i is determined to be a new, previously unseen landmark, or,
assoc[i] == -2 if measurement i is too ambiguous to use and should be discarded.
'''
if ekf_state["num_landmarks"] == 0:
# set association to init new landmarks for all measurements
return [-1 for m in measurements]
###
# Implement this function.
###
state = ekf_state['x']
xv = state[0]
yv = state[1]
phi = state[2]
R = np.eye(2)
R[0, 0] = sigmas['range']**2
R[1, 1] = sigmas['bearing']**2
num_measurements = len(measurements)
num_landmarks = ekf_state["num_landmarks"]
M = np.ones(
(num_measurements, num_measurements + num_landmarks)) * chi2.ppf(
0.95, 2)
for i, m in enumerate(measurements):
for j in range(num_landmarks):
zhat, full_H = laser_measurement_model(ekf_state, j)
# P = np.zeros((5,5))
# P[:3, :3] = ekf_state['P'][:3, :3]
# P[3:, 3:] = ekf_state['P'][3+j*2:3+(j+1)*2, 3+j*2:3+(j+1)*2]
# P[3:, :3] = ekf_state['P'][3+j*2:3+(j+1)*2, :3]
# P[:3, 3:] = ekf_state['P'][:3, 3+j*2:3+(j+1)*2]
P = ekf_state['P']
# H = np.zeros((2,5))
# H[:2, :3] = full_H[:2, :3]
# H[:2, 3:] = full_H[:2, 3+j*2:3+(j+1)*2]
H = full_H
r = np.vstack([m[0], m[1]]) - zhat
S = np.dot(np.dot(H, P), H.T) + R
M[i, j] = np.dot(np.dot(r.T, np.linalg.inv(S)), r)
matchings = slam_utils.solve_cost_matrix_heuristic(M.copy())
# row_ind, col_ind = linear_sum_assignment(M.copy())
# matchings = list(zip(row_ind, col_ind))
assoc = [-2 for m in measurements]
for m in matchings:
if m[1] >= num_landmarks and np.min(
M[m[0], :num_landmarks]) > chi2.ppf(0.99, 2):
assoc[m[0]] = -1
elif m[1] >= num_landmarks:
continue
else:
assoc[m[0]] = m[1]
return assoc
