def one_predict(self, sa):
Theta, _ = self.get_theta(
sa) # Get hyper-parameters for this query point
K = self.K
idx = self.kdt.query(sa.reshape(1, -1), k=K, return_distance=False)
X_nn = self.Xtrain[idx, :].reshape(K, self.state_action_dim)
Y_nn = self.Ytrain[idx, :].reshape(K, self.state_dim)
if useDiffusionMaps:
X_nn, Y_nn = self.reduction(sa, X_nn, Y_nn)
ds_next = np.zeros((self.state_dim, ))
std_next = np.zeros((self.state_dim, ))
for i in range(self.state_dim):
gp_est = GaussianProcess(X_nn[:, :self.state_action_dim],
Y_nn[:, i],
optimize=False,
theta=Theta[i],
algorithm='Matlab')
mm, vv = gp_est.predict(sa[:self.state_action_dim])
ds_next[i] = mm
std_next[i] = np.sqrt(vv)
s_next = sa[:self.
state_dim] + ds_next #np.random.normal(ds_next, std_next)
if self.state_dim == 5:
s_next[4] += 1.0 if s_next[4] < 0.0 else 0.0
s_next[4] -= 1.0 if s_next[4] > 1.0 else 0.0
return s_next
def predict(sa):
idx = kdt.query(sa.T, k=K, return_distance=False)
X_nn = Xtrain[idx, :].reshape(K, state_action_dim)
Y_nn = Ytrain[idx, :].reshape(K, state_dim)
if useDiffusionMaps:
X_nn, Y_nn = reduction(sa, X_nn, Y_nn)
m = np.zeros(state_dim)
s = np.zeros(state_dim)
for i in range(state_dim):
if i == 0:
gp_est = GaussianProcess(X_nn[:, :4],
Y_nn[:, i],
optimize=True,
theta=None)
theta = gp_est.cov.theta
else:
gp_est = GaussianProcess(X_nn[:, :4],
Y_nn[:, i],
optimize=False,
theta=theta)
m[i], s[i] = gp_est.predict(sa[:4].reshape(1, -1)[0])
return m, s
def one_predict(sa, X, Y, kdt, K=100):
state_dim = 4
idx = kdt.query(sa.reshape(1, -1), k=K, return_distance=False)
X_nn = X[idx, :].reshape(K, state_dim + 2)
Y_nn = Y[idx, :].reshape(K, state_dim)
ds_next = np.zeros((state_dim, ))
std_next = np.zeros((state_dim, ))
try:
for i in range(state_dim):
if i == 0:
gp_est = GaussianProcess(X_nn[:, :state_dim],
Y_nn[:, i],
optimize=True,
theta=None)
Theta = gp_est.cov.theta
else:
gp_est = GaussianProcess(X_nn[:, :state_dim],
Y_nn[:, i],
optimize=False,
theta=Theta)
mm, vv = gp_est.predict(sa[:state_dim])
ds_next[i] = mm
std_next[i] = np.sqrt(np.diag(vv))
except:
print "pass"
return np.array([-1, -1, -1, -1]), np.array([-1, -1, -1, -1])
# SKlearn
# for i in range(state_dim):
# gp_est = GaussianProcessRegressor(n_restarts_optimizer=9)
# gp_est.fit(X_nn[:,:state_dim], Y_nn[:,i])
# mm, vv = gp_est.predict(sa[:state_dim].reshape(1,-1), return_std=True)
# ds_next[i] = mm
# std_next[i] = vv#np.sqrt(np.diag(vv))
# GPy
# for i in range(state_dim):
# kernel = GPy.kern.RBF(input_dim=state_dim, variance=1., lengthscale=1.)
# gp_est = GPy.models.GPRegression(X_nn[:,:state_dim], Y_nn[:,i].reshape(-1,1), kernel)
# gp_est.optimize(messages=False)
# # m.optimize_restarts(num_restarts = 10)
# mm, vv = gp_est.predict(sa[:state_dim].reshape(1,state_dim))
# ds_next[i] = mm
# std_next[i] = np.sqrt(np.diag(vv))
s_next = sa[:4] + ds_next
return s_next, std_next, X_nn
def one_predict(sa, X, Y, kdt, K=100):
state_dim = 4
idx = kdt.query(sa.reshape(1, -1), k=K, return_distance=False)
X_nn = X[idx, :].reshape(K, state_dim + 2)
Y_nn = Y[idx, :].reshape(K, state_dim)
ds_next = np.zeros((state_dim, ))
std_next = np.zeros((state_dim, ))
try:
for i in range(state_dim):
if i == 0:
gp_est = GaussianProcess(X_nn[:, :state_dim],
Y_nn[:, i],
optimize=True,
theta=None)
Theta = gp_est.cov.theta
else:
gp_est = GaussianProcess(X_nn[:, :state_dim],
Y_nn[:, i],
optimize=False,
theta=Theta)
mm, vv = gp_est.predict(sa[:state_dim])
ds_next[i] = mm
std_next[i] = np.sqrt(np.diag(vv))
except:
print "pass"
return np.array([-1, -1, -1, -1]), np.array([-1, -1, -1, -1])
# for i in range(state_dim):
# gp_est = GaussianProcessRegressor(n_restarts_optimizer=9)
# gp_est.fit(X_nn[:,:state_dim], Y_nn[:,i])
# mm, vv = gp_est.predict(sa[:state_dim].reshape(1,-1), return_std=True)
# ds_next[i] = mm
# std_next[i] = vv#np.sqrt(np.diag(vv))
s_next = sa[:4] + ds_next
return s_next, std_next
# Compute real function
x_real = np.linspace(0, 6, 100).reshape(-1, 1)
y_real = np.array([func(i, 0) for i in x_real])
# Initiate GP with training data
gp_est = GaussianProcess(x_data,
y_data.reshape((-1, )),
optimize=True,
theta=None,
algorithm='Girard')
# Compute prediction for new data
x_new = np.linspace(0, 6, 100).reshape(-1, 1)
means = np.empty(100)
variances = np.empty(100)
for i in range(100):
means[i], variances[i] = gp_est.predict(x_new[i])
# Plot
plt.plot(x_data, y_data, '+k', label='data')
plt.plot(x_real, y_real, '--k', label='true function')
msl = (means.reshape(1, -1)[0] - np.sqrt(variances)) #.reshape(-1,1)
msu = (means.reshape(1, -1)[0] + np.sqrt(variances)) #.reshape(-1,1)[0]
plt.plot(x_new, means, '-r', label='prediction mean')
plt.fill_between(x_new.reshape(1, -1)[0], msl, msu, label='prediction std.')
plt.ylabel('f(x)')
plt.xlabel('x')
plt.title('GP')
plt.legend()
plt.show()
Ks = self.cov(Xs[idx], X) # cross-covariances
ms = self.mean(Xs[idx])
al, sW, L, C = self.post.alpha, self.post.sW, self.post.L, self.post.C
fmu[idx] = ms + np.dot(Ks, al)
if L == None:
fs2[idx] = kss + np.sum(Ks * np.dot(Ks, L), axis=1)
else:
V = np.linalg.solve(L, sW * Ks.T)
fs2[idx] = kss - np.sum(V * V, axis=0)
if ys == 0: yi = 0
else: yi = ys[idx]
lp[idx], ymu[idx], ys2[idx] = self.lik.pred(yi, fmu[idx], fs2[idx])
na += nb
return fmu, fs2, ymu, ys2, lp
if __name__ == "__main__":
def f(x):
return x * np.sin(x)
X = np.atleast_2d([1., 3., 5., 6., 7., 8.]).T
y = f(X).ravel()
Xs = np.atleast_2d(np.linspace(0, 10, 2007)).T
from gp import GaussianProcess as gp
gp = gp(mean=1.0 * one())
post, nlZ, dnlZ = gp.inference(X, y, deriv=True)
fmu, fs2, ymu, ys2, lp = gp.predict(X, y, Xs)
x_n = np.array([3.0]) # The mean of a normal distribution
var_n = np.diag([
0.2**2
]) # The covariance matrix (must be diagonal because of lazy programming)
m, s = gpup_est.predict(x_n, var_n)
# m, s = gp_est.predict(x_n)
print m, s
plt.errorbar(x_n, m, yerr=np.sqrt(s), ecolor='y')
plt.plot(x_n, m, '*y')
# exit(1)
x_new = np.linspace(0, 6, 100).reshape(-1, 1)
means = np.empty(100)
variances = np.empty(100)
for i in range(100):
means[i], variances[i] = gp_est.predict(x_new[i])
msl = (means.reshape(1, -1)[0] - np.sqrt(variances)) #.reshape(-1,1)
msu = (means.reshape(1, -1)[0] + np.sqrt(variances)) #.reshape(-1,1)[0]
ax1.plot(x_new, means, '-r')
ax1.fill_between(x_new.reshape(1, -1)[0], msl, msu)
# exit(1)
N = int(1e4)
X_belief = np.array([np.random.normal(x_n, np.sqrt(var_n))
for _ in range(N)]).reshape(N, 1) #
ax4 = plt.subplot2grid((3, 5), (2, 2), colspan=3, rowspan=1)
plt.plot(X_belief, np.tile(0., N), '.k')
x = np.linspace(0, 6, 1000).reshape(-1, 1)
plt.plot(x, scipy.stats.norm.pdf(x, x_n, np.sqrt(var_n)))
plt.xlabel('x')
def show_1d(X,y,Xs,ym,ys):
plt.fill(np.concatenate([Xs, Xs[::-1]]),
np.concatenate([ym - 1.96*ys,(ym + 1.96*ys)[::-1]]),
alpha=0.25, fc='k', ec='k', label='95% confidence interval')
plt.plot(X,y,'b+',ms=10)
plt.plot(Xs,ym,'b',lw=2)
plt.grid()
plt.xlabel('input X')
plt.ylabel('output y')
def f(x): return x * np.sin(x)
X = np.atleast_2d([1., 3., 5., 6., 7., 8.]).T
y = f(X).ravel()
Xs = np.atleast_2d(np.linspace(0, 10, 2007)).T
from sklearn import gaussian_process
gp = gaussian_process.GaussianProcess(corr='cubic', theta0=1e-2, thetaL=1e-4, thetaU=1e-1, random_start=100)
gp.fit(X,y)
ymn,ys2 = gp.predict(Xs, eval_MSE=True); ysd = np.sqrt(ys2)
from gp import GaussianProcess as gp
gp = gp(X=X,y=y)
post = gp.inference(X,y)
fmu,fs2,ymu,ys2,lp = gp.predict(X,y,Xs)
f0 = plt.figure(0); plt.clf()
show_1d(X,y,Xs,ymn,ysd)
f1 = plt.figure(1); plt.clf()
show_1d(X,y,Xs,ymu,ys2)
def main():
# Initialize the system.
sys = System(f, g, DT, X0 - REF, T0)
m1s = np.array([0])
k1s = np.array([SIGNAL_SIGMA])
m2s = np.array([0])
k2s = np.array([SIGNAL_SIGMA])
start_time = time.time()
elapsed_time = np.array([0])
gp1 = GaussianProcess(SEKernel, signal_sigma=SIGNAL_SIGMA)
gp2 = GaussianProcess(SEKernel, signal_sigma=SIGNAL_SIGMA)
# Simulate the system.
while sys.t < TF:
x = sys.x
# Predict the output.
input_observations = [[x[0], x[1], sys.ref[1]]]
m1, k1 = gp1.predict(input_observations)
m2, k2 = gp2.predict(input_observations)
u = np.dot(-K, sys.y)
sys.step(u)
# Observe the actual output.
input_observations = [[x[0], x[1], u[1]]]
gp1.observe(input_observations, [[sys.y[0]]])
gp2.observe(input_observations, [[sys.y[1]]])
# Record results.
m1s = np.append(m1s, m1)
k1s = np.append(k1s, np.sqrt(k1))
m2s = np.append(m2s, m2)
k2s = np.append(k2s, np.sqrt(k2))
elapsed_time = np.append(elapsed_time, time.time() - start_time)
print_sim_time(sys.t, DT)
print()
# Add operating point back to get back to normal coordinates.
ys = sys.ys + np.tile(REF, (sys.ys.shape[0], 1))
m1s = m1s + np.ones(m1s.shape[0]) * REF[0]
ts = sys.ts
# Plot the results.
plt.figure(1)
plt.subplot(211)
plt.plot([T0, TF], [REF[0], REF[0]], label='Reference')
plt.plot(ts, ys[:, 0], label='Actual')
plt.plot(ts, m1s, label='Predicted')
plot_sigma_bounds(ts, m1s, k1s, 3, (0.9, ) * 3)
plot_sigma_bounds(ts, m1s, k1s, 2, (0.8, ) * 3)
plot_sigma_bounds(ts, m1s, k1s, 1, (0.7, ) * 3)
plt.title('Inverted Pendulum')
plt.ylabel('Angle (rad)')
plt.legend()
plt.grid()
plt.subplot(212)
plt.plot([T0, TF], [REF[1], REF[1]], label='Reference')
plt.plot(ts, ys[:, 1], label='Actual')
plt.plot(ts, m2s, label='Predicted')
plot_sigma_bounds(ts, m2s, k2s, 3, (0.9, ) * 3)
plot_sigma_bounds(ts, m2s, k2s, 2, (0.8, ) * 3)
plot_sigma_bounds(ts, m2s, k2s, 1, (0.7, ) * 3)
plt.xlabel('Time (s)')
plt.ylabel('Angular velocity (rad/s)')
plt.legend()
plt.grid()
plt.show()
# Plot real time vs. simulation time, so we can see how the required
# computation increases as more data is observed over the course of the
# simulation.
plt.figure(2)
plt.plot(ts, elapsed_time)
plt.xlabel('Simulation time (s)')
plt.ylabel('Real time (s)')
plt.title('Real time vs. Simulation time')
plt.grid()
plt.show()
# Plot angle uncertainty over a portion of the state-space, with input u
# fixed at 0.
print('Plotting sample of state-space...')
fig = plt.figure(3)
ax = fig.add_subplot(111, projection='3d')
# Sample the GP across the state-space for u = 0.
samples = np.mgrid[-2:2:0.1, -2:2:0.1, 0:1].reshape(3, -1).T
m, cov = gp1.predict(samples)
# Generate plotting values.
X1, X2 = np.mgrid[-2:2:0.1, -2:2:0.1]
Z = np.sqrt(np.diag(cov)).T.reshape(40, 40)
ax.plot_surface(X1, X2, Z, cmap=cm.coolwarm, linewidth=0)
plt.xlabel('Angle')
plt.ylabel('Angular velocity')
plt.show()
