def mstep(self, gamma, x, u, **kwargs):
xs, ys, ws = [], [], []
for _x, _u, _w in zip(x, u, gamma):
xs.append(
np.hstack((_x[:-1, :], _u[:-1, :self.dm_act],
np.ones((_x.shape[0] - 1, 1)))))
ys.append(_x[1:, :])
ws.append(_w[1:, :])
_cov = np.zeros((self.nb_states, self.dm_obs, self.dm_obs))
for k in range(self.nb_states):
coef_, sigma = linear_regression(Xs=np.vstack(xs),
ys=np.vstack(ys),
weights=np.vstack(ws)[:, k],
fit_intercept=False,
**self.prior)
self.A[k, ...] = coef_[:, :self.dm_obs]
self.B[k, ...] = coef_[:, self.dm_obs:self.dm_obs + self.dm_act]
self.c[k, ...] = coef_[:, -1]
_cov[k, ...] = sigma
# usage = sum([_gamma.sum(0) for _gamma in gamma])
# unused = np.where(usage < 1)[0]
# used = np.where(usage > 1)[0]
# if len(unused) > 0:
# for k in unused:
# i = npr.choice(used)
# self.A[k] = self.A[i] + 0.01 * npr.randn(*self.A[i].shape)
# self.B[k] = self.B[i] + 0.01 * npr.randn(*self.B[i].shape)
# self.c[k] = self.c[i] + 0.01 * npr.randn(*self.c[i].shape)
# _cov[k] = _cov[i]
self.cov = _cov
def initialize(self, x, u, **kwargs):
localize = kwargs.get('localize', True)
Ts = [_x.shape[0] for _x in x]
if localize:
from sklearn.cluster import KMeans
km = KMeans(self.nb_states, random_state=1)
km.fit((np.vstack(x)))
zs = np.split(km.labels_, np.cumsum(Ts)[:-1])
zs = [z[:-1] for z in zs]
else:
zs = [npr.choice(self.nb_states, size=T - 1) for T in Ts]
_cov = np.zeros((self.nb_states, self.dm_obs, self.dm_obs))
for k in range(self.nb_states):
ts = [np.where(z == k)[0] for z in zs]
xs = [
np.hstack((_x[t, :], _u[t, :])) for t, _x, _u in zip(ts, x, u)
]
ys = [_x[t + 1, :] for t, _x in zip(ts, x)]
coef_, intercept_, sigma = linear_regression(xs, ys)
self.A[k, ...] = coef_[:, :self.dm_obs]
self.B[k, ...] = coef_[:, self.dm_obs:]
self.c[k, :] = intercept_
_cov[k, ...] = sigma
self.cov = _cov
def initialize(self, x, u, **kwargs):
localize = kwargs.get('localize', False)
Ts = [_x.shape[0] for _x in x]
if localize:
from sklearn.cluster import KMeans
km = KMeans(self.nb_states, random_state=1)
km.fit((np.vstack(x)))
zs = np.split(km.labels_, np.cumsum(Ts)[:-1])
zs = [z[:-1] for z in zs]
else:
zs = [npr.choice(self.nb_states, size=T - 1) for T in Ts]
_cov = np.zeros((self.nb_states, self.dm_obs, self.dm_obs))
for k in range(self.nb_states):
## Select the transformation
si = int(self.rot_lds[k, 0])
sj = int(self.rot_lds[k, 1])
T = self.T[sj, ...]
ts = [np.where(z == k)[0] for z in zs]
xs = []
ys = []
for i in range(len(ts)):
_x = x[i][ts[i], :]
_x = np.dot(T, _x.T).T
_y = x[i][ts[i] + 1, :]
_y = np.dot(T, _y.T).T
xs.append(_x)
ys.append(_y)
## THIS SHOULD NOT BE LIKE THIS , DUE TO IF SEVERAL TRANSFORMATIONS NOT WORK
coef_, intercept_, sigma = linear_regression(xs, ys)
self.A[si, ...] = coef_[:, :self.dm_obs]
#self.B[k, ...] = coef_[:, self.dm_obs:]
self.c[si, :] = intercept_
_cov[si, ...] = sigma
self.cov = _cov
self.covt = np.zeros([self.nb_states, self.dm_obs, self.dm_obs])
for k in range(self.nb_states):
i = int(self.rot_lds[k, 0])
j = int(self.rot_lds[k, 1])
T_inv = self.T_inv[j, ...]
self.covt[k, ...] = np.dot(T_inv, self.cov[i, ...])
def mstep(self, gamma, x, u):
xs, ys, ws = [], [], []
for _x, _u, _w in zip(x, u, gamma):
#xs.append(np.hstack((_x[:-1, :], _u[:-1, :self.dm_act], np.ones((_x.shape[0] - 1, 1)))))
xs.append(_x[:-1, :])
ys.append(_x[1:, :])
ws.append(_w[1:, :])
_J_diag = np.concatenate(
(self.reg * np.ones(self.dm_obs), self.reg * np.ones(self.dm_act),
self.reg * np.ones(1)))
_J = np.tile(np.diag(_J_diag)[None, :, :], (self.nb_lds, 1, 1))
_h = np.zeros(
(self.nb_lds, self.dm_obs + self.dm_act + 1, self.dm_obs))
for k in range(self.nb_states):
x_k = []
y_k = []
w_k = []
Hx_k = []
for _x, _y, _w in zip(xs, ys, ws):
i = int(self.rot_lds[k, 0])
j = int(self.rot_lds[k, 1])
## Transformation Matrix
T = self.T[j, ...]
x_ = np.einsum('kh,...h->...k', T, _x)
y_ = np.einsum('kh,...h->...k', T, _y)
w_ = _w[:, k]
y_k.append(y_)
x_k.append(x_)
w_k.append(w_)
coef_, intercept_, sigma = linear_regression(Xs=x_k,
ys=y_k,
weights=w_k,
fit_intercept=True,
**self.prior)
self.A[k, ...] = coef_
self.c[k, :] = intercept_
self.cov[k, ...] = sigma