def _disc_dQ_lyapunov( self, dt: np.ndarray, QQ: np.ndarray, dA: np.ndarray, dQQ: np.ndarray, Ad: np.ndarray, dAd: np.ndarray, ) -> Tuple[np.ndarray, np.ndarray]: """Discretization partial derivative of the diffusion matrix with the Lyapunov equation Args: dt: Sampling time QQ: Diffusion matrix dA: Jacobian state matrix dQQ: Jacobian diffusion matrix Ad: Discrete state matrix dAd: Jacobian discrete state matrix Returns: 2-elements tuple containing - **QQd**: Process noise covariance matrix - **dQQd**: Jacobian process noise covariance matrix """ N = dA.shape[0] QQd = self._disc_Q_lyapunov(QQ, Ad) eq = (-dA @ QQd - QQd @ dA.swapaxes(1, 2) - dQQ + dAd @ QQ @ Ad.T + Ad @ dQQ @ Ad.T + Ad @ QQ @ dAd.swapaxes(1, 2)) dQQd = np.asarray( [solve_continuous_lyapunov(self.A, eq[i, :, :]) for i in range(N)]) return QQd, dQQd
def model_covariance(self, tau=0.0): """ Calculates theoretical (lagged) covariances of the model given the parameters (forward step). Notice that this is not the empirical covariance matrix as estimated from simulated time series. Parameters ---------- tau : scalar The time lag to calculate the covariance. It can be a positive or negative. Returns ------- FC : ndarray of rank-2 The (lagged) covariance matrix. """ # Calculate zero lag-covariance Q0 by solving Lyapunov equation Q0 = spl.solve_continuous_lyapunov(self.J.T, -self.Sigma) # Calculate the effect of the lag (still valid for tau = 0.0) if tau >= 0.0: return np.dot(Q0, spl.expm(tau * self.J)) else: return np.dot(spl.expm(-tau * self.J.T), Q0)
def computeB(Fp, Fpp, Fppp, Qp, Qpp, V, W, D, C): FppC = symetric_tensor(np.tensordot(Fpp, C, 2)) FpppD = symetric_tensor(np.tensordot(Fppp, D, 3)) QpV = symetric_tensor(np.tensordot(Qp, V, 1)) QppW = symetric_tensor(np.tensordot(Qpp, W, 2)) b = -(FppC + FpppD / 3 + QpV + QppW / 2) return (solve_continuous_lyapunov(Fp, b))
def efficient_solver(wm, gm, k, d0, g0, ep, g2, n, recalib=True, checks=False): ##initialisation of some data arrays for speed## r = np.zeros((4), dtype=np.complex128) ##finding x0## a = 0.25 * g2**2 * (gm**2 + 4 * wm**2) b = -((g0 * g2 * (gm**2 + 4 * wm**2)) / 2**0.5) c = (1 / 2) * g0**2 * (gm**2 + 4 * wm**2) + (1 / 2) * d0 * g2 * (gm**2 + 4 * wm**2) d = -((d0 * g0 * (gm**2 + 4 * wm**2)) / 2**0.5) e = 2 * ep**2 * g2 * wm + (1 / 4) * d0**2 * (gm**2 + 4 * wm**2) + ( 1 / 16) * k**2 * (gm**2 + 4 * wm**2) f = (-2**0.5) * ep**2 * g0 * wm roots_x0 = np.roots(np.array([a, b, c, d, e, f], dtype=np.complex128)) #numpy roots function r[2] = roots_x0[ len(roots_x0) - 1] #last root in the array given is always the real root, also this is x0 x = r[ 2] #as far as i know this utilises data casting, so no extra memory use or speed loss ##finding p0,x1,p1, and putting it all into a numpy array## r[3] = (0.5 * gm * (wm)**-1 * x) #p0 ## ##recalibration if needed-idk about how this works but ill implement for the moment and turn it off## d_eff = wm + 2**0.5 * g0 * x - g2 * x**2 if recalib == True: d0 = wm w_eff = wm + (wm**2 + 0.25 * k**2)**-1 * 2 * g2 * ep**2 g_eff = 2 * g2 * x - 2**0.5 * g0 else: pass ## r[0] = (2**-0.5 * -2 * d_eff * ep * (d_eff**2 + k**2 * 0.25)**-1) #x1 (Q0) r[1] = (2**-0.5 * -1 * k * ep * (d_eff**2 + k**2 * 0.25)**-1) # p1 (P1) ##solving covariance matrix H = np.array([[hb * d_eff, 0, hb * r[0] * g_eff, 0], [0, hb * d_eff, hb * r[1] * g_eff, 0], [hb * r[0] * g_eff, hb * g_eff * r[1], hb * w_eff, 0], [0, 0, 0, hb * wm]]) gamma = 0.5 * np.array([[k, -k * i, 0, 0], [k * i, k, 0, 0], [0, 0, gm * (2 * n + 1), -1 * i * gm], [0, 0, gm * i, gm * (2 * n + 1)]]) gamma_A = 0.5 * (gamma - np.transpose(gamma)) gamma_S = 0.5 * (gamma + np.transpose(gamma)) A = -1 * i / hb * np.dot(W, H) + np.dot(W, gamma_A) C = -1 * np.dot(W, np.dot(gamma_S, W)) cov = linalg.solve_continuous_lyapunov(A, C) if checks == True: rs_test = 2 * cov + W print('RS-Matrix-eigenvalues:') print(np.linalg.eig(rs_test)[0]) print('B-Matrix-eigenvalues:') print(np.linalg.eig(np.transpose(A))[0]) else: pass return np.real(r), np.real(cov)
def _disc_Q_lyapunov(self, QQ: np.ndarray, Ad: np.ndarray) -> np.ndarray: """Discretization diffusion matrix by Lyapunov equation Args: QQ: Diffusion matrix Ad: Discrete state matrix Returns: Process noise covariance matrix """ return solve_continuous_lyapunov(self.A, -QQ + Ad @ QQ @ Ad.T)
def solve_lyap_dense(A, E, B, trans=False, options=None): """Compute the solution of a Lyapunov equation. See :func:`pymor.algorithms.lyapunov.solve_lyap_dense` for a general description. This function uses `scipy.linalg.solve_continuous_lyapunov`, which is a dense solver for Lyapunov equations with E=I. .. note:: If E is not `None`, the problem will be reduced to a standard continuous-time algebraic Lyapunov equation by inverting E. Parameters ---------- A The operator A as a 2D |NumPy array|. E The operator E as a 2D |NumPy array| or `None`. B The operator B as a 2D |NumPy array|. trans Whether the first operator in the Lyapunov equation is transposed. options The solver options to use (see :func:`lyap_dense_solver_options`). Returns ------- X Lyapunov equation solution as a |NumPy array|. """ _solve_lyap_dense_check_args(A, E, B, trans) options = _parse_options(options, lyap_dense_solver_options(), 'scipy', None, False) if options['type'] == 'scipy': if E is not None: A = solve(E, A) if not trans else solve(E.T, A.T).T B = solve(E, B) if not trans else solve(E.T, B.T).T if trans: A = A.T B = B.T X = solve_continuous_lyapunov(A, -B.dot(B.T)) else: raise ValueError( f"Unexpected Lyapunov equation solver ({options['type']}).") return X
def build_ctle(affine_linearizable_dynamics, K, Q): """Build AffineQuadCLF from affine and linearizable dynamics with gain matrix by solving continuous-time Lyapunov equation (CTLE). CTLE is A' * P + P * A = -Q, where A = F - G * K is closed-loop matrix. Inputs: Affine and linearizable dynamics, affine_linearizable_dynamics: AffineDynamics, LinearizableDynamics Positive-definite state cost matrix, Q: numpy array Gain matrix, K: numpy array """ A = affine_linearizable_dynamics.closed_loop_linear_system(K) P = solve_continuous_lyapunov(A.T, -Q) return AffineQuadCLF(affine_linearizable_dynamics, P)
def disc_diffusion_lyap(A: np.ndarray, Q: np.ndarray, Ad: np.ndarray) -> np.ndarray: """Discretize the diffusion matrix by solving the Lyapunov equation Args: A: State matrix Q: Diffusion matrix Ad: Discrete state matrix Returns: Process noise covariance matrix """ if not Q.any(): return Q return solve_continuous_lyapunov(A, -Q + Ad @ Q @ Ad.T)
def solve_lyap_dense(A, E, B, trans=False, options=None): """Compute the solution of a Lyapunov equation. See :func:`pymor.algorithms.lyapunov.solve_lyap_dense` for a general description. This function uses `scipy.linalg.solve_continuous_lyapunov`, which is a dense solver for Lyapunov equations with E=I. .. note:: If E is not `None`, the problem will be reduced to a standard continuous-time algebraic Lyapunov equation by inverting E. Parameters ---------- A The operator A as a 2D |NumPy array|. E The operator E as a 2D |NumPy array| or `None`. B The operator B as a 2D |NumPy array|. trans Whether the first operator in the Lyapunov equation is transposed. options The solver options to use (see :func:`lyap_dense_solver_options`). Returns ------- X Lyapunov equation solution as a |NumPy array|. """ _solve_lyap_dense_check_args(A, E, B, trans) options = _parse_options(options, lyap_dense_solver_options(), 'scipy', None, False) if options['type'] == 'scipy': if E is not None: A = solve(E, A) if not trans else solve(E.T, A.T).T B = solve(E, B) if not trans else solve(E.T, B.T).T if trans: A = A.T B = B.T X = solve_continuous_lyapunov(A, -B.dot(B.T)) else: raise ValueError(f"Unexpected Lyapunov equation solver ({options['type']}).") return X
def controllability_gramian(self): """Controllability gramian matrix.""" from scipy import linalg B = self.B.evaluate() Q = -B @ B.T # Find Wc given A @ Wc + Wc @ A.T = Q # Wc > o if (A, B) controllable Wc = linalg.solve_continuous_lyapunov(self.A.evaluate(), Q) # Wc should be symmetric positive semi-definite Wc = (Wc + Wc.T) / 2 return Matrix(Wc)
def observability_gramian(self): """Observability gramian matrix.""" from scipy import linalg C = self.C.evaluate() Q = -C.T @ C # Find Wo given A.T @ Wo + Wo @ A = Q # Wo > o if (C, A) observable Wo = linalg.solve_continuous_lyapunov(self.A.evaluate().T, Q) # Wo should be symmetric positive semi-definite Wo = (Wo + Wo.T) / 2 return Matrix(Wo)
def solve_cov(self, r): self.gamma = 0.5 * np.array( [[self.k, -self.k * i, 0, 0], [self.k * i, self.k, 0, 0], [0, 0, self.gm * (2 * self.n + 1), -1 * i * self.gm], [0, 0, self.gm * i, self.gm * (2 * self.n + 1)]]) self.gamma_A = 0.5 * (self.gamma - np.transpose(self.gamma)) self.gamma_S = 0.5 * (self.gamma + np.transpose(self.gamma)) self.H = np.array([[hb * self.deff, 0, hb * self.r[0] * self.g_eff, 0], [0, hb * self.deff, hb * self.r[1] * self.g_eff, 0], [ hb * self.r[0] * self.g_eff, hb * self.g_eff * self.r[1], hb * self.w_eff, 0 ], [0, 0, 0, hb * self.wm]]) self.A = -1 * i / hb * np.dot(W, self.H) + np.dot(W, self.gamma_A) self.C = -1 * np.dot(W, np.dot(self.gamma_S, W)) self.cov = linalg.solve_continuous_lyapunov(self.A, self.C) return self.cov
def createDiscreteTimeSys(self, Ts): """ Builds the discrete time state-space system in canonical form, using numerator and denominator coefficients of the companion form INPUT: Sampling time Ts for the discretization OUTPUT: matrix A,C,V,Q: state matrix, output matrix, state variance matrix (solution of lyapunov equation), and measurement variance matrix """ # state dimension num_coeff, den_coeff = self.psd() F, state_dim = self.state_matrix() # State matrix A = self.state_transition(Ts) # State transition matrix = expm(F*Ts) G = np.eye(1, state_dim, k=state_dim - 1).T # Input matrix # output matrix C = np.zeros((1, state_dim))[0] C[0:np.max(num_coeff.shape)] = num_coeff # state variance as solution of the lyapunov equation V = solve_continuous_lyapunov(F, -np.matmul(G, G.conj().T)) # discretization of the noise matrix Q = np.zeros(state_dim) Ns = 10000 t = Ts / Ns if state_dim == 1: for n in np.arange(t, Ts + t, step=t): Q = Q + t * np.exp(F * n) * np.dot(G, G.conj().T) * np.exp( F * n).conj().T else: for n in np.arange(t, Ts + t, step=t): #Q = Q + np.linalg.multi_dot([t, expm(np.dot(F,n)), np.dot(G,G.conj().T), expm(np.dot(F,n)).conj().T]) Fn = self.state_transition( n) # REMEMBER TO WRITE ABOUT THIS OPTIMIZATION Q = Q + t * Fn @ (G @ G.conj().T) @ Fn.conj().T return A, C, V, Q
def controllability_gramian_continuous(mat_a, mat_b): """ Calculates the controllabilty gramian of a continuous time LTI system. Parameters ---------- mat_a : ndarray The state matrix. mat_b : ndarray The input matrix. Returns ------- mat_wc : ndarray The controllability gramian. """ mat_q = -mat_b @ mat_b.T mat_wc = linalg.solve_continuous_lyapunov(mat_a, mat_q) return mat_wc
def observability_gramian_continuous(mat_a, mat_c): """ Calculates the observability gramian of a continuous time LTI system. Parameters ---------- mat_a : ndarray The state matrix. mat_c : ndarray The output matrix. Returns ------- mat_wo : ndarray The observability gramian. """ mat_q = -mat_c.T @ mat_c mat_wo = linalg.solve_continuous_lyapunov(mat_a.T, mat_q) return mat_wo
def calc_Q0_from_param(self, J, Sigma): """ Calculates the zero-lag covariance matrix from a Jacobian J and an input covariance matrix Sigma Parameters ---------- J : ndarray of rank 2 The Jacobian matrix. Shape [n x n] Sigma : ndarray of rank 2 The input covariance matrix. Shape [n x n] Returns ------- Q0 : ndarray of rank 2 The theoretical zero-lag covariance matrix. Shape [n, n] """ Q0 = spl.solve_continuous_lyapunov(J.T, -Sigma) return Q0
def build_ctle(feedback_linearizable_output, K, Q): """Build a quadratic CLF from a FeedbackLinearizableOutput with auxilliary control gain matrix, by solving the continuous time Lyapunov equation (CTLE). CTLE is A_cl' P + P A_cl = -Q for specified Q. Outputs a QuadraticControlLyapunovFunction. Inputs: Auxilliary control gain matrix, K: numpy array (k, p) Positive definite matrix for CTLE, Q: numpy array (p, p) """ A = feedback_linearizable_output.closed_loop_dynamics(K) P = solve_continuous_lyapunov(A.T, -Q) alpha = min(eigvals(Q)) / max(eigvals(P)) return QuadraticControlLyapunovFunction(feedback_linearizable_output, P, alpha)
def lna_covariance(rfp, gfp, atc, iptg, par, lamb, size): ''' This function estimates the covariance matrix of the rfp and gfp measurments taken at a specific point using the LNA approximation. This covariance matrix is not scaled by the system size by default, the user must do this to the result i.e. cov/size This function computes the covariance of a single celled observation. If many single-celled observations are taken at the same input point, the covariance matrix entries will be smaller for the mean of the observations than for the individual values (law of large numbers). The covariance matrix for a given single celled observation (as computed here) can be scaled by the number of observations taken to get the covariance matrix for the mean. ''' #define the steady-state functions for each species rfp_func = lambda gfp, atc: param[0] + param[1] / (1 + ( (gfp / param[2]) * (1 / (1 + (atc / param[3])**2.0)))**2.0) gfp_func = lambda rfp, iptg: param[4] + param[5] / (1 + ( (rfp / param[6]) * (1 / (1 + (iptg / param[7])**2.0)))**2.0) #define the derivatives of the steady state-functions w.r.t. the opposite species rfp_d_gfp = lambda gfp,atc: -2*param[1]*gfp*((1/param[2])*(1/(1+(atc/param[3])**2.0)))**2.0 \ /(1+(gfp**2)*((1/param[2])*(1/(1+(atc/param[3])**2.0)))**2.0) gfp_d_rfp = lambda rfp,iptg: -2*param[5]*rfp*((1/param[6])*(1/(1+(iptg/param[7])**2.0)))**2.0 \ /(1+(rfp**2)*((1/param[6])*(1/(1+(iptg/param[7])**2.0)))**2.0) #compute the A (damping) matrix A = np.array([[-lamb, lamb * rfp_d_gfp(gfp, atc)], [lamb * gfp_d_rfp(rfp, iptg), -lamb]]) #compute the B (fluctuation) matrix B = np.array([[lamb * (rfp_func(gfp, atc) + rfp), 0], [0, lamb * (gfp_d_rfp(rfp, iptg) + gfp)]]) #solve the sylvester/lyapunov equation (this may be ill-conditioned near where a branch # disappears i.e. a bifurcation point C = ln.solve_continuous_lyapunov(A, -B) return C
def disc_d_diffusion_lyap( A: np.ndarray, Q: np.ndarray, Ad: np.ndarray, dA: np.ndarray, dQ: np.ndarray, dAd: np.ndarray ) -> Tuple[np.ndarray, np.ndarray]: """Discretize diffusion matrix and its derivative with the Lyapunov equation Args: A: Sampling time Q: Diffusion matrix Ad: Discrete state matrix dA: Derivative state matrix dQ: Derivative diffusion matrix dAd: Derivative discrete state matrix Returns: 2-elements tuple containing - **Qd**: Process noise covariance matrix - **dQd**: Derivative process noise covariance matrix """ if not Q.any(): return Q nj, nx, _ = dA.shape Qd = disc_diffusion_lyap(A, Q, Ad) dQd = np.zeros((nj, nx, nx)) tmp = ( -dA @ Qd - Qd @ dA.swapaxes(1, 2) - dQ + dAd @ Q @ Ad.T + Ad @ dQ @ Ad.T + Ad @ Q @ dAd.swapaxes(1, 2) ) for n in range(nj): if tmp[n].any(): dQd[n] = solve_continuous_lyapunov(A, tmp[n]) return Qd, dQd
def fit_moments(self, Q0_obj, Q1_obj, mask_C=None): """ Estimation of MOU parameters (connectivity C, noise covariance Sigma, and time constant tau_x) with moments method. Parameters ---------- Q0_obj : ndarray of rank 2 The zero-lag covariance matrix of the time series to fit. Q1_obj : ndarray of rank 2 The 1-lag covariance matrix of the time series to fit. mask_C : boolean ndarray of rank-2 (optional) Mask of known non-zero values for connectivity matrix, for example estimated by DTI. Returns ------- J : ndarray of rank 2 The estimated Jacobian. Shape [n_nodes, n_nodes] Sigma : ndarray of rank 2 Estimated noise covariance. Shape [n_nodes, n_nodes] d_fit : dictionary A dictionary with diagnostics of the fit. Keys are: ['iterations', 'distance', 'correlation']. """ # Jacobian estimate inv_Q0 = np.linalg.inv(Q0_obj) J = spl.logm(np.dot(inv_Q0, Q1_obj)) # Sigma estimate Sigma = -np.dot(J.conjugate(), Q0_obj) - np.dot(Q0_obj, J) # masks for existing positions mask_diag = np.eye(self.n_nodes, dtype=np.bool) if mask_C is None: # Allow all possible connections to be tuned except self-connections (on diagonal) mask_C = np.logical_not(mask_diag) # cast to real matrices if np.any(np.iscomplex(J)): print("Warning: complex values in J; casting to real!") J_best = np.real(J) J_best[np.logical_not(np.logical_or(mask_C, mask_diag))] = 0 if np.any(np.iscomplex(Sigma)): print("Warning: complex values in Sigma; casting to real!") Sigma_best = np.real(Sigma) # model theoretical covariances with real J and Sigma Q0 = spl.solve_continuous_lyapunov(J_best.T, -Sigma_best) Q1 = np.dot(Q0, spl.expm(J_best)) # Calculate error between model and empirical data for Q0 and FC_tau (matrix distance) dist_Q0 = np.linalg.norm(Q0 - Q0_obj) / np.linalg.norm(Q0_obj) dist_Qtau = np.linalg.norm(Q1 - Q1_obj) / np.linalg.norm(Q1_obj) self.d_fit['distance'] = 0.5 * (dist_Q0 + dist_Qtau) # Average correlation between empirical and theoretical Pearson_Q0 = stt.pearsonr(Q0.reshape(-1), Q0_obj.reshape(-1))[0] Pearson_Qtau = stt.pearsonr(Q1.reshape(-1), Q1_obj.reshape(-1))[0] self.d_fit['correlation'] = 0.5 * (Pearson_Q0 + Pearson_Qtau) # Save the results and return self.J = J_best # matrix self.Sigma = Sigma_best # matrix return self
def fit_LO(self, Q_obj, i_tau_opt=1, mask_C=None, mask_Sigma=None, epsilon_C=0.0001, epsilon_Sigma=0.01, regul_C=0.0, regul_Sigma=0.0, min_val_C=0.0, max_val_C=1.0, min_val_Sigma_diag=0.0, max_iter=10000, min_iter=10, **kwargs): """ Estimation of MOU parameters (connectivity C, noise covariance Sigma, and time constant tau_x) with Lyapunov optimization as in: Gilson et al. Plos Computational Biology (2016). Parameters ---------- Q_obj : ndarray The covariance matrix i_tau_opt : integer (optional) Mask of known non-zero values for connectivity matrix, for example estimated by DTI. mask_C : boolean ndarray of rank-2 (optional) Mask of known non-zero values for connectivity matrix, for example estimated by DTI. By default, all connections are allowed (except self-connections) mask_Sigma : boolean ndarray of rank-2 (optional) Mask of known non-zero values for the input covariance matrix (by default diagonal). epsilon_C : float (optional) Learning rate for connectivity (this should be about n_nodes times smaller than epsilon_Sigma). epsilon_Sigma : float (optional) Learning rate for Sigma (this should be about n_nodes times larger than epsilon_EC). regul_C : float (optional) Regularization parameter for connectivity. Try first a value of 0.5. regul_Sigma : float (optional) Regularization parameter for Sigma. Try first a value of 0.001. min_val_C : float (optional) Minimum value to bound connectivity estimate. This should be zero or slightly negative (too negative limit can bring to an inhibition dominated system). If the empirical covariance has many negative entries then a slightly negative limit can improve the estimation accuracy. max_val_C : float (optional) Maximum value to bound connectivity estimate. This is useful to avoid large weight that make the system unstable. If the estimated connectivity saturates toward this value (it usually doesn't happen) it can be increased. max_iter : integer (optional) Number of maximum optimization steps. If final number of iterations reaches this maximum it means the algorithm has not converged. min_iter : integer (optional) Number of minimum optimization steps before testing if end of optimization (increase of model error). Returns ------- J : ndarray of rank 2 The estimated Jacobian. Shape [n_nodes, n_nodes] Sigma : ndarray of rank 2 Estimated noise covariance. Shape [n_nodes, n_nodes] d_fit : dictionary A dictionary with diagnostics of the fit. Keys are: ['iterations', 'distance', 'correlation']. """ # TODO: make better graphics (deal with axes separation, etc.) if (not type(i_tau_opt) == int) or (i_tau_opt <= 0): raise ValueError('Scalar value i_tau_opt must be non-zero') # Objective FC matrices (empirical) Q0_obj = Q_obj[0] Qtau_obj = Q_obj[i_tau_opt] # Autocovariance time constant (exponential decay) log_ac = np.log(np.maximum(Q_obj.diagonal(axis1=1, axis2=2), 1e-10)) v_tau = np.arange(Q_obj.shape[0], dtype=np.float) lin_reg = np.polyfit(np.repeat(v_tau, self.n_nodes), log_ac.reshape(-1), 1) tau_obj = -1.0 / lin_reg[0] # coefficients to balance the model error between Q0 and Qtau norm_Q0_obj = np.linalg.norm(Q0_obj) norm_Qtau_obj = np.linalg.norm(Qtau_obj) # mask for existing connections for EC and Sigma mask_diag = np.eye(self.n_nodes, dtype=bool) if mask_C is None: # Allow all possible connections to be tuned except self-connections (on diagonal) mask_C = np.logical_not(mask_diag) if mask_Sigma is None: # Independent noise (no cross-covariances for Sigma) mask_Sigma = np.eye(self.n_nodes, dtype=bool) # Initialise network and noise. Give initial parameters C = np.zeros([self.n_nodes, self.n_nodes], dtype=np.float) tau_x = np.copy(tau_obj) Sigma = np.eye(self.n_nodes, dtype=np.float) # Best distance between model and empirical data best_dist = 1e10 best_Pearson = 0.0 # Arrays to record model parameters and outputs # model error = matrix distance between FC matrices dist_Q_hist = np.zeros([max_iter], dtype=np.float) # Pearson correlation between model and objective FC matrices Pearson_Q_hist = np.zeros([max_iter], dtype=np.float) # identity matrix id_mat = np.eye(self.n_nodes, dtype=np.float) # run the optimization process stop_opt = False i_iter = 0 while not stop_opt: # calculate Jacobian of dynamical system J = -id_mat / tau_x + C # Calculate Q0 and Qtau for model Q0 = spl.solve_continuous_lyapunov(J.T, -Sigma) Qtau = np.dot(Q0, spl.expm(J * i_tau_opt)) # difference matrices between model and objectives Delta_Q0 = Q0_obj - Q0 Delta_Qtau = Qtau_obj - Qtau # Calculate error between model and empirical data for Q0 and FC_tau (matrix distance) dist_Q0 = np.linalg.norm(Delta_Q0) / norm_Q0_obj dist_Qtau = np.linalg.norm(Delta_Qtau) / norm_Qtau_obj dist_Q_hist[i_iter] = 0.5 * (dist_Q0 + dist_Qtau) # Calculate corr between model and empirical data for Q0 and FC_tau Pearson_Q0 = stt.pearsonr(Q0.reshape(-1), Q0_obj.reshape(-1))[0] Pearson_Qtau = stt.pearsonr(Qtau.reshape(-1), Qtau_obj.reshape(-1))[0] Pearson_Q_hist[i_iter] = 0.5 * (Pearson_Q0 + Pearson_Qtau) # Best fit given by best Pearson correlation coefficient # for both Q0 and Qtau (better than matrix distance) if dist_Q_hist[i_iter] < best_dist: best_dist = dist_Q_hist[i_iter] best_Pearson = Pearson_Q_hist[i_iter] J_best = np.copy(J) Sigma_best = np.copy(Sigma) else: # wait at least 5 optimization steps before stopping stop_opt = i_iter > min_iter # Jacobian update with weighted FC updates depending on respective error Delta_J = np.dot( np.linalg.pinv(Q0), Delta_Q0 ) + np.dot( Delta_Q0, np.linalg.pinv(Q0) ) \ + np.dot( np.linalg.pinv(Qtau), Delta_Qtau ) + np.dot( Delta_Qtau, np.linalg.pinv(Qtau) ) # Update effective conectivity matrix (regularization is L2) C[mask_C] += epsilon_C * (Delta_J - regul_C * C)[mask_C] C[mask_C] = np.clip(C[mask_C], min_val_C, max_val_C) # Update noise matrix Sigma (regularization is L2) Delta_Sigma = -np.dot(J.T, Delta_Q0) - np.dot(Delta_Q0, J) Sigma[mask_Sigma] += epsilon_Sigma * ( Delta_Sigma - regul_Sigma * Sigma)[mask_Sigma] Sigma[mask_diag] = np.maximum(Sigma[mask_diag], min_val_Sigma_diag) # Check if max allowed number of iterations have been reached if i_iter >= max_iter - 1: stop_opt = True print( "Optimization did not converge. Maximum number of iterations arrived." ) # Check if iteration has finished or still continues if stop_opt: self.d_fit['iterations'] = i_iter + 1 self.d_fit['distance'] = best_dist self.d_fit['correlation'] = best_Pearson self.d_fit['distance history'] = dist_Q_hist self.d_fit['correlation history'] = Pearson_Q_hist else: i_iter += 1 # Save the results and return self.J = J_best # matrix self.Sigma = Sigma_best # matrix return self
def computeW(Fp, Q): return (solve_continuous_lyapunov(Fp, -Q))
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain Ap = np.zeros((12, 12)) Ap[0:6, 6:] = np.eye(6) Ap[6, 4] = self.g Ap[7, 3] = -self.g Bp = np.zeros((12, 4)) Bp[8, 0] = 1 / self.m Bp[9, 1] = 1 / self.Ix Bp[10, 2] = 1 / self.Iy Bp[11, 3] = 1 / self.Iz Cp = np.zeros((4, 12)) Cp[:3, 0:3] = np.eye(3) Cp[3, 5] = 1 Aupper = np.hstack((Ap, np.zeros((12, 4)))) Alower = np.hstack((Cp, np.zeros((4, 4)))) At = np.vstack((Aupper, Alower)) Bt = np.vstack((Bp, np.zeros((4, 4)))) Bc = np.vstack((np.zeros((12, 4)), -np.eye(4))) BtBc = np.hstack((Bt, Bc)) Ct = np.hstack((Cp, np.zeros((4, 4)))) Dt = np.zeros((4, 8)) delT = 0.01 CT_sys = signal.StateSpace(At, BtBc, Ct, Dt) DT_sys = CT_sys.to_discrete(delT) A_dt = DT_sys.A B_dt = DT_sys.B A_d = A_dt B_td = B_dt[:, :4] B_d = B_td Bc_d = B_dt[:, 4:] B = Bt A = At # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1 / max_states**2) R = np.diag(1 / max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # self.Gamma = 3e-3 * np.eye(16) Q_lyap = np.copy(Q) Q_lyap[0:3, 0:3] *= 30 Q_lyap[6:9, 6:9] *= 150 Q_lyap[14, 14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model delT = self.timestep*1e-3 g = self.g m = self.m Ix = self.Ix Iy = self.Iy Iz = self.Iz n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain A = np.zeros((16, 16)) A[0,6] = 1; A[1,7] = 1; A[2,8] = 1; A[3,9] = 1; A[4,10] = 1; A[5,11] = 1 A[6,4] = g; A[7,3] = -g A[12,0] = 1 A[13,1] = 1 A[14,2] = 1 A[15,5] = 1 B = np.zeros((16, 4)) B[8,0] = 1 / m; B[9,1] = 1 / Ix; B[10,2] = 1 / Iy; B[11,3] = 1 / Iz Bc = np.zeros((16,4)) Bc[12,0] = -1; Bc[13,1] = -1; Bc[14,2] = -1; Bc[15,3] = -1 concentrate_B = np.concatenate((B, Bc), axis=1) C = np.zeros((4,16)) C[0,0] = 1; C[1,1] = 1; C[2,2] = 1; C[3,5] = 1 D = np.zeros((4,8)) sys_Con = signal.StateSpace(A,concentrate_B,C,D) sys_Dis = sys_Con.to_discrete(delT) A_d = sys_Dis.A B_d = sys_Dis.B[:,0:4] Bc_d = sys_Dis.B[:,4:8] C_d = sys_Dis.C D_d = sys_Dis.D # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # # max_pos = 15.0 # max_ang = 0.2 * self.pi # max_vel = 6.0 # max_rate = 0.015 * self.pi # max_eyI = 3. # max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, # max_ang, max_ang, max_ang, # 0.5 * max_vel, 0.5 * max_vel, max_vel, # max_rate, max_rate, max_rate, # 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) # max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2) # R = np.diag(1/max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3.0 max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1/max_states**2) R = np.diag(1/max_inputs**2) # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # # self.Gamma = 3e-3 * np.eye(16) # Q_lyap = np.copy(Q) # Q_lyap[0:3,0:3] *= 30 # Q_lyap[6:9,6:9] *= 150 # Q_lyap[14,14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance self.Gamma = 3e-3 * np.eye(16) Q_lyap = np.copy(Q) Q_lyap[0:3,0:3] *= 30 Q_lyap[6:9,6:9] *= 150 Q_lyap[14,14] *= 2e-3 # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)
def weight_stability(t, n, eta, init_params, jacobians, ntk, init_preds, Y, continuous=False, without_sgd=True, l2_reg_coef=0.0, large_model_regime=False, model=None, dataset=None, return_change_vectors=True, **kwargs): """ :param without_sgd: if without_sgd = True, then only ||w1-w2|| will be returned, otherwise (w1-w2)^T H Sigma^{-1} (w1-w2). """ if l2_reg_coef > 0: ntk = ntk + l2_reg_coef * torch.eye( ntk.shape[0], dtype=torch.float, device=ntk.device) ntk_inv = torch.inverse(ntk) old_weights = get_weights_at_time_t(t=t, eta=eta, init_params=init_params, jacobians=jacobians, ntk=ntk, ntk_inv=ntk_inv, init_preds=init_preds, Y=Y, continuous=continuous, large_model_regime=large_model_regime, model=model, dataset=dataset, **kwargs) steady_state_inv_cov = None if not without_sgd: if large_model_regime: raise ValueError("SGD formula works only for small models") # compute the SGD noise covariance matrix at the end assert (model is not None) and (dataset is not None) with utils.SetTemporaryParams(model=model, params=old_weights): sgd_cov = get_sgd_covariance_full(model=model, dataset=dataset, cpu=False, **kwargs) # add small amount of isotropic Gaussian noise to make sgd_cov invertible sgd_cov += 1e-10 * torch.eye( sgd_cov.shape[0], device=sgd_cov.device, dtype=torch.float) # now we compute H Sigma^{-1} jacobians_cat = [ v.view((v.shape[0], -1)) for k, v in jacobians.items() ] jacobians_cat = torch.cat(jacobians_cat, dim=1) # (n_samples * n_outputs, n_params) H = torch.mm(jacobians_cat.T, jacobians_cat) + l2_reg_coef * torch.eye( jacobians_cat.shape[1], device=ntk.device, dtype=torch.float) # steady_state_inv_cov = torch.mm(H, torch.inverse(sgd_cov)) with utils.Timing(description="Solving the Lyapunov equation"): steady_state_cov = solve_continuous_lyapunov( a=utils.to_numpy(H), q=utils.to_numpy(sgd_cov)) steady_state_cov = torch.tensor(steady_state_cov, dtype=torch.float, device=ntk.device) # add small amount of isotropic Gaussian noise to make steady_state_cov invertible steady_state_cov += 1e-10 * torch.eye(steady_state_cov.shape[0], device=steady_state_cov.device, dtype=torch.float) steady_state_inv_cov = torch.inverse(steady_state_cov) change_vectors = [] change_quantities = [] n_outputs = init_preds.shape[-1] for sample_idx in tqdm(range(n)): example_indices = [i for i in range(n) if i != sample_idx] example_output_indices = [] for i in example_indices: example_output_indices.extend( range(i * n_outputs, (i + 1) * n_outputs)) new_ntk = ntk.clone()[example_output_indices] new_ntk = new_ntk[:, example_output_indices] new_ntk_inv = misc.update_ntk_inv(ntk=ntk, ntk_inv=ntk_inv, keep_indices=example_output_indices) new_init_preds = init_preds[example_indices] new_Y = Y[example_indices] if not large_model_regime: new_jacobians = dict() for k, v in jacobians.items(): new_jacobians[k] = v[example_output_indices] else: new_jacobians = None new_dataset = Subset(dataset, example_indices) new_weights = get_weights_at_time_t( t=t, eta=eta * n / (n - 1), init_params=init_params, jacobians=new_jacobians, ntk=new_ntk, ntk_inv=new_ntk_inv, init_preds=new_init_preds, Y=new_Y, continuous=continuous, large_model_regime=large_model_regime, model=model, dataset=new_dataset, **kwargs) total_change = 0.0 param_changes = dict() for k in old_weights.keys(): param_changes[k] = (new_weights[k] - old_weights[k]).cpu() # to save GPU memory if return_change_vectors: change_vectors.append(param_changes) if without_sgd: for k in old_weights.keys(): total_change += torch.sum(param_changes[k]**2) else: param_changes = [v.flatten() for k, v in param_changes.items()] param_changes = torch.cat(param_changes, dim=0) total_change = torch.mm( param_changes.view((1, -1)), torch.mm(steady_state_inv_cov.cpu(), param_changes.view(-1, 1))) change_quantities.append(total_change) return change_vectors, change_quantities
def measure_group_stability(model, measure, prefix='n_groups_', **kwargs): Nf = model.results['n'][-1].copy() # alive=(Nf>10**-5 *np.mean(Nf) ) alive = (Nf > measure['n_death'] * 1.1) if not np.sum(alive): return if not prefix + 'members' in measure: measure_groups(model, measure, prefix=prefix, **kwargs) r = model.data['growth'].matrix.copy() Aij = model.find_data(role='interactions').matrix.copy() if 'selfint' in model.data: D = model.data['selfint'].matrix.copy() else: D = np.ones(r.shape) if np.max(D) == 0: D[:] = 1 Aij = (Aij.T / D).T r /= D mwhere = deepcopy(measure[prefix + 'members']) S = np.array([len(i) for i in mwhere]) Smean = np.mean(S) conn, mu, sigma, sigrow, gamma, Amean, Astd, Asym = group_interactions( Smean, Aij, mwhere, return_all=1) from scipy.linalg import inv, solve_continuous_lyapunov Alive = Aij[np.ix_(alive, alive)] Slive = np.sum(alive) Nlive = Nf[alive] Dlive = D[alive] B = (Alive - np.eye(Slive)) DN = np.diag(Nlive) diag = np.diag(Nlive * Dlive) if 0 and not model.find_data(role='threshold'): J = np.dot(diag, B) else: #FUNCTIONAL RESPONSE # t=model.find_data(role='threshold') # q=model.find_data(role='exponent') # if not len(q): # q=1 ref = model.get_dx(0, model.get_labeled_result(idx=-1))['n'].matrix refx = model.get_labeled_result(idx=-1) dN = np.diag(0.01 * Nf) def adding(d, v): d2 = deepcopy(d) d2['n'] = d['n'] + v return d2 J = np.array([ (model.get_dx(0, adding(refx, dN[i]))['n'].matrix - ref) / (np.ones(len(Nf)) * dN[i, i]) for i in range(len(Nf)) ]).T J[np.isnan(J)] = 0 J = J[np.ix_(alive, alive)] # Jrel=np.dot(np.dot(np.diag(1./Nlive),B),np.diag(Nlive)) # Jrel = np.dot(J,np.diag(1./Nlive)) Jrel = np.dot(np.dot(np.diag(1. / Nlive**2 / Dlive), J), np.diag(Nlive)) # code_debugger() invJ = inv(J) Press = -invJ #*Nlive PressRel = -inv(Jrel) # code_debugger() measure[prefix + 'Pressrel'] = PressRel measure[prefix + 'Press'] = Press walive = list(np.where(alive)[0]) livemwhere = [[walive.index(i) for i in m if i in walive] for m in mwhere] Pgamma, Pmean, Pstd, Psym = group_interactions(Smean, Press, livemwhere, return_all=1, remove_diag=1)[-4:] measure[prefix + 'Pressmean'] = Pmean measure[prefix + 'Pressstd'] = Pstd measure[prefix + 'Presssym'] = Pgamma Vgamma, Vmean, Vstd, Vsym = group_interactions(Smean, PressRel, livemwhere, return_all=1, remove_diag=1)[-4:] measure[prefix + 'Vmean'] = Vmean measure[prefix + 'Vstd'] = Vstd measure[prefix + 'Vsym'] = Vgamma Vmean_self = Vmean.copy() # np.fill_diagonal(Vmean_self,0) Vmean_self += np.diag([ getmean(np.diag(PressRel)[m]) / len(m) if len(m) else 0 for m in livemwhere ]) #V adjusted so that diagonal reflects self-control measure[prefix + 'Vmean_self'] = Vmean_self def make_vec(i, S): vec = np.zeros(S) vec[i] = 1 return vec Var = np.array([ solve_continuous_lyapunov(J, -diag * make_vec(m, Slive)) for m in livemwhere ]) for iv, V in enumerate(Var): measure[prefix + 'Var_{}'.format(iv)] = np.array( [[np.sum(V[np.ix_(m1, m2)]) for m2 in livemwhere] for m1 in livemwhere]) Var_rel = np.array([ solve_continuous_lyapunov(Jrel, -diag * make_vec(m, Slive)) for m in livemwhere ]) for iv, V in enumerate(Var_rel): measure[prefix + 'Varrel_{}'.format(iv)] = np.array( [[np.sum(V[np.ix_(m1, m2)]) for m2 in livemwhere] for m1 in livemwhere]) conn, mu, sigma, sigrow, gamma, Amean, Astd, Asym = group_interactions( Smean, Aij, mwhere, return_all=1) def repl(A): slive = [len(i) for i in livemwhere] return np.concatenate([np.ones(si) * a for a, si in zip(A, slive)]) feedback = 1 - np.array([ np.dot(repl(Amean[i]), np.dot(Press, repl(Amean[:, i]))) for i in range(len(livemwhere)) ])
control, in_vecs, c_matrix = state_controllability(A, B) # Question: Why does A.transpose give the same eigenvectors as the book # and not plain A?? # Answer: The eig function in the numpy library only calculates # the right hand eigenvectors. The Scipy library provides a eig function # in which you can specify whether you want left or right handed eigenvectors. # To determine controllability you need to calculate the input pole vectors # which is dependant on the left eigenvectors. # Calculate eigen vectors and pole vectors val, vec = LA.eig(A, None, 1, 0, 0, 0) n = lin.matrix_rank(c_matrix) P = LA.solve_continuous_lyapunov(A, -B * B.T) # Display results G_tf = G.to_tf() print('\nThe transfer function realization is:') print('G(s) = ') print(np.poly1d(G_tf.num[0], variable='s')) print("----------------") print(np.poly1d(G_tf.den, variable='s')) print('\n1) Eigenvalues are: p1 = ', val[0], 'and p2 = ', val[1]) print(' with eigenvectors: q1 = ', vec[:, 0], 'and q2 = ', vec[:, 1]) print(' Input pole vectors are: up1 = ', in_vecs[0], 'and up2 = ', in_vecs[1]) print('\n2) The controllability matrix has rank', n, 'and is given as:')
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms delT = 1e-3 * self.timestep # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain A = np.zeros((16, 16)) B = np.zeros((16, 4)) C = np.zeros((4, 16)) D = np.zeros((4, 4)) for i in range(0, 6): A[i][i + 6] = 1 A[6][4] = self.g A[7][3] = -self.g B[8][0] = 1 / self.m B[9][1] = 1 / self.Ix B[10][2] = 1 / self.Iy B[11][3] = 1 / self.Iz Bc = np.zeros((12, 4)) Bc = np.vstack((Bc, -np.eye(4))) Bcon = np.concatenate((B, Bc), axis=-1) Dcon = np.zeros((4, 8)) for i in range(0, 4): C[i][i] = 1 # for i in range(12,16): for i in range(0, 4): A[i + 12][i] = 1 CT = signal.StateSpace(A, Bcon, C, Dcon) DT = CT.to_discrete(delT) # DT = signal.StateSpace(A, B, C, D, dt=delT) A_d = DT.A B_d = DT.B[:, :4] Bc_d = DT.B[:, 4:] C_d = DT.C D_d = DT.D[:, :4] # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2)*22 Q = np.diag(1 / max_states**2) # R = np.diag(1/max_inputs**2)*80 R = np.diag(1 / max_inputs**2) * 0.5 # Q = np.eye(16)*0.5 # R = np.eye(4)*100 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -0.8 * K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # print("Kbl_CT size\n") # print(np.shape(Kbl_CT)) # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # # self.Gamma = 3e-3 * np.eye(16) # Q_lyap = np.copy(Q) # Q_lyap[0:3,0:3] *= 30 # Q_lyap[6:9,6:9] *= 150 # Q_lyap[14,14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance self.Gamma = 3e-3 * np.eye(16) Q_lyap = np.copy(Q) Q_lyap[0:3, 0:3] *= 30 Q_lyap[6:9, 6:9] *= 150 Q_lyap[14, 14] *= 2e-3 # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT print(np.shape(A_m)) self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)
def check_continuous_case(self, a, q): x = solve_continuous_lyapunov(a, q) assert_array_almost_equal( np.dot(a, x) + np.dot(x, a.conj().transpose()), q)
def solve_lyap(A, E, B, trans=False, options=None): """Find a factor of the solution of a Lyapunov equation. Returns factor :math:`Z` such that :math:`Z Z^T` is approximately the solution :math:`X` of a Lyapunov equation (if E is `None`). .. math:: A X + X A^T + B B^T = 0 or generalized Lyapunov equation .. math:: A X E^T + E X A^T + B B^T = 0. If trans is `True`, then it solves (if E is `None`) .. math:: A^T X + X A + B^T B = 0 or .. math:: A^T X E + E^T X A + B^T B = 0. This uses the `scipy.linalg.spla.solve_continuous_lyapunov` method. It is only applicable to the standard Lyapunov equation (E = I). Furthermore, it can only solve medium-sized dense problems and assumes access to the matrix data of all operators. Parameters ---------- A The |Operator| A. E The |Operator| E or `None`. B The |Operator| B. trans If the dual equation needs to be solved. options The |solver_options| to use (see :func:`lyap_solver_options`). Returns ------- Z Low-rank factor of the Lyapunov equation solution, |VectorArray| from `A.source`. """ _solve_lyap_check_args(A, E, B, trans) options = _parse_options(options, lyap_solver_options(), 'scipy', None, False) assert options['type'] == 'scipy' if E is not None: raise NotImplementedError import scipy.linalg as spla A_mat = to_matrix(A, format='dense') B_mat = to_matrix(B, format='dense') if not trans: X = spla.solve_continuous_lyapunov(A_mat, -B_mat.dot(B_mat.T)) else: X = spla.solve_continuous_lyapunov(A_mat.T, -B_mat.T.dot(B_mat)) Z = chol(X, copy=False) Z = A.source.from_numpy(np.array(Z).T) return Z
control, in_vecs, c_matrix = state_controllability(A, B) # Question: Why does A.transpose give the same eigenvectors as the book # and not plain A?? # Answer: The eig function in the numpy library only calculates # the right hand eigenvectors. The Scipy library provides a eig function # in which you can specify whether you want left or right handed eigenvectors. # To determine controllability you need to calculate the input pole vectors # which is dependant on the left eigenvectors. # Calculate eigen vectors and pole vectors val, vec = LA.eig(A, None, 1, 0, 0, 0) n = lin.matrix_rank(c_matrix) P = LA.solve_continuous_lyapunov(A, -B * B.T) # Display results G_tf = G.to_tf() print('\nThe transfer function realization is:') print('G(s) = ') print(np.poly1d(G_tf.num[0], variable='s')) print("----------------") print(np.poly1d(G_tf.den, variable='s')) print('\n1) Eigenvalues are: p1 = ', val[0], 'and p2 = ', val[1]) print(' with eigenvectors: q1 = ', vec[:, 0], 'and q2 = ', vec[:, 1]) print(' Input pole vectors are: up1 = ', in_vecs[0], 'and up2 = ', in_vecs[1]) print('\n2) The controlabillity matrix has rank', n, 'and is given as:')
m = 1 g = 9.8 L0 = 1 delta = 0.05 Lmax = 1.5 Lmin = 0.5 p = PendulumParams(m, g, L0, delta, Lmax, Lmin) f_ode = lambda x: RHS(x, p) Dfx = nd.Jacobian(f_ode) E = np.array([0, 0]) A = Dfx(E) I = np.identity(2) P = solve_continuous_lyapunov(A, -10*I) muA = weighted_mu(A, P) d = compute_d(A, P) pointsx = np.empty(shape=[1, 0]) pointsy = np.empty(shape=[1, 0]) pointsW = np.empty(shape=[1, 0]) pointsWdot = np.empty(shape=[1, 0]) x1 = Symbol('x1') x2 = Symbol('x2') t = Symbol('t') x = [x1, x2]
def optimize(FC0_obj, FC1_obj, tau_x, mask_EC, mask_Sigma): N = FC0_obj.shape[0] # optimzation rates (to avoid explosion of activity, Sigma is tuned quicker) epsilon_EC = 0.0005 epsilon_Sigma = 0.05 # minimal value for tuned EC elements min_val_EC = 0. # maximal value for tuned EC elements max_val_EC = 1. # minimal value for tuned Sigma elements min_val_Sigma = 0.01 # initial EC EC = np.zeros([N, N]) # initial connectivity Sigma = np.eye(N) # initial noise # record best fit (matrix distance between model and empirical FC) best_dist = 1e10 # scaling coefficients for FC0 and FC1 (to compensate that matrix FC1 has elements of smaller magnitude than FC0) a0 = np.linalg.norm(FC1_obj) / (np.linalg.norm(FC0_obj) + np.linalg.norm(FC1_obj)) a1 = 1. - a0 stop_opt = False i_opt = 0 while not stop_opt: # calculate Jacobian of dynamical system J = -np.eye(N) / tau_x + EC # calculate FC0 and FC1 for model FC0 = spl.solve_continuous_lyapunov( J, -Sigma) # change for solve_lyapunov for python 2 FC1 = np.dot(FC0, spl.expm(J.T)) # matrices of model error Delta_FC0 = FC0_obj - FC0 Delta_FC1 = FC1_obj - FC1 # calculate error between model and empirical data for FC0 and FC_tau (matrix distance) dist_FC_tmp = 0.5 * ( np.linalg.norm(Delta_FC0) / np.linalg.norm(FC0_obj) + np.linalg.norm(Delta_FC1) / np.linalg.norm(FC1_obj)) # calculate Pearson correlation between model and empirical data for FC0 and FC_tau Pearson_FC_tmp = 0.5 * ( stt.pearsonr(FC0.reshape(-1), FC0_obj.reshape(-1))[0] + stt.pearsonr(FC1.reshape(-1), FC1_obj.reshape(-1))[0]) # record best model parameters if dist_FC_tmp < best_dist: best_dist = dist_FC_tmp best_Pearson = Pearson_FC_tmp i_best = i_opt J_mod_best = np.array(J) Sigma_mod_best = np.array(Sigma) else: stop_opt = i_opt > 50 # Jacobian update Delta_J = np.dot( np.linalg.pinv(FC0), a0 * Delta_FC0 + np.dot(a1 * Delta_FC1, spl.expm(-J.T))).T # update EC (recurrent connectivity) EC[mask_EC] += epsilon_EC * Delta_J[mask_EC] EC[mask_EC] = np.clip(EC[mask_EC], min_val_EC, max_val_EC) # update Sigma (input variances) Delta_Sigma = -np.dot(J, Delta_FC0) - np.dot(Delta_FC0, J.T) Sigma[mask_Sigma] += epsilon_Sigma * Delta_Sigma[mask_Sigma] Sigma[mask_Sigma] = np.maximum(Sigma[mask_Sigma], min_val_Sigma) # check for stop if not stop_opt: i_opt += 1 else: print('stop at step', i_best, 'with best FC dist:', best_dist, '; best FC Pearson:', best_Pearson) return (J_mod_best, Sigma_mod_best)