def ravel_tril_indices(n, k=0, m=None): if m is None: size = (n, n) else: size = (n, m) idxs = np.tril_indices(n, k, m) return np.ravel_multi_index(idxs, size)
def meddistance(X, subsample=None, mean_on_fail=True): """ Compute the median of pairwise distances (not distance squared) of points in the matrix. Useful as a heuristic for setting Gaussian kernel's width. Parameters ---------- X : n x d numpy array mean_on_fail: True/False. If True, use the mean when the median distance is 0. This can happen especially, when the data are discrete e.g., 0/1, and there are more slightly more 0 than 1. In this case, the m Return ------ median distance """ if subsample is None: D = dist_matrix(X, X) Itri = np.tril_indices(D.shape[0], -1) Tri = D[Itri] med = np.median(Tri) if med <= 0: # use the mean return np.mean(Tri) return med else: assert subsample > 0 rand_state = np.random.get_state() np.random.seed(9827) n = X.shape[0] ind = np.random.choice(n, min(subsample, n), replace=False) np.random.set_state(rand_state) # recursion just one return meddistance(X[ind, :], None, mean_on_fail)
def _vectorize_ld_matrix(mat): """ Linearize the lower diagonal of a square matrix. Parameters: mat A square matrix. Returns: 1-d vector The lower diagonal of `mat` stacked into a vector. Specifically, we map the matrix [ x11 x12 ... x1n ] [ x21 x22 x2n ] [... ] [ xn1 ... xnn ] to the vector [ x11, x21, x22, x31, ..., xnn ]. The entries above the diagonal are ignored. """ nrow, ncol = np.shape(mat) if nrow != ncol: raise ValueError('mat must be square') return mat[np.tril_indices(nrow)]
def step(self, action): #Y = self.obs(self.x) self.counter += 1 self.x, self.P, K = self.EKF(self.x, self.P, action) if not is_pos_def(self.P): print("x:", self.x) print("P:", self.P) print(np.linalg.eigvalsh(self.P)) #e = -min(np.linalg.eigvalsh(self.P)) #self.P = self.P + np.eye(5)*(e + 1e-5) self.L = np.linalg.cholesky(self.P) ind_tril = np.tril_indices(self.L.shape[0]) terminal, final_reward = self.terminal_state(self.x) done = terminal or self.counter >= self.episode_len reward = self.reward_func(action) + final_reward r = np.sqrt(np.sum(self.x[:2]**2)) rel_ang = self.x[2] - math.atan2(-self.x[1], -self.x[0]) rel_ang %= 2 * pi rel_ang = rel_ang if rel_ang < pi else (rel_ang - 2 * pi) rel_x = np.array([r, rel_ang, self.x[3], self.x[4]]) self.state = np.append(rel_x, self.L[ind_tril]) return self.state, reward, done, {}
def reset(self): pos = random.uniform(self.world_box[1]+1, self.world_box[0]-1) #pos = random.multivariate_normal(np.zeros(2), np.eye(2)*1.25) ang = math.atan2(-pos[1], -pos[0]) + random.uniform(-pi/8, pi/8) ang %= 2*pi self.x = np.append(pos, [ang, 0., 0.]) self.P = np.eye(5) * (0.0001**2) self.L = np.linalg.cholesky(self.P) ind_tril = np.tril_indices(self.L.shape[0]) self.counter = 0 self.state = np.append(self.x, self.L[ind_tril]) #state = self.state#self._get_state() return self.state
def step(self, action): #Y = self.obs(self.x) self.counter += 1 self.x, self.P, K = self.EKF(self.x, self.P, action) self.L = np.linalg.cholesky(self.P) ind_tril = np.tril_indices(self.L.shape[0]) done = self.terminal_state(self.x) reward = self.reward_func(action) self.state = np.append(self.x, self.L[ind_tril]) if self.counter > 256: done = True return self.state, reward, done, {}
def reset(self): #pos = random.uniform(self.world_box[1], self.world_box[0]) pos = random.multivariate_normal(np.zeros(2), np.eye(2)*4) ang = math.atan2(-pos[1], -pos[0]) + random.uniform(-pi/4, pi/4) ang %= 2*pi self.x = np.append(pos, [ang, 0., 0.]) self.P = np.eye(5) * (0.0001**2) self.L = np.linalg.cholesky(self.P) ind_tril = np.tril_indices(self.L.shape[0]) self.counter = 0 self.state = np.append(self.x, self.L[ind_tril]) #print("pretag:", self.state) return self.state
def step(self, action): #Y = self.obs(self.x) self.counter += 1 self.x, self.P, K = self.EKF(self.x, self.P, action) if not is_pos_def(self.P): print("x:", self.x) print("P:", self.P) print(np.linalg.eigvalsh(self.P)) #e = -min(np.linalg.eigvalsh(self.P)) #self.P = self.P + np.eye(5)*(e + 1e-5) self.L = np.linalg.cholesky(self.P) ind_tril = np.tril_indices(self.L.shape[0]) terminal, final_reward = self.terminal_state(self.x) done = terminal or self.counter >= self.episode_len reward = self.reward_func(action) + final_reward self.state = np.append(self.x, self.L[ind_tril]) return self.state, reward, done, {}
def tril_to_vec(x, k=0): """ """ n = x.shape[-1] rows, cols = tril_indices(n, k=k) return x[..., rows, cols]
def _vectorize_ld_matrix(mat): nrow, ncol = np.shape(mat) if nrow != ncol: raise ValueError('mat must be square') return mat[np.tril_indices(nrow)]
def pos_def_mat_from_vector(vec, target_size, jitter=0): L = np.zeros((target_size, target_size)) L[np.tril_indices(target_size)] = vec return np.matmul(L, L.T) + np.eye(target_size) * jitter