Ytot = np.vstack((Ytot,Yexp))
if Xtot.shape[0] > keep-1:
Xtot = Xtot[-(keep-1):,:]
Ytot = Ytot[-(keep-1):,:]
" Train 'drift' GP"
if i % drift == 0:
mdrift = local.doOSGPR(Xtot[-drift:], Ytot[-drift:], local.mdrift,num_inducing ,use_old_Z=False)
mkl = []
for j in range(0, local.xdim):
mkl.append(1/(mdrift.kern.lengthscale[j]**2))
W = np.diag(mkl)
#W = np.diag([1/(mdrift.kern.lengthscale[0]**2), 1/(mdrift.kern.lengthscale[1]**2)])
local.W = W
local.mdrift = mdrift
"Partition experience "
local.partition(Xexp.reshape(len(Xexp),local.xdim),Yexp.reshape(len(Yexp),local.ndim), flag = partition_flag)
# local.train()
" Train Local Models "
try:
local.train(flag = train_flag)
except:
pass
t2 = time.time()
tc[i-1]= t2-t1
def model_callback(self, LocMsg):
"""
Unserializes SOLAR_GP model messages passed across ROS Topic into SOLAR_GP Objects
"""
W = LocMsg.W
W = np.diag([W[0], W[1], W[2]])
M = LocMsg.M
LocalData = []
Models = []
xdim = LocMsg.xdim
ndim = LocMsg.ndim
for L in LocMsg.localGPs:
X_loc = []
X_loc.append(L.numloc)
X_loc.append(L.numloc)
X_loc.append(np.array(L.xmean).reshape(1, xdim))
X_loc.append(np.array(L.ymean).reshape(1, ndim))
X_loc.append(True)
LocalData.append(X_loc)
X = np.empty([0, xdim])
Y = np.empty([0, ndim])
Z = np.empty([0, xdim])
Z_old = np.empty([0, xdim])
mu_old = np.empty([0, ndim])
Su_old = np.empty([0, len(L.Z_old)])
Kaa_old = np.empty([0, len(L.Z_old)])
kern = GPy.kern.RBF(xdim, ARD=True)
kern.variance = L.kern_var
kern.lengthscale = L.kern_lengthscale
for x, y in zip(L.X, L.Y):
X = np.vstack((X, np.array(x.array).reshape(1, xdim)))
Y = np.vstack((Y, np.array(y.array).reshape(1, ndim)))
for z in L.Z:
Z = np.vstack((Z, np.array(z.array).reshape(1, xdim)))
for z, mu, su, ka in zip(L.Z_old, L.mu_old, L.Su_old, L.Kaa_old):
Z_old = np.vstack((Z_old, np.array(z.array).reshape(1, xdim)))
mu_old = np.vstack(
(mu_old, np.array(mu.array).reshape(1, ndim)))
Su_old = np.vstack(
(Su_old, np.array(su.array).reshape(1, len(L.Z_old))))
Kaa_old = np.vstack(
(Kaa_old, np.array(ka.array).reshape(1, len(L.Z_old))))
m = osgpr_GPy.OSGPR_VFE(X, Y, kern, mu_old, Su_old, Kaa_old, Z_old,
Z)
m.kern.variance = L.kern_var
m.kern.lengthscale = np.array(L.kern_lengthscale)
m.likelihood.variance = L.likelihood_var
Models.append(m)
local = LocalModels()
local.W = W
local.M = M
local.LocalData = LocalData
local.Models = Models
local.xdim = xdim
local.ndim = ndim
self.model = local