def delete_landmark(self, ind): self.L -= 1 self.dim_state -= 2 del self.observation_counter[ind] # remove landmark from all ensemble members # print([_ for _ in range(self.state_without_landmarks + ind, self.state_without_landmarks + ind+self.dim_landmarks)]) self.ensemble = np.delete(self.ensemble, [_ for _ in range(self.state_without_landmarks + self.dim_landmarks*ind, self.state_without_landmarks + self.dim_landmarks*ind+self.dim_landmarks)], axis=1) # re-calculate estimation self.state = calc_mean(self.ensemble, 2)
def sigma_pos(self): """ Overwrite to only compute covariance upon call! :return: The covariance approximation C """ x_dash = calc_mean(self.ensemble[:, :3], 2) X = (1.0 / sqrt(self.N - 1)) * (self.ensemble[:, :3] - x_dash).T C = np.dot(X, X.T) return C
def get_sigma(self): """ Overwrite to only compute covariance upon call! :return: The covariance approximation C """ x_dash = calc_mean(self.ensemble, 2) X = (1.0 / sqrt(self.N - 1)) * (calc_diff(self.ensemble, x_dash, 2)).T C = np.dot(X, X.T) return C
def add_landmark(self, z): self.L += 1 self.observation_counter.append(0) self.dim_state += 2 tmp_ensemble = np.zeros((self.N, self.dim_state)) for j in range(self.N): tmp_ensemble[j] = np.concatenate((self.ensemble[j], get_coordinate_from_range_bearing(z, self.ensemble[j]))) self.ensemble = tmp_ensemble # don't forget to update the state self.state = calc_mean(self.ensemble, 2)
def ACE(data, s, window_size=3):
"""
ACE Global Algorithm
:param data: the data to run on
:param s: the signal to match
:return: the results of ace local
"""
step = window_size // 2
Xdim, Ydim, Bands = data.shape
# calc valid values
min_i = min_j = step
max_i, max_j = Xdim - step - 1, Ydim - step - 1
ace = np.zeros((Xdim, Ydim))
for i, row in enumerate(data):
if i not in range(min_i, max_i):
continue
for j, x in enumerate(row):
if j not in range(min_j, max_j):
continue
# calc window
window = data[(i - step):(i + step + 1),
(j - step):(j + step + 1), :]
# calc mu and sigma
mu = calc_mean(window)
sigma = calc_cov(window)
# what if its not invertable ... ?
if (np.linalg.det(sigma) < 0.00001):
Pad = 0.1 * np.identity(Bands)
# make an "identity matrix" and put some small padding along it!
sigma = np.add(sigma, Pad)
sigma_inv = np.linalg.inv(sigma)
# calc num
numerator = np.square(
np.matmul(np.matmul(s - mu, sigma_inv),
np.matrix.transpose(x - mu)))
# calc denom
lowerl = np.matmul(np.matmul(s - mu, sigma_inv),
np.matrix.transpose(s - mu))
lowerr = np.matmul(np.matmul(x - mu, sigma_inv),
np.matrix.transpose(x - mu))
denominator = lowerl * lowerr
# divide and store!
ace[i, j] = numerator / denominator
return ace
def predict(self, f, u=0, f_args=(), landmark_white_noise=1.0): """ Predict the next prior. :param f: The state transition function :param u: The control arguments for the state transition function :param f_args: Additional arguments for the state transition function :param landmark_white_noise: The spread for the white noise added to each landmark """ s_landmarks = [0] * (self.dim_landmarks * self.L) for ind in range((self.dim_landmarks * self.L)): if abs(self.state[ind + self.state_without_landmarks]) < self.eps: s_landmarks[ind] = 0.0 else: s_landmarks[ind] = landmark_white_noise # s = np.concatenate((self.Q.diagonal(), s_landmarks)) s = np.concatenate(([0.0, 0.0, 0.0], s_landmarks)) self.ensemble = np.asarray( [f(member, u, self.Q, f_args, do_noise=True) + np.random.normal(0, s, self.dim_state) for member in self.ensemble]) self.normalize_ensemble_angles() self.state = calc_mean(self.ensemble, 2)
def update(self, z, h, ind, h_args=(), full_ensemble=False): """ Update the current prediction using incoming measurements :param full_ensemble: :param z: The incoming measurement :param h: The measurement function :param ind: The index of the landmark according to the measurement :param h_args: Additional arguments for the state transition function """ # according to roth_2017, we do not need a batch update, but can iterate over the updates individually. # check if landmark was never seen before position_of_landmark_in_state_vector = 2 * ind + self.state_without_landmarks new_landmark = abs(self.state[position_of_landmark_in_state_vector]) < self.eps \ and abs(self.state[position_of_landmark_in_state_vector + 1]) < self.eps if new_landmark: # remember landmark - IN EACH MEMBER! for i in range(self.N): self.ensemble[i, position_of_landmark_in_state_vector:position_of_landmark_in_state_vector + self.dim_landmarks] \ = get_coordinate_from_range_bearing(z, self.ensemble[i]) if full_ensemble: tmp_ensemble = self.ensemble col_indx = [col_ind for col_ind in range(self.dim_state)] else: # lets try something: select only robot state and landmark state and form new tmp state # row_indx -> ensemble member, col_indx -> value in ensemble member col_indx = np.array([v for v in range(self.state_without_landmarks)] + [b for b in range( position_of_landmark_in_state_vector, position_of_landmark_in_state_vector + self.dim_landmarks)]) tmp_ensemble = self.ensemble[:, col_indx] tap_N = len(tmp_ensemble) # Use this function for using correct landmark # x_h = np.asarray([h(member, h_args) for member in tmp_ensemble]) if full_ensemble: x_h = np.asarray([h(member, self.get_ith_landmark_from_member(member, ind)) for member in tmp_ensemble]) else: x_h = np.asarray([h(member, self.get_ith_landmark_from_member(member, 0)) for member in tmp_ensemble]) # Angles can not be "meaned" like normal values! # Therefore: Use own mean function # Calculate the mean of the chosen ensemble x_dash = calc_mean(tmp_ensemble, 2) # Calculate the expected measurement for each ensemble member y_dash = calc_mean(x_h, 1) # Tang_2015 argue, that the following has to hold: y_dash == h(np.mean(tmp_ensemble, axis=0), h_args) # This is the normal calculation X = (1.0 / sqrt(tap_N - 1)) * (calc_diff(tmp_ensemble, x_dash, 2)).T Y = (1.0 / sqrt(tap_N - 1)) * (calc_diff(x_h, y_dash, 1)).T # Follow simple calculation for computation of Kalman gain D = np.dot(Y, Y.T) + self.R K = np.dot(X, Y.T) K = np.dot(K, inv(D)) v_r = np.random.multivariate_normal([0] * self.dim_z, self.R, self.N) for j in range(self.N): # self.ensemble[j, col_indx] += np.dot(K, z + v_r[j] - x_h[j]) diff = z - x_h[j] + v_r[j] diff[1] = normalize_angle(diff[1]) update = np.dot(K, diff) self.ensemble[j, col_indx] += update self.normalize_ensemble_angles() self.state = calc_mean(self.ensemble, 2)
def save_and_print(self):
# Auto-save
if self.iter % self.autosave_iter == 0:
if self.use_animation:
self.animation.save_file(self.iter, self.log_dir)
if self.agent.name == 'rl':
self.agent.save_file(self.log_dir, self.iter)
# 결과 출력 코드
# # 파일로 저장
filename = "results"
extension = ".csv"
if not os.path.isfile(self.log_dir + filename + extension):
headstr = 'Iteration, num_f_survived, total rewards, loss'
else:
headstr = False
tempdic_rawdata = {"%d" % self.iter: "%d, %.2f, %.4f" % (self.env.num_f_survived, self.episode_reward, calc_mean(self.agent.loss_history))}
write_data(self.log_dir, data=tempdic_rawdata, filename=filename, head=headstr, extension=extension)
# # Program에 출력
if self.termination[0][:4].lower() == 'time':
temp_time_h = (self.termination[1] - time() + self.start_time) / (60.0 * 60)
temp_time_m = (self.termination[1] - time() + self.start_time) % (60.0 * 60) / 60.0
temp_time_s = (self.termination[1] - time() + self.start_time) % 60.0
txt = "simulation iter %d ends. %dh:%dm:%.0fs left. num_f_survived: %d, cum_rewards: %.2f, loss: %.3f, sec/iter: %.1f" \
% (self.iter, temp_time_h, temp_time_m, temp_time_s, self.env.num_f_survived, self.episode_reward, calc_mean(self.agent.loss_history), time()-self.iter_start_time)
elif self.termination[0][:4].lower() == 'iter':
txt = "simulation iter %d of %d ends. num_f_survived: %d, cum_rewards: %.2f, loss: %.3f, sec/iter: %.1f" \
% (self.iter, self.termination[1] - 1, self.env.num_f_survived, self.episode_reward, calc_mean(self.agent.loss_history), time()-self.iter_start_time)
print(txt)
if self.agent.name == 'rl':
print("\n[time spent at]")
for key in self.agent.time_check.keys():
print(key, ": %.2f" % self.agent.time_check[key])
print("\n[num_visit]")
for key in self.agent.num_visit.keys():
print(key, ": ", self.agent.num_visit[key])
print("\n\n")
if self.gui_framework:
self.gui_framework.write_console(txt)