def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P']
residual = np.transpose([gps - ekf_state['x'][:2]])
H_mat = np.matrix(np.zeros([2, P.shape[0]]))
H_mat[0, 0], H_mat[1, 1] = 1, 1
R_mat = np.diag([sigmas['gps']**2, sigmas['gps']**2])
S_mat = H_mat * P * H_mat.T + R_mat
d = (np.matrix(residual)).T * np.matrix(
slam_utils.invert_2x2_matrix(np.array(S_mat))) * np.matrix(residual)
if d <= chi2.ppf(0.999, 2):
Kt = P * H_mat.T * np.matrix(
slam_utils.invert_2x2_matrix(np.array(S_mat)))
ekf_state['x'] = ekf_state['x'] + np.squeeze(
np.array(Kt * np.matrix(residual)))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.array((np.matrix(np.eye(P.shape[0])) - Kt * H_mat) * P))
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P'].copy()
x = ekf_state['x'].copy()
H = np.zeros([2, x.shape[0]])
H[:2, :2] = np.array([[1, 0], [0, 1]])
Q = np.diag([sigmas['gps']**2, sigmas['gps']**2])
S = H.dot(P).dot(H.T) + Q.T
K = P.dot(H.T).dot(slam_utils.invert_2x2_matrix(S))
r = (gps - x[:2]).reshape([2, 1])
if r.T.dot(slam_utils.invert_2x2_matrix(S)).dot(r) < 13.8:
x_new = x + K.dot(gps - x[:2])
cor_new = (np.eye(x.shape[0]) - K.dot(H)).dot(P)
cor_new = slam_utils.make_symmetric(cor_new)
ekf_state['P'] = cor_new
ekf_state['x'] = x_new
# ekf_state=slam.gps_update(gps, ekf_state, sigmas)
return ekf_state
def laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Perform a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
# Implement the EKF update for a set of range, bearing measurements.
###
if np.array(assoc).size == 0:
return ekf_state
assoc = np.array(assoc, dtype=np.int)
dtrees = np.array(trees)[:, :2]
new_ind = np.where(assoc == -1)[0]
new_trees = dtrees[new_ind, :]
prevnum = ekf_state['num_landmarks']
ekf_state = initialize_landmark(ekf_state, np.array(trees)[new_ind, :])
newnum = ekf_state['num_landmarks']
update_ind = np.where(assoc >= 0)[0]
ekf_state_prev = ekf_state.copy()
cor_prev = ekf_state_prev['P']
x_prev = ekf_state_prev['x']
for i in range(prevnum, newnum):
z = new_trees[i - prevnum, :]
zhat, H = laser_measurement_model(ekf_state_prev, i)
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
K = cor_prev.dot(H.T).dot(
slam_utils.invert_2x2_matrix(H.dot(cor_prev).dot(H.T) + Q.T))
ekf_state['x'] += K.dot(z - zhat)
ekf_state['P'] += -K.dot(H).dot(cor_prev)
for i in update_ind[::-1]:
ii = assoc[i]
z = dtrees[i, :]
zhat, H = laser_measurement_model(ekf_state_prev, ii)
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
K = cor_prev.dot(H.T).dot(
slam_utils.invert_2x2_matrix(H.dot(cor_prev).dot(H.T) + Q.T))
ekf_state['x'] += K.dot(z - zhat)
ekf_state['P'] += -K.dot(H).dot(cor_prev)
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Perform a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
# Implement the GPS update.
###
P = ekf_state['P']
dim = P.shape[0]-2
H = np.hstack((np.eye(2),np.zeros((2,dim))))
r = np.transpose([gps - ekf_state['x'][:2]])
Q = (sigmas['gps']**2)*(np.eye(2))
S = np.matmul(np.matmul(H,P),H.T) + Q
S_inv = slam_utils.invert_2x2_matrix(S)
d = np.matmul(np.matmul(r.T,S_inv),r)
if d <= chi2.ppf(0.999, 2):
K = np.matmul(np.matmul(P,H.T),S_inv)
ekf_state['x'] = ekf_state['x'] + np.squeeze(np.matmul(K,r))
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
P_temp = np.matmul((np.eye(P.shape[0])- np.matmul(K,H)),P)
ekf_state['P'] = slam_utils.make_symmetric(P_temp)
return ekf_state
def gps_update(gps, ekf_state, sigmas):
'''
Performs a measurement update of the EKF state given a GPS measurement (x,y).
Returns the updated ekf_state.
'''
###
x = ekf_state["x"].copy()
P = ekf_state["P"].copy()
H = np.zeros([2, len(x)])
H[0, 0] = 1
H[1, 1] = 1
R_t = (sigmas['gps']**2) * np.eye(2)
S = P[0:2, 0:2] + R_t
update = gps.reshape(2, 1) - x[0:2].reshape(2, 1)
inv_S = slam_utils.invert_2x2_matrix(S)
d = np.dot(np.dot(np.transpose(update), inv_S), update)
if d < 13.8:
K = np.dot(P[:, 0:2], inv_S)
ekf_state['x'] = x + np.dot(K, update).flatten()
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.dot((np.eye(len(x)) - np.dot(K, H)), P))
return ekf_state
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 laser_update(trees, assoc, ekf_state, sigmas, params):
'''
Performs a measurement update of the EKF state given a set of tree measurements.
trees is a list of measurements, where each measurement is a tuple (range, bearing, diameter).
assoc is the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as
it is too ambiguous to use.
The diameter component of the measurement can be discarded.
Returns the ekf_state.
'''
###
assoc = np.asarray(assoc)
trees = np.asarray(trees)
n = ekf_state['num_landmarks']
if assoc.shape[0] != 0:
new_tree_index = assoc == -1
new_trees = trees[new_tree_index, :]
for i in range(new_trees.shape[0]):
initialize_landmark(ekf_state, new_trees[i, :])
assoc[new_tree_index] = np.linspace(n, n + new_trees.shape[0] - 1,
new_trees.shape[0])
Q = np.diag([sigmas['range']**2, sigmas['bearing']**2])
exisiting_trees_index = assoc > -1
exisiting_trees = trees[exisiting_trees_index, :]
landmark_ids = assoc[exisiting_trees_index]
for i in range(exisiting_trees.shape[0]):
measure = exisiting_trees[i, 0:2].reshape(2, 1)
zhat, H = laser_measurement_model(ekf_state, int(landmark_ids[i]))
P = ekf_state['P'].copy()
K = np.dot(
np.dot(P, H.transpose()),
slam_utils.invert_2x2_matrix(
np.dot(np.dot(H, P), H.transpose()) + Q))
ekf_state['x'] = ekf_state['x'] + np.dot(K, (measure - zhat))[:, 0]
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(
np.dot((np.eye(P.shape[0]) - np.dot(K, H)), P))
###
return ekf_state
def gps_update(gps, ekf_state, sigmas):
"""
Perform a measurement update of the EKF state given a GPS measurement (x,y)
Returns the updated ekf_state.
:param gps:
:param ekf_state:
:param sigmas:
:return:
"""
# measurement noise
gps_noise = sigmas['gps']
# check validity
r = gps.reshape((2, 1)) - ekf_state['x'][:2].copy().reshape((2, 1))
S = slam_utils.make_symmetric(ekf_state['P'][0:2, 0:2].copy() + 10e-6)
d = np.dot(np.dot(r.T, slam_utils.invert_2x2_matrix(S)), r)
if d > 13.8:
return ekf_state
### Kalman Gain ###
x = ekf_state['x'][0:3].copy().reshape((3, 1))
P = ekf_state['P'][0:3, 0:3].copy()
H = np.asarray([[1, 0, 0], [0, 1, 0]])
Q = np.diag((gps_noise**2, gps_noise**2))
# Kalman Gain update
K_t = np.dot(P, H.T)
K_t = np.dot(K_t,
slam_utils.invert_2x2_matrix(np.dot(np.dot(H, P), H.T) + Q))
### Measurement Model ###
# update mu and sigma based on measurement
x_t = x + np.dot(K_t, r)
P_t = np.dot(np.identity(3) - np.dot(K_t, H), P)
ekf_state['x'][0:3] = x_t.reshape(-1)
ekf_state['P'][0:3, 0:3] = slam_utils.make_symmetric(P_t)
return ekf_state
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 laser_update(trees, assoc, ekf_state, sigmas, params):
"""
The diameter component of the measurement can be discarded
:param trees: a list of measurements, where each measurement is a tuple (range, bearing, diameter)
:param assoc: the data association for the given set of trees, i.e. trees[i] is an observation of the
ith landmark. If assoc[i] == -1, initialize a new landmark with the function initialize_landmark
in the state for measurement i. If assoc[i] == -2, discard the measurement as it is too ambiguous to use
:param ekf_state:
:param sigmas:
:param params:
:return:
Returns the ekf_state
"""
# measurement noise
range_noise = sigmas['range']
bearing_noise = sigmas['bearing']
Q = np.diag((range_noise**2, bearing_noise**2))
for i in range(len(trees)):
# add state with new landmark
if assoc[i] == -1:
ekf_state = initialize_landmark(ekf_state, trees[i])
# discard ambiguous measurement
elif assoc[i] == -2:
continue
# update state with existing landmark
else:
landmark_id = assoc[i]
z_t, H = laser_measurement_model(ekf_state, landmark_id)
x = ekf_state['x']
P = ekf_state['P']
N = len(ekf_state['x'])
# Kalman Gain update
K_t = np.dot(P, H.T)
K_t = np.dot(
K_t,
slam_utils.invert_2x2_matrix(np.dot(np.dot(H, P), H.T) + Q))
z = np.asarray([trees[i][0], trees[i][1]]).reshape((2, 1))
ekf_state['x'] = x + np.dot(K_t, (z - z_t)).reshape(-1)
ekf_state['P'] = np.dot(np.identity(N) - np.dot(K_t, H), P)
ekf_state['x'][2] = slam_utils.clamp_angle(ekf_state['x'][2])
ekf_state['P'] = slam_utils.make_symmetric(ekf_state['P'] + 10e-6)
return ekf_state
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
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