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