From d061a1020786636652410b54e0e6bca25fdf034b Mon Sep 17 00:00:00 2001 From: Arno Solin Date: Thu, 26 Feb 2015 19:12:26 +0200 Subject: [PATCH] Updated Kalman filter implementation to new GPy --- GPy/examples/state_space.py | 10 + GPy/models/state_space.py | 745 ++++++++++++++++++++++++++++++++++++ 2 files changed, 755 insertions(+) create mode 100644 GPy/examples/state_space.py create mode 100644 GPy/models/state_space.py diff --git a/GPy/examples/state_space.py b/GPy/examples/state_space.py new file mode 100644 index 00000000..9e51088b --- /dev/null +++ b/GPy/examples/state_space.py @@ -0,0 +1,10 @@ +import GPy +import numpy as np +import matplotlib.pyplot as plt +from GPy.models.state_space import StateSpace + +X = np.linspace(0, 10, 2000)[:, None] +Y = np.sin(X) + np.random.randn(*X.shape)*0.1 + +kernel = GPy.kern.Matern32(X.shape[1]) +m = StateSpace(X,Y, kernel) diff --git a/GPy/models/state_space.py b/GPy/models/state_space.py new file mode 100644 index 00000000..0e3498d8 --- /dev/null +++ b/GPy/models/state_space.py @@ -0,0 +1,745 @@ +# Copyright (c) 2013, Arno Solin. +# Licensed under the BSD 3-clause license (see LICENSE.txt) +# +# This implementation of converting GPs to state space models is based on the article: +# +# @article{Sarkka+Solin+Hartikainen:2013, +# author = {Simo S\"arkk\"a and Arno Solin and Jouni Hartikainen}, +# year = {2013}, +# title = {Spatiotemporal learning via infinite-dimensional {B}ayesian filtering and smoothing}, +# journal = {IEEE Signal Processing Magazine}, +# volume = {30}, +# number = {4}, +# pages = {51--61} +# } +# + +import numpy as np +from scipy import linalg +from ..core import Model +from .. import kern +from GPy.plotting.matplot_dep.models_plots import gpplot +from GPy.plotting.matplot_dep.base_plots import x_frame1D +from GPy.plotting.matplot_dep import Tango +import pylab as pb +from GPy.core.parameterization.param import Param + +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 +