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 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
