def doOSGPR(self,X,Y,m_old, num_inducing,use_old_Z=True, driftZ = False, fixTheta = False):
Zopt = m_old.Z.param_array
mu, Su = m_old.predict(Zopt, full_cov = True)
Su = Su + 1e-4*np.eye(mu.shape[0])
Kaa = m_old.kern.K(Zopt)
Zinit = self.init_Z(Zopt, X, num_inducing, use_old_Z, driftZ)
m_new = osgpr_GPy.OSGPR_VFE(X, Y, m_old.kern, mu, Su, Kaa,
Zopt, Zinit)
m_new.likelihood.variance = m_old.likelihood.variance
m_new.kern.variance = m_old.kern.variance
m_new.kern.lengthscale = m_old.kern.lengthscale
"Fix parameters"
if driftZ:
m_new.Z.fix()
if fixTheta:
m_new.kern.fix()
m_new.likelihood.variance.fix()
m_new.optimize()
m_new.Z.unfix()
m_new.kern.unfix()
m_new.likelihood.variance.unfix()
return m_new
def doOSGPR(self,X,Y,m_old, num_inducing,use_old_Z=True, driftZ = False, fixTheta = False):
Zopt = m_old.Z.param_array
mu, Su = m_old.predict(Zopt, full_cov = True)
# mu = np.unwrap(mu)
# mu[mu<0] += 2*np.pi
# mu = mu +2*np.pi
Su = Su + 1e-4*np.eye(mu.shape[0])
Kaa = m_old.kern.K(Zopt)
Zinit = self.init_Z(Zopt, X, num_inducing, use_old_Z, driftZ)
# m_new = osgpr_GPy.OSGPR_VFE(X, Y, GPy.kern.RBF(2,ARD=True), mu, Su, Kaa,
# Zopt, Zinit)
# Y = Y-2*np.pi
# Y = circstats.wrapdiff(Y)
# Y = Y + 2*np.pi
m_new = osgpr_GPy.OSGPR_VFE(X, Y, GPy.kern.RBF(2,ARD=True), mu, Su, Kaa,
Zopt, Zinit)
# m_new = osgpr_GPy.OSGPR_VFE(X, Y, GPy.kern.RBF(2,ARD=True), mu, Su, Kaa,
# Zopt, Zinit)
m_new.likelihood.variance = m_old.likelihood.variance
m_new.kern.variance = m_old.kern.variance
m_new.kern.lengthscale = m_old.kern.lengthscale
#
# m_new.likelihood.variance = self.mdrift.likelihood.variance
# m_new.kern.variance = self.mdrift.kern.variance
# m_new.kern.lengthscale = self.mdrift.kern.lengthscale
"Fix parameters"
if driftZ:
# m_new.kern.fix()
# m_new.likelihood.variance.fix()
m_new.Z.fix()
if fixTheta:
m_new.kern.fix()
m_new.likelihood.variance.fix()
m_new.optimize(messages=False)
m_new.Z.unfix()
m_new.kern.unfix()
m_new.likelihood.variance.unfix()
return m_new
def doOSGPR(self,
X,
Y,
m_old,
num_inducing,
use_old_Z=True,
driftZ=False,
fixTheta=False):
# try:
Zopt = copy(m_old.Z.param_array)
mu, Su = m_old.predict(Zopt, full_cov=True)
Su = Su + 5e-4 * np.eye(mu.shape[0])
Kaa = m_old.kern.K(Zopt)
kern = GPy.kern.RBF(self.xdim, ARD=True)
kern.variance = copy(m_old.kern.variance)
kern.lengthscale = copy(m_old.kern.lengthscale)
Zinit = self.init_Z(Zopt, X, num_inducing, use_old_Z, driftZ)
m_new = osgpr_GPy.OSGPR_VFE(X, Y, kern, mu, Su, Kaa, Zopt, Zinit,
m_old.likelihood)
# m_new.likelihood.variance = copy(m_old.likelihood.variance)
# m_new.kern.variance = copy(m_old.kern.variance)
# m_new.kern.lengthscale = copy(m_old.kern.lengthscale)
"Fix parameters"
if driftZ:
m_new.Z.fix()
if fixTheta:
m_new.kern.fix()
m_new.likelihood.variance.fix()
m_new.optimize()
# print('num_inducing: ' + str(len(m_new.Z)))
m_new.Z.unfix()
m_new.kern.unfix()
m_new.likelihood.variance.unfix()
# except Warning:
# warnings.warn("warning during training")
# return m_old
return m_new
def doOSGPR(self,
in_new,
out_new,
m_old,
num_idc,
use_old_Z=True,
driftZ=False,
fixTheta=False):
'''
This function implement the online sparse GP regression to update (train) local models according to the new data
'''
# record the old inducing points Z and its parameters
cur_Z = copy(m_old.Z.param_array)
mu, Su = m_old.predict(cur_Z, full_cov=True)
Su = Su + 5e-4 * np.eye(mu.shape[0])
Kaa = m_old.kern.K(cur_Z)
kern = GPy.kern.RBF(self.dim_in, ARD=True)
kern.variance = copy(m_old.kern.variance)
kern.lengthscale = copy(m_old.kern.lengthscale)
# merge the old Z and the new input data
Z_new = self.updateZ(cur_Z, in_new, num_idc, use_old_Z, driftZ)
m_new = osgpr_GPy.OSGPR_VFE(in_new, out_new, kern, mu, Su, Kaa, cur_Z,
Z_new, m_old.likelihood)
"Fix parameters"
if driftZ:
m_new.Z.fix()
if fixTheta:
m_new.kern.fix()
m_new.likelihood.variance.fix()
# optimization stuff
m_new.optimize()
m_new.Z.unfix()
m_new.kern.unfix()
m_new.likelihood.variance.unfix()
return m_new
def deconstructMsg(LocMsg):
W = LocMsg.W
W = np.diag([1 / (W[0]**2), 1 / (W[1]**2)])
M = LocMsg.M
LocalData = []
Models = []
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, 2))
X_loc.append(np.array(L.ymean).reshape(1, 2))
X_loc.append(True)
LocalData.append(X_loc)
# kern_var = L.kern_var
# kern_lengthscale = L.kern_lengthscale
X = np.empty([0, 2])
Y = np.empty([0, 2])
Z = np.empty([0, 2])
Z_old = np.empty([0, 2])
mu_old = np.empty([0, 2])
Su_old = np.empty([0, len(L.Z_old)])
Kaa_old = np.empty([0, len(L.Z_old)])
kern = GPy.kern.RBF(2, 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, 2)))
Y = np.vstack((Y, np.array(y.array).reshape(1, 2)))
for z in L.Z:
Z = np.vstack((Z, np.array(z.array).reshape(1, 2)))
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, 2)))
mu_old = np.vstack((mu_old, np.array(mu.array).reshape(1, 2)))
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))))
# X = np.array(L.X).reshape(1,2)
# Y = np.array(L.Y).reshape(1,2)
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
return local
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
