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): # 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