class TestLikelihood(Parameterized): def __init__(self, param1 = 2., param2 = 3.): super(TestLikelihood, self).__init__("TestLike") self.p1 = Param('param1', param1) self.p2 = Param('param2', param2) self.link_parameter(self.p1) self.link_parameter(self.p2) self.p1.fix() self.p1.unfix() self.p2.constrain_negative() self.p1.fix() self.p2.constrain_positive() self.p2.fix() self.p2.constrain_positive()
class TestLikelihood(Parameterized): def __init__(self, param1=2., param2=3.): super(TestLikelihood, self).__init__("TestLike") self.p1 = Param('param1', param1) self.p2 = Param('param2', param2) self.link_parameter(self.p1) self.link_parameter(self.p2) self.p1.fix() self.p1.unfix() self.p2.constrain_negative() self.p1.fix() self.p2.constrain_positive() self.p2.fix() self.p2.constrain_positive()
class StateSpace(Model): def __init__(self, X, Y, kernel=None, sigma2=1.0, name='StateSpace'): super(StateSpace, self).__init__(name=name) self.num_data, input_dim = X.shape assert input_dim == 1, "State space methods for time only" num_data_Y, self.output_dim = Y.shape assert num_data_Y == self.num_data, "X and Y data don't match" assert self.output_dim == 1, "State space methods for single outputs only" # Make sure the observations are ordered in time sort_index = np.argsort(X[:, 0]) self.X = X[sort_index] self.Y = Y[sort_index] # Noise variance self.sigma2 = Param('Gaussian_noise', sigma2) self.link_parameter(self.sigma2) # Default kernel if kernel is None: self.kern = kern.Matern32(1) else: self.kern = kernel self.link_parameter(self.kern) self.sigma2.constrain_positive() # Assert that the kernel is supported if not hasattr(self.kern, 'sde'): raise NotImplementedError( 'SDE must be implemented for the kernel being used') #assert self.kern.sde() not False, "This kernel is not supported for state space estimation" def parameters_changed(self): """ Parameters have now changed """ # Get the model matrices from the kernel (F, L, Qc, H, Pinf, dF, dQc, dPinf) = self.kern.sde() # Use the Kalman filter to evaluate the likelihood self._log_marginal_likelihood = self.kf_likelihood( F, L, Qc, H, self.sigma2, Pinf, self.X.T, self.Y.T) gradients = self.compute_gradients() self.sigma2.gradient_full[:] = gradients[-1] self.kern.gradient_full[:] = gradients[:-1] def log_likelihood(self): return self._log_marginal_likelihood def compute_gradients(self): # Get the model matrices from the kernel (F, L, Qc, H, Pinf, dFt, dQct, dPinft) = self.kern.sde() # Allocate space for the full partial derivative matrices dF = np.zeros([dFt.shape[0], dFt.shape[1], dFt.shape[2] + 1]) dQc = np.zeros([dQct.shape[0], dQct.shape[1], dQct.shape[2] + 1]) dPinf = np.zeros( [dPinft.shape[0], dPinft.shape[1], dPinft.shape[2] + 1]) # Assign the values for the kernel function dF[:, :, :-1] = dFt dQc[:, :, :-1] = dQct dPinf[:, :, :-1] = dPinft # The sigma2 derivative dR = np.zeros([1, 1, dF.shape[2]]) dR[:, :, -1] = 1 # Calculate the likelihood gradients gradients = self.kf_likelihood_g(F, L, Qc, H, self.sigma2, Pinf, dF, dQc, dPinf, dR, self.X.T, self.Y.T) return gradients def predict_raw(self, Xnew, Ynew=None, filteronly=False): # Set defaults if Ynew is None: Ynew = self.Y # Make a single matrix containing training and testing points X = np.vstack((self.X, Xnew)) Y = np.vstack((Ynew, np.nan * np.zeros(Xnew.shape))) # Sort the matrix (save the order) _, return_index, return_inverse = np.unique(X, True, True) X = X[return_index] Y = Y[return_index] # Get the model matrices from the kernel (F, L, Qc, H, Pinf, dF, dQc, dPinf) = self.kern.sde() # Run the Kalman filter (M, P) = self.kalman_filter(F, L, Qc, H, self.sigma2, Pinf, X.T, Y.T) # Run the Rauch-Tung-Striebel smoother if not filteronly: (M, P) = self.rts_smoother(F, L, Qc, X.T, M, P) # Put the data back in the original order M = M[:, return_inverse] P = P[:, :, return_inverse] # Only return the values for Xnew M = M[:, self.num_data:] P = P[:, :, self.num_data:] # Calculate the mean and variance m = H.dot(M).T V = np.tensordot(H[0], P, (0, 0)) V = np.tensordot(V, H[0], (0, 0)) V = V[:, None] # Return the posterior of the state return (m, V) def predict(self, Xnew, filteronly=False): # Run the Kalman filter to get the state (m, V) = self.predict_raw(Xnew, filteronly=filteronly) # Add the noise variance to the state variance V += self.sigma2 # Lower and upper bounds lower = m - 2 * np.sqrt(V) upper = m + 2 * np.sqrt(V) # Return mean and variance return (m, V, lower, upper) def plot(self, plot_limits=None, levels=20, samples=0, fignum=None, ax=None, resolution=None, plot_raw=False, plot_filter=False, linecol=Tango.colorsHex['darkBlue'], fillcol=Tango.colorsHex['lightBlue']): # Deal with optional parameters if ax is None: fig = pb.figure(num=fignum) ax = fig.add_subplot(111) # Define the frame on which to plot resolution = resolution or 200 Xgrid, xmin, xmax = x_frame1D(self.X, plot_limits=plot_limits) # Make a prediction on the frame and plot it if plot_raw: m, v = self.predict_raw(Xgrid, filteronly=plot_filter) lower = m - 2 * np.sqrt(v) upper = m + 2 * np.sqrt(v) Y = self.Y else: m, v, lower, upper = self.predict(Xgrid, filteronly=plot_filter) Y = self.Y # Plot the values gpplot(Xgrid, m, lower, upper, axes=ax, edgecol=linecol, fillcol=fillcol) ax.plot(self.X, self.Y, 'kx', mew=1.5) # Optionally plot some samples if samples: if plot_raw: Ysim = self.posterior_samples_f(Xgrid, samples) else: Ysim = self.posterior_samples(Xgrid, samples) for yi in Ysim.T: ax.plot(Xgrid, yi, Tango.colorsHex['darkBlue'], linewidth=0.25) # Set the limits of the plot to some sensible values ymin, ymax = min(np.append(Y.flatten(), lower.flatten())), max( np.append(Y.flatten(), upper.flatten())) ymin, ymax = ymin - 0.1 * (ymax - ymin), ymax + 0.1 * (ymax - ymin) ax.set_xlim(xmin, xmax) ax.set_ylim(ymin, ymax) def prior_samples_f(self, X, size=10): # Sort the matrix (save the order) (_, return_index, return_inverse) = np.unique(X, True, True) X = X[return_index] # Get the model matrices from the kernel (F, L, Qc, H, Pinf, dF, dQc, dPinf) = self.kern.sde() # Allocate space for results Y = np.empty((size, X.shape[0])) # Simulate random draws #for j in range(0,size): # Y[j,:] = H.dot(self.simulate(F,L,Qc,Pinf,X.T)) Y = self.simulate(F, L, Qc, Pinf, X.T, size) # Only observations Y = np.tensordot(H[0], Y, (0, 0)) # Reorder simulated values Y = Y[:, return_inverse] # Return trajectory return Y.T def posterior_samples_f(self, X, size=10): # Sort the matrix (save the order) (_, return_index, return_inverse) = np.unique(X, True, True) X = X[return_index] # Get the model matrices from the kernel (F, L, Qc, H, Pinf, dF, dQc, dPinf) = self.kern.sde() # Run smoother on original data (m, V) = self.predict_raw(X) # Simulate random draws from the GP prior y = self.prior_samples_f(np.vstack((self.X, X)), size) # Allocate space for sample trajectories Y = np.empty((size, X.shape[0])) # Run the RTS smoother on each of these values for j in range(0, size): yobs = y[0:self.num_data, j:j + 1] + np.sqrt( self.sigma2) * np.random.randn(self.num_data, 1) (m2, V2) = self.predict_raw(X, Ynew=yobs) Y[j, :] = m.T + y[self.num_data:, j].T - m2.T # Reorder simulated values Y = Y[:, return_inverse] # Return posterior sample trajectories return Y.T def posterior_samples(self, X, size=10): # Make samples of f Y = self.posterior_samples_f(X, size) # Add noise Y += np.sqrt(self.sigma2) * np.random.randn(Y.shape[0], Y.shape[1]) # Return trajectory return Y def kalman_filter(self, F, L, Qc, H, R, Pinf, X, Y): # KALMAN_FILTER - Run the Kalman filter for a given model and data # Allocate space for results MF = np.empty((F.shape[0], Y.shape[1])) PF = np.empty((F.shape[0], F.shape[0], Y.shape[1])) # Initialize MF[:, -1] = np.zeros(F.shape[0]) PF[:, :, -1] = Pinf.copy() # Time step lengths dt = np.empty(X.shape) dt[:, 0] = X[:, 1] - X[:, 0] dt[:, 1:] = np.diff(X) # Solve the LTI SDE for these time steps As, Qs, index = self.lti_disc(F, L, Qc, dt) # Kalman filter for k in range(0, Y.shape[1]): # Form discrete-time model #(A, Q) = self.lti_disc(F,L,Qc,dt[:,k]) A = As[:, :, index[k]] Q = Qs[:, :, index[k]] # Prediction step MF[:, k] = A.dot(MF[:, k - 1]) PF[:, :, k] = A.dot(PF[:, :, k - 1]).dot(A.T) + Q # Update step (only if there is data) if not np.isnan(Y[:, k]): if Y.shape[0] == 1: K = PF[:, :, k].dot( H.T) / (H.dot(PF[:, :, k]).dot(H.T) + R) else: LL = linalg.cho_factor(H.dot(PF[:, :, k]).dot(H.T) + R) K = linalg.cho_solve(LL, H.dot(PF[:, :, k].T)).T MF[:, k] += K.dot(Y[:, k] - H.dot(MF[:, k])) PF[:, :, k] -= K.dot(H).dot(PF[:, :, k]) # Return values return (MF, PF) def rts_smoother(self, F, L, Qc, X, MS, PS): # RTS_SMOOTHER - Run the RTS smoother for a given model and data # Time step lengths dt = np.empty(X.shape) dt[:, 0] = X[:, 1] - X[:, 0] dt[:, 1:] = np.diff(X) # Solve the LTI SDE for these time steps As, Qs, index = self.lti_disc(F, L, Qc, dt) # Sequentially smooth states starting from the end for k in range(2, X.shape[1] + 1): # Form discrete-time model #(A, Q) = self.lti_disc(F,L,Qc,dt[:,1-k]) A = As[:, :, index[1 - k]] Q = Qs[:, :, index[1 - k]] # Smoothing step LL = linalg.cho_factor(A.dot(PS[:, :, -k]).dot(A.T) + Q) G = linalg.cho_solve(LL, A.dot(PS[:, :, -k])).T MS[:, -k] += G.dot(MS[:, 1 - k] - A.dot(MS[:, -k])) PS[:, :, -k] += G.dot(PS[:, :, 1 - k] - A.dot(PS[:, :, -k]).dot(A.T) - Q).dot(G.T) # Return return (MS, PS) def kf_likelihood(self, F, L, Qc, H, R, Pinf, X, Y): # Evaluate marginal likelihood # Initialize lik = 0 m = np.zeros((F.shape[0], 1)) P = Pinf.copy() # Time step lengths dt = np.empty(X.shape) dt[:, 0] = X[:, 1] - X[:, 0] dt[:, 1:] = np.diff(X) # Solve the LTI SDE for these time steps As, Qs, index = self.lti_disc(F, L, Qc, dt) # Kalman filter for likelihood evaluation for k in range(0, Y.shape[1]): # Form discrete-time model #(A,Q) = self.lti_disc(F,L,Qc,dt[:,k]) A = As[:, :, index[k]] Q = Qs[:, :, index[k]] # Prediction step m = A.dot(m) P = A.dot(P).dot(A.T) + Q # Update step only if there is data if not np.isnan(Y[:, k]): v = Y[:, k] - H.dot(m) if Y.shape[0] == 1: S = H.dot(P).dot(H.T) + R K = P.dot(H.T) / S lik -= 0.5 * np.log(S) lik -= 0.5 * v.shape[0] * np.log(2 * np.pi) lik -= 0.5 * v * v / S else: LL, isupper = linalg.cho_factor(H.dot(P).dot(H.T) + R) lik -= np.sum(np.log(np.diag(LL))) lik -= 0.5 * v.shape[0] * np.log(2 * np.pi) lik -= 0.5 * linalg.cho_solve((LL, isupper), v).dot(v) K = linalg.cho_solve((LL, isupper), H.dot(P.T)).T m += K.dot(v) P -= K.dot(H).dot(P) # Return likelihood return lik[0, 0] def kf_likelihood_g(self, F, L, Qc, H, R, Pinf, dF, dQc, dPinf, dR, X, Y): # Evaluate marginal likelihood gradient # State dimension, number of data points and number of parameters n = F.shape[0] steps = Y.shape[1] nparam = dF.shape[2] # Time steps t = X.squeeze() # Allocate space e = 0 eg = np.zeros(nparam) # Set up m = np.zeros([n, 1]) P = Pinf.copy() dm = np.zeros([n, nparam]) dP = dPinf.copy() mm = m.copy() PP = P.copy() # Initial dt dt = -np.Inf # Allocate space for expm results AA = np.zeros([2 * n, 2 * n, nparam]) FF = np.zeros([2 * n, 2 * n]) # Loop over all observations for k in range(0, steps): # The previous time step dt_old = dt # The time discretization step length if k > 0: dt = t[k] - t[k - 1] else: dt = 0 # Loop through all parameters (Kalman filter prediction step) for j in range(0, nparam): # Should we recalculate the matrix exponential? if abs(dt - dt_old) > 1e-9: # The first matrix for the matrix factor decomposition FF[:n, :n] = F FF[n:, :n] = dF[:, :, j] FF[n:, n:] = F # Solve the matrix exponential AA[:, :, j] = linalg.expm3(FF * dt) # Solve the differential equation foo = AA[:, :, j].dot(np.vstack([m, dm[:, j:j + 1]])) mm = foo[:n, :] dm[:, j:j + 1] = foo[n:, :] # The discrete-time dynamical model if j == 0: A = AA[:n, :n, j] Q = Pinf - A.dot(Pinf).dot(A.T) PP = A.dot(P).dot(A.T) + Q # The derivatives of A and Q dA = AA[n:, :n, j] dQ = dPinf[:,:,j] - dA.dot(Pinf).dot(A.T) \ - A.dot(dPinf[:,:,j]).dot(A.T) - A.dot(Pinf).dot(dA.T) # The derivatives of P dP[:,:,j] = dA.dot(P).dot(A.T) + A.dot(dP[:,:,j]).dot(A.T) \ + A.dot(P).dot(dA.T) + dQ # Set predicted m and P m = mm P = PP # Start the Kalman filter update step and precalculate variables S = H.dot(P).dot(H.T) + R # We should calculate the Cholesky factor if S is a matrix # [LS,notposdef] = chol(S,'lower'); # The Kalman filter update (S is scalar) HtiS = H.T / S iS = 1 / S K = P.dot(HtiS) v = Y[:, k] - H.dot(m) vtiS = v.T / S # Loop through all parameters (Kalman filter update step derivative) for j in range(0, nparam): # Innovation covariance derivative dS = H.dot(dP[:, :, j]).dot(H.T) + dR[:, :, j] # Evaluate the energy derivative for j eg[j] = eg[j] \ - .5*np.sum(iS*dS) \ + .5*H.dot(dm[:,j:j+1]).dot(vtiS.T) \ + .5*vtiS.dot(dS).dot(vtiS.T) \ + .5*vtiS.dot(H.dot(dm[:,j:j+1])) # Kalman filter update step derivatives dK = dP[:, :, j].dot(HtiS) - P.dot(HtiS).dot(dS) / S dm[:, j:j + 1] = dm[:, j:j + 1] + dK.dot(v) - K.dot(H).dot( dm[:, j:j + 1]) dKSKt = dK.dot(S).dot(K.T) dP[:, :, j] = dP[:, :, j] - dKSKt - K.dot(dS).dot(K.T) - dKSKt.T # Evaluate the energy # e = e - .5*S.shape[0]*np.log(2*np.pi) - np.sum(np.log(np.diag(LS))) - .5*vtiS.dot(v); e = e - .5 * S.shape[0] * np.log(2 * np.pi) - np.sum( np.log(np.sqrt(S))) - .5 * vtiS.dot(v) # Finish Kalman filter update step m = m + K.dot(v) P = P - K.dot(S).dot(K.T) # Make sure the covariances stay symmetric P = (P + P.T) / 2 dP = (dP + dP.transpose([1, 0, 2])) / 2 # raise NameError('Debug me') # Return the gradient return eg def kf_likelihood_g_notstable(self, F, L, Qc, H, R, Pinf, dF, dQc, dPinf, dR, X, Y): # Evaluate marginal likelihood gradient # State dimension, number of data points and number of parameters steps = Y.shape[1] nparam = dF.shape[2] n = F.shape[0] # Time steps t = X.squeeze() # Allocate space e = 0 eg = np.zeros(nparam) # Set up Z = np.zeros(F.shape) QC = L.dot(Qc).dot(L.T) m = np.zeros([n, 1]) P = Pinf.copy() dm = np.zeros([n, nparam]) dP = dPinf.copy() mm = m.copy() PP = P.copy() # % Initial dt dt = -np.Inf # Allocate space for expm results AA = np.zeros([2 * F.shape[0], 2 * F.shape[0], nparam]) AAA = np.zeros([4 * F.shape[0], 4 * F.shape[0], nparam]) FF = np.zeros([2 * F.shape[0], 2 * F.shape[0]]) FFF = np.zeros([4 * F.shape[0], 4 * F.shape[0]]) # Loop over all observations for k in range(0, steps): # The previous time step dt_old = dt # The time discretization step length if k > 0: dt = t[k] - t[k - 1] else: dt = t[1] - t[0] # Loop through all parameters (Kalman filter prediction step) for j in range(0, nparam): # Should we recalculate the matrix exponential? if abs(dt - dt_old) > 1e-9: # The first matrix for the matrix factor decomposition FF[:n, :n] = F FF[n:, :n] = dF[:, :, j] FF[n:, n:] = F # Solve the matrix exponential AA[:, :, j] = linalg.expm3(FF * dt) # Solve using matrix fraction decomposition foo = AA[:, :, j].dot(np.vstack([m, dm[:, j:j + 1]])) # Pick the parts mm = foo[:n, :] dm[:, j:j + 1] = foo[n:, :] # Should we recalculate the matrix exponential? if abs(dt - dt_old) > 1e-9: # Define W and G W = L.dot(dQc[:, :, j]).dot(L.T) G = dF[:, :, j] # The second matrix for the matrix factor decomposition FFF[:n, :n] = F FFF[2 * n:-n, :n] = G FFF[:n, n:2 * n] = QC FFF[n:2 * n, n:2 * n] = -F.T FFF[2 * n:-n, n:2 * n] = W FFF[-n:, n:2 * n] = -G.T FFF[2 * n:-n, 2 * n:-n] = F FFF[2 * n:-n, -n:] = QC FFF[-n:, -n:] = -F.T # Solve the matrix exponential AAA[:, :, j] = linalg.expm3(FFF * dt) # Solve using matrix fraction decomposition foo = AAA[:, :, j].dot( np.vstack([P, np.eye(n), dP[:, :, j], np.zeros([n, n])])) # Pick the parts C = foo[:n, :] D = foo[n:2 * n, :] dC = foo[2 * n:-n, :] dD = foo[-n:, :] # The prediction step covariance (PP = C/D) if j == 0: PP = linalg.solve(D.T, C.T).T PP = (PP + PP.T) / 2 # Sove dP for j (C/D == P_{k|k-1}) dP[:, :, j] = linalg.solve(D.T, (dC - PP.dot(dD)).T).T # Set predicted m and P m = mm P = PP # Start the Kalman filter update step and precalculate variables S = H.dot(P).dot(H.T) + R # We should calculate the Cholesky factor if S is a matrix # [LS,notposdef] = chol(S,'lower'); # The Kalman filter update (S is scalar) HtiS = H.T / S iS = 1 / S K = P.dot(HtiS) v = Y[:, k] - H.dot(m) vtiS = v.T / S # Loop through all parameters (Kalman filter update step derivative) for j in range(0, nparam): # Innovation covariance derivative dS = H.dot(dP[:, :, j]).dot(H.T) + dR[:, :, j] # Evaluate the energy derivative for j eg[j] = eg[j] \ - .5*np.sum(iS*dS) \ + .5*H.dot(dm[:,j:j+1]).dot(vtiS.T) \ + .5*vtiS.dot(dS).dot(vtiS.T) \ + .5*vtiS.dot(H.dot(dm[:,j:j+1])) # Kalman filter update step derivatives dK = dP[:, :, j].dot(HtiS) - P.dot(HtiS).dot(dS) / S dm[:, j:j + 1] = dm[:, j:j + 1] + dK.dot(v) - K.dot(H).dot( dm[:, j:j + 1]) dKSKt = dK.dot(S).dot(K.T) dP[:, :, j] = dP[:, :, j] - dKSKt - K.dot(dS).dot(K.T) - dKSKt.T # Evaluate the energy # e = e - .5*S.shape[0]*np.log(2*np.pi) - np.sum(np.log(np.diag(LS))) - .5*vtiS.dot(v); e = e - .5 * S.shape[0] * np.log(2 * np.pi) - np.sum( np.log(np.sqrt(S))) - .5 * vtiS.dot(v) # Finish Kalman filter update step m = m + K.dot(v) P = P - K.dot(S).dot(K.T) # Make sure the covariances stay symmetric P = (P + P.T) / 2 dP = (dP + dP.transpose([1, 0, 2])) / 2 # raise NameError('Debug me') # Report #print e #print eg # Return the gradient return eg def simulate(self, F, L, Qc, Pinf, X, size=1): # Simulate a trajectory using the state space model # Allocate space for results f = np.zeros((F.shape[0], size, X.shape[1])) # Initial state f[:, :, 1] = np.linalg.cholesky(Pinf).dot(np.random.randn(F.shape[0], size)) # Time step lengths dt = np.empty(X.shape) dt[:, 0] = X[:, 1] - X[:, 0] dt[:, 1:] = np.diff(X) # Solve the LTI SDE for these time steps As, Qs, index = self.lti_disc(F, L, Qc, dt) # Sweep through remaining time points for k in range(1, X.shape[1]): # Form discrete-time model A = As[:, :, index[1 - k]] Q = Qs[:, :, index[1 - k]] # Draw the state f[:, :, k] = A.dot(f[:, :, k - 1]) + np.dot( np.linalg.cholesky(Q), np.random.randn(A.shape[0], size)) # Return values return f def lti_disc(self, F, L, Qc, dt): # Discrete-time solution to the LTI SDE # Dimensionality n = F.shape[0] index = 0 # Check for numbers of time steps if dt.flatten().shape[0] == 1: # The covariance matrix by matrix fraction decomposition Phi = np.zeros((2 * n, 2 * n)) Phi[:n, :n] = F Phi[:n, n:] = L.dot(Qc).dot(L.T) Phi[n:, n:] = -F.T AB = linalg.expm(Phi * dt).dot( np.vstack((np.zeros((n, n)), np.eye(n)))) Q = linalg.solve(AB[n:, :].T, AB[:n, :].T) # The dynamical model A = linalg.expm(F * dt) # Return return A, Q # Optimize for cases where time steps occur repeatedly else: # Time discretizations (round to 14 decimals to avoid problems) dt, _, index = np.unique(np.round(dt, 14), True, True) # Allocate space for A and Q A = np.empty((n, n, dt.shape[0])) Q = np.empty((n, n, dt.shape[0])) # Call this function for each dt for j in range(0, dt.shape[0]): A[:, :, j], Q[:, :, j] = self.lti_disc(F, L, Qc, dt[j]) # Return return A, Q, index