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