def _lti_disc(self, dt):
"""Discretization of LTI state-space model
Args:
dt: sampling time
Returns:
Ad: Discrete state matrix
B0d, B1d: Discrete input matrix
Qd: Upper Cholesky factor process noise covariance
"""
Ad = expm(self.A * dt)
if not np.all(self.Q == 0):
Q2 = self.Q.T @ self.Q
Qd = pseudo_cholesky(lyap(self.A, -Q2 + Ad @ Q2 @ Ad.T))
else:
Qd = self._0xx[0, :, :]
if self.Nu != 0:
bis = solve(self.A, Ad - self._Ixx[0, :self.Nx, :self.Nx])
B0d = bis @ self.B
if self.hold_order == 'foh':
B1d = solve(self.A, -bis + Ad * dt) @ self.B
else:
B1d = self._0xu[0, :, :]
else:
B0d = self._0xu[0, :, :]
B1d = B0d
return Ad, B0d, B1d, Qd
def proj(self, Y, H):
eta = self._project_rows(Y, H)
# Projection onto the horizontal space
YtY = Y.T.dot(Y)
AS = Y.T.dot(eta) - H.T.dot(Y)
Omega = lyap(YtY, -AS)
return eta - Y.dot((Omega - Omega.T) / 2)
def proj(self, Y, H):
eta = self._project_rows(Y, H)
# Projection onto the horizontal space
YtY = Y.T.dot(Y)
AS = Y.T.dot(eta) - H.T.dot(Y)
Omega = lyap(YtY, -AS)
return eta - Y.dot((Omega - Omega.T) / 2)
def proj(self, Y, H):
# Projection onto the horizontal space
YtY = Y.T.dot(Y)
AS = Y.T.dot(H) - H.T.dot(Y)
Omega = lyap(YtY, AS)
return H - Y.dot(Omega)
def proj(self, Y, H):
# Projection onto the horizontal space
YtY = Y.T.dot(Y)
AS = Y.T.dot(H) - H.T.dot(Y)
Omega = lyap(YtY, AS)
return H - Y.dot(Omega)
def get_ergodic(F: np.ndarray, Q: np.ndarray, B: np.ndarray=None,
x_0: np.ndarray=None, force_diffuse: List[bool]=None,
is_warning: bool=True) -> Tuple[np.ndarray, np.ndarray]:
"""
Calculate initial state covariance matrix, and identify
diffuse state. It effectively solves a Lyapuov equation
Parameters:
----------
F : state transition matrix
Q : initial error covariance matrix
B : regression matrix
x_0 : initial x, used for calculating ergodic mean
force_diffuse : List of booleans of user-determined diffuse state
is_warning : whether to show warning message
Returns:
----------
P_0 : the initial state covariance matrix, np.inf for diffuse state
xi_0 : the initial state mean, 0 for diffuse state
"""
Q_ = Q.copy()
dim = Q.shape[0]
# Is is_diffuse is not supplied, create the list
if force_diffuse is None:
is_diffuse = np.zeros(dim, dtype=np.bool)
else:
is_diffuse = deepcopy(force_diffuse)
if len(is_diffuse) != dim:
raise ValueError('is_diffuse has wrong size')
# Check F and Q
if F.shape[0] != F.shape[1]:
raise TypeError('F must be a square matrix')
if Q.shape[0] != Q.shape[1]:
raise TypeError('Q must be a square matrix')
if F.shape[0] != Q.shape[0]:
raise TypeError('Q and F must be of same size')
# If explosive roots, use fully diffuse initialization
# and issue a warning
eig = linalg.eigvals(F)
if np.any(np.abs(eig) > 1):
if is_warning:
warnings.warn('Ft contains explosive roots. Assumptions ' + \
'of marginal LL correction may be violated, and ' + \
'results may be biased or inconsistent. Please provide ' + \
'user-defined xi_1_0 and P_1_0.', RuntimeWarning)
is_diffuse_explosive = get_explosive_diffuse(F)
is_diffuse = is_diffuse | is_diffuse_explosive
# Modify Q_ to reflect diffuse states
Q_ = mask_nan(is_diffuse, Q_, diag=inf_val)
# Calculate raw P_0
with warnings.catch_warnings():
warnings.simplefilter("ignore")
P_0 = lyap(F, Q_, 'bilinear')
# Clean up P_0
for i in range(dim):
if np.abs(P_0[i][i]) > max_val:
is_diffuse[i] = True
P_0 = mask_nan(is_diffuse, P_0, diag=0)
# Enforce PSD
P_0_PSD = get_nearest_PSD(P_0)
# Add nan to diffuse diagonal values
P_0_PSD += np.diag(np.array([np.nan if i else 0 for i in is_diffuse]))
# Compute ergodic mean
if B is None or x_0 is None:
Bx = np.zeros([dim, 1])
else:
if B.shape[0] != dim:
raise ValueError('B has the wrong dimension')
Bx = B.dot(x_0)
Bx[is_diffuse] = 0
F_star = F.copy()
F_star[is_diffuse] = 0
xi_0 = inv(np.eye(dim) - F_star).dot(Bx)
return P_0_PSD, xi_0
def sde(self):
"""
Return the state space representation of the covariance.
Note! For Sparse GP inference too small or two high values of lengthscale
lead to instabilities. This is because Qc are too high or too low
and P_inf are not full rank. This effect depends on approximatio order.
For N = 10. lengthscale must be in (0.8,8). For other N tests must be conducted.
N=6: (0.06,31)
Variance should be within reasonable bounds as well, but its dependence is linear.
The above facts do not take into accout regularization.
"""
#import pdb; pdb.set_trace()
if self.approx_order is not None:
N = self.approx_order
else:
N = 10 # approximation order ( number of terms in exponent series expansion)
roots_rounding_decimals = 6
fn = np.math.factorial(N)
p_lengthscale = float(self.lengthscale)
p_variance = float(self.variance)
kappa = 1.0 / 2.0 / p_lengthscale**2
Qc = np.array(
((p_variance * np.sqrt(np.pi / kappa) * fn * (4 * kappa)**N, ), ))
eps = 1e-12
if (float(Qc) > 1.0 / eps) or (float(Qc) < eps):
warnings.warn(
"""sde_RBF kernel: the noise variance Qc is either very large or very small.
It influece conditioning of P_inf: {0:e}""".
format(float(Qc)))
pp1 = np.zeros(
(2 * N + 1,
)) # array of polynomial coefficients from higher power to lower
for n in range(0, N + 1): # (2N+1) - number of polynomial coefficients
pp1[2 * (N - n)] = fn * (4.0 * kappa)**(
N - n) / np.math.factorial(n) * (-1)**n
pp = sp.poly1d(pp1)
roots = sp.roots(pp)
neg_real_part_roots = roots[
np.round(np.real(roots), roots_rounding_decimals) < 0]
aa = sp.poly1d(neg_real_part_roots, r=True).coeffs
F = np.diag(np.ones((N - 1, )), 1)
F[-1, :] = -aa[-1:0:-1]
L = np.zeros((N, 1))
L[N - 1, 0] = 1
H = np.zeros((1, N))
H[0, 0] = 1
# Infinite covariance:
Pinf = lyap(F, -np.dot(L, np.dot(Qc[0, 0], L.T)))
Pinf = 0.5 * (Pinf + Pinf.T)
# Allocating space for derivatives
dF = np.empty([F.shape[0], F.shape[1], 2])
dQc = np.empty([Qc.shape[0], Qc.shape[1], 2])
dPinf = np.empty([Pinf.shape[0], Pinf.shape[1], 2])
# Derivatives:
dFvariance = np.zeros(F.shape)
dFlengthscale = np.zeros(F.shape)
dFlengthscale[-1, :] = -aa[-1:0:-1] / p_lengthscale * np.arange(
-N, 0, 1)
dQcvariance = Qc / p_variance
dQclengthscale = np.array(
((p_variance * np.sqrt(2 * np.pi) * fn * 2**N *
p_lengthscale**(-2 * N) * (1 - 2 * N), ), ))
dPinf_variance = Pinf / p_variance
lp = Pinf.shape[0]
coeff = np.arange(1, lp + 1).reshape(lp, 1) + np.arange(
1, lp + 1).reshape(1, lp) - 2
coeff[np.mod(coeff, 2) != 0] = 0
dPinf_lengthscale = -1 / p_lengthscale * Pinf * coeff
dF[:, :, 0] = dFvariance
dF[:, :, 1] = dFlengthscale
dQc[:, :, 0] = dQcvariance
dQc[:, :, 1] = dQclengthscale
dPinf[:, :, 0] = dPinf_variance
dPinf[:, :, 1] = dPinf_lengthscale
P0 = Pinf.copy()
dP0 = dPinf.copy()
if self.balance:
# Benefits of this are not very sound. Helps only in one case:
# SVD Kalman + RBF kernel
import GPy.models.state_space_main as ssm
(F, L, Qc, H, Pinf, P0, dF, dQc, dPinf,
dP0) = ssm.balance_ss_model(F, L, Qc, H, Pinf, P0, dF, dQc, dPinf,
dP0)
return (F, L, Qc, H, Pinf, P0, dF, dQc, dPinf, dP0)
def sde(self):
"""
Return the state space representation of the covariance.
"""
N = 10 # approximation order ( number of terms in exponent series expansion)
roots_rounding_decimals = 6
fn = np.math.factorial(N)
kappa = 1.0 / 2.0 / self.lengthscale**2
Qc = np.array(
(self.variance * np.sqrt(np.pi / kappa) * fn * (4 * kappa)**N, ), )
pp = np.zeros(
(2 * N + 1,
)) # array of polynomial coefficients from higher power to lower
for n in range(0, N + 1): # (2N+1) - number of polynomial coefficients
pp[2 * (N - n)] = fn * (4.0 * kappa)**(
N - n) / np.math.factorial(n) * (-1)**n
pp = sp.poly1d(pp)
roots = sp.roots(pp)
neg_real_part_roots = roots[
np.round(np.real(roots), roots_rounding_decimals) < 0]
aa = sp.poly1d(neg_real_part_roots, r=True).coeffs
F = np.diag(np.ones((N - 1, )), 1)
F[-1, :] = -aa[-1:0:-1]
L = np.zeros((N, 1))
L[N - 1, 0] = 1
H = np.zeros((1, N))
H[0, 0] = 1
# Infinite covariance:
Pinf = lyap(F, -np.dot(L, np.dot(Qc[0, 0], L.T)))
Pinf = 0.5 * (Pinf + Pinf.T)
# Allocating space for derivatives
dF = np.empty([F.shape[0], F.shape[1], 2])
dQc = np.empty([Qc.shape[0], Qc.shape[1], 2])
dPinf = np.empty([Pinf.shape[0], Pinf.shape[1], 2])
# Derivatives:
dFvariance = np.zeros(F.shape)
dFlengthscale = np.zeros(F.shape)
dFlengthscale[-1, :] = -aa[-1:0:-1] / self.lengthscale * np.arange(
-N, 0, 1)
dQcvariance = Qc / self.variance
dQclengthscale = np.array(
((self.variance * np.sqrt(2 * np.pi) * fn * 2**N *
self.lengthscale**(-2 * N) * (1 - 2 * N, ), )))
dPinf_variance = Pinf / self.variance
lp = Pinf.shape[0]
coeff = np.arange(1, lp + 1).reshape(lp, 1) + np.arange(
1, lp + 1).reshape(1, lp) - 2
coeff[np.mod(coeff, 2) != 0] = 0
dPinf_lengthscale = -1 / self.lengthscale * Pinf * coeff
dF[:, :, 0] = dFvariance
dF[:, :, 1] = dFlengthscale
dQc[:, :, 0] = dQcvariance
dQc[:, :, 1] = dQclengthscale
dPinf[:, :, 0] = dPinf_variance
dPinf[:, :, 1] = dPinf_lengthscale
P0 = Pinf.copy()
dP0 = dPinf.copy()
# Benefits of this are not very sound. Helps only in one case:
# SVD Kalman + RBF kernel
import GPy.models.state_space_main as ssm
(F, L, Qc, H, Pinf, P0, dF, dQc, dPinf, dP0,
T) = ssm.balance_ss_model(F, L, Qc, H, Pinf, P0, dF, dQc, dPinf, dP0)
return (F, L, Qc, H, Pinf, P0, dF, dQc, dPinf, dP0)
def _lti_jacobian_disc(self, dt):
"""Discretization of augmented LTI state-space model
Args:
dt: sampling time
Returns:
Ad: Discrete state matrix
B0d, B1d: Discrete input matrix
Qd: Upper Cholesky factor process noise covariance
dAd: Derivative discrete state matrix
dB0d, dB1d: Derivative discrete input matrix
dQd: Derivative upper Cholesky factor process noise covariance
"""
names = self.parameters.names_free
N = len(names)
# Discrete state matrix and its derivative
self._AA[:N, :self.Nx, :self.Nx] = self.A
self._AA[:N, self.Nx:, self.Nx:] = self.A
self._AA[:N, self.Nx:, :self.Nx] = [self.dA[n] for n in names]
dA = self._AA[:N, self.Nx:, :self.Nx]
Ad = expm(self.A * dt)
for i in range(N):
self._AAd[i, :self.Nx, :self.Nx] = Ad
self._AAd[i, self.Nx:, self.Nx:] = Ad
if not np.all(dA[i, :, :] == 0):
self._AAd[i, self.Nx:, :self.Nx] = (expm_frechet(
self.A * dt, dA[i, :, :] * dt, 'SPS', False))
dAd = self._AAd[:N, self.Nx:, :self.Nx]
if not np.all(self.Q == 0):
dQ = np.asarray([self.dQ[k] for k in names])
# transform to covariance matrix
Q2 = self.Q.T @ self.Q
dQ2 = dQ.swapaxes(1, 2) @ self.Q + self.Q.T @ dQ
# covariance matrix of the process noise
Qd2 = lyap(self.A, -Q2 + Ad @ Q2 @ Ad.T)
eq = (-dA @ Qd2 - Qd2 @ dA.swapaxes(1, 2) - dQ2 + dAd @ Q2 @ Ad.T +
Ad @ dQ2 @ Ad.T + Ad @ Q2 @ dAd.swapaxes(1, 2))
# Derivative process noise covariance
dQd2 = np.asarray([lyap(self.A, eq[i, :, :]) for i in range(N)])
# Get Cholesky factor
Qd = pseudo_cholesky(Qd2)
tmp = solve(Qd.T, solve(Qd.T, dQd2).swapaxes(1, 2))
dQd = (np.triu(tmp, 1) + self._Ixx[0, :self.Nx, :self.Nx] * 0.5 *
tmp.diagonal(0, 1, 2)[:, np.newaxis, :]) @ Qd
else:
Qd = self._0xx[0, :, :]
dQd = self._0xx[:N, :, :]
if self.Nu != 0:
# Discrete input matrices and their derivatives
self._BB[:N, :self.Nx, :] = self.B
self._BB[:N, self.Nx:, :] = [self.dB[k] for k in names]
AA = self._AA[:N, :, :]
AAd = self._AAd[:N, :, :]
BB = self._BB[:N, :, :]
bis = solve(AA, AAd - self._Ixx[:N, :, :])
BB0d = bis @ BB
B0d = BB0d[0, :self.Nx, :]
dB0d = BB0d[:, self.Nx:, :]
if self.hold_order == 'foh':
BBd1_free = solve(AA, -bis + AAd * dt) @ BB
B1d = BBd1_free[0, :self.Nx, :]
dB1d = BBd1_free[:, self.Nx:, :]
else:
B1d = self._0xu[0, :, :]
dB1d = self._0xu[:N, :, :]
else:
B0d = self._0xu[0, :, :]
dB0d = self._0xu[:N, :, :]
B1d = B0d
dB1d = dB0d
return Ad, B0d, B1d, Qd, dAd, dB0d, dB1d, dQd
