mirror of
https://github.com/SheffieldML/GPy.git
synced 2026-05-02 16:22:39 +02:00
745 lines
24 KiB
Python
745 lines
24 KiB
Python
# 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
|
|
|