def lse(self, L): """ Performs Least Squares Estimation with input data provided and returns the least squares estimate """ # Unpack the required dynamics data Q = self.Q R = self.R n = self.n m = self.m ell = self.ell kronA = self.kronA kronB = self.kronB # Perform least-squares estimation Q_est = np.copy(Q) R_est = np.copy(R) if self.unknown_params.Q and self.unknown_params.R: S = np.hstack([kronA, kronB]) vecTheta = mdot(la.pinv(S), vec(L)) vecQ_est = vecTheta[0:ell**2] vecR_est = vecTheta[ell**2:] Q_est = np.reshape(vecQ_est, [n, n]) R_est = np.reshape(vecR_est, [m, m]) elif not self.unknown_params.Q and self.unknown_params.R: S = np.copy(kronB) vecCW = mdot(kronA, vec(Q)) vecR_est = mdot(la.pinv(S), vec(L) - vecCW) R_est = np.reshape(vecR_est, [m, m]) elif self.unknown_params.Q and not self.unknown_params.R: S = np.copy(kronA) vecCV = mdot(kronB, vec(R)) vecQ_est = mdot(la.pinv(S), vec(L) - vecCV) Q_est = np.reshape(vecQ_est, [n, n]) return Q_est, R_est
def dare_obj(obj): if isinstance(obj,LQRSys) and not isinstance(obj,LQRSysMult): Pare = dare(obj.A,obj.B,obj.Q,obj.R) Kare = -la.solve((obj.R+mdot(obj.B.T,Pare,obj.B)), mdot(obj.B.T,Pare,obj.A)) return Pare,Kare elif isinstance(obj,LQRSys) and isinstance(obj,LQRSysMult): return dare_mult(obj.A,obj.B,obj.a,obj.Aa,obj.b,obj.Bb,obj.Q,obj.R)
def func2(y): eta = theta*y LHS = Q + mdot(K.T, R, K) for i in range(p): LHS += a[i]*mdot(Aa[i].T, P, Aa[i]) RHS = np.zeros_like(LHS) for i in range(p): RHS += eta[i]*positive_definite_part(mdot(Aa[i].T, P, ABK) + mdot(ABK.T, P, Aa[i])) for j in range(p): RHS += eta[i]*eta[j]*positive_definite_part(mdot(Aa[i].T, P, Aa[j]) + mdot(Aa[j].T, P, Aa[i])) if is_pos_def(LHS - RHS): return True else: return None
def dlyap_obj(obj,matrixtype='P',algo='iterative',show_warn=False,check_pd=False,P00=None,S00=None): # obj is a LQRSys or LQRSysMult instance if isinstance(obj,LQRSys) and not isinstance(obj,LQRSysMult): if matrixtype=='P': AA = obj.AK.T QQ = obj.Q + sympart(mdot(obj.K.T,obj.R,obj.K)) P = dlyap(AA,QQ) if check_pd: if not is_pos_def(P): P = np.full_like(P,np.inf) elif matrixtype=='S': AA = obj.AK QQ = obj.S0 S = dlyap(AA,QQ) if check_pd: if not is_pos_def(S): S = np.full_like(S,np.inf) elif matrixtype=='PS': P = dlyap_obj(obj,'P',algo,show_warn,check_pd,P00,S00) S = dlyap_obj(obj,'S',algo,show_warn,check_pd,P00,S00) if matrixtype=='P': return P elif matrixtype=='S': return S elif matrixtype=='PS': return P,S elif isinstance(obj,LQRSys) and isinstance(obj,LQRSysMult): return dlyap_mult(obj.A,obj.B,obj.K,obj.a,obj.Aa,obj.b, obj.Bb,obj.Q,obj.R,obj.S0,matrixtype=matrixtype, algo=algo,show_warn=show_warn,check_pd=check_pd, P00=P00,S00=S00)
def algo1(A, B, Aa, Q, R, theta): def calc_control(z): a = theta*z return gare(A, B, a, Aa, Q, R) def func(y): return calc_control(y)[0] z = bisection(y_lwr_0=0, y_upr_0=1, objective=func) a = theta*z P, K = calc_control(z) ABK = A + mdot(B, K) def func2(y): eta = theta*y LHS = Q + mdot(K.T, R, K) for i in range(p): LHS += a[i]*mdot(Aa[i].T, P, Aa[i]) RHS = np.zeros_like(LHS) for i in range(p): RHS += eta[i]*positive_definite_part(mdot(Aa[i].T, P, ABK) + mdot(ABK.T, P, Aa[i])) for j in range(p): RHS += eta[i]*eta[j]*positive_definite_part(mdot(Aa[i].T, P, Aa[j]) + mdot(Aa[j].T, P, Aa[i])) if is_pos_def(LHS - RHS): return True else: return None y = bisection(y_lwr_0=0, y_upr_0=1, objective=func2) eta = theta*y return K, eta
def lqr_gradient(obj): """Calculate the cost (policy) gradient of an LQR system""" # obj is a LQRSys or LQRSysMult instance if obj.P is None: raise Exception('Attempted to compute gradient of system with undefined cost P matrix!') if isinstance(obj, LQRSys) and not isinstance(obj, LQRSysMult): RK = obj.R + mdot(obj.B.T,obj.P,obj.B) elif isinstance(obj, LQRSys) and isinstance(obj, LQRSysMult): # Uncertain part BPBunc = np.zeros([obj.m,obj.m]) for j in range(obj.q): BPBunc += obj.b[j]*mdot(obj.Bb[:,:,j].T, obj.P, obj.Bb[:,:,j]) RK = obj.R + mdot(obj.B.T, obj.P, obj.B) + BPBunc EK = np.dot(RK,obj.K) + mdot(obj.B.T,obj.P,obj.A) # Compute gradient grad = 2*np.dot(EK,obj.S) return grad, RK, EK
def check_mss_obj(obj): """Check whether or not system is closed-loop mean-square stable""" # Options max_iters = 1000 epsilon = 1e-6 # Initialize iterc = 0 stop_early = False converged = False stop = False x0 = np.ones([obj.n, 1]) S00 = np.dot(x0, x0.T) S00n = la.norm(S00, 'fro') S = S00 # norm_hist = np.zeros(max_iters+1) while not stop: # Record previous iterate S_prev = S # norm_hist[iterc] = la.norm(S) # Recurse if isinstance(obj, LQRSys) and not isinstance(obj, LQRSysMult): S = mdot(obj.AK, S, obj.AK.T) elif isinstance(obj, LQRSys) and isinstance(obj, LQRSysMult): # Intermediate expressions ASAunc = np.zeros([obj.n, obj.n]) for i in range(obj.p): ASAunc += obj.a[i] * mdot(obj.Aa[:, :, i], S, obj.Aa[:, :, i].T) BSBunc = np.zeros([obj.n, obj.n]) for j in range(obj.q): BSBunc += obj.b[j] * mdot(obj.Bb[:, :, j], obj.K, S, obj.K.T, obj.Bb[:, :, j].T) S = mdot(obj.AK, S, obj.AK.T) + ASAunc + BSBunc # Check for stopping condition if la.norm(S - S_prev, 'fro') / S00n < epsilon: converged = True if iterc >= max_iters: stop_early = True else: iterc += 1 stop = converged or stop_early # norm_hist = norm_hist[0:iterc] return converged
def gare(A, B, a, Aa, Q, R): # Options max_iters = 1000 epsilon = 1e-6 Pelmax = 1e40 n = A.shape[1] p = len(a) # Initialize P = Q iterc = 0 stop_early = False converged = False stop = False while not stop: # Record previous iterate P_prev = P # Certain part APAcer = mdot(A.T, P, A) BPBcer = mdot(B.T, P, B) # Uncertain part APAunc = np.zeros([n, n]) for i in range(p): APAunc += a[i]*mdot(Aa[i].T, P, Aa[i]) APAsum = APAcer + APAunc BPBsum = np.copy(BPBcer) # Recurse P = Q + APAsum - mdot(A.T, P, B, la.solve(R + BPBsum, B.T), P, A) # Check for stopping condition if la.norm(P - P_prev, 'fro')/la.norm(P, 'fro') < epsilon: converged = True if iterc >= max_iters or np.any(np.abs(P) > Pelmax): stop_early = True else: iterc += 1 stop = converged or stop_early # Compute the gains if stop_early: P, K = None, None else: K = -mdot(la.solve(R + BPBsum, B.T), P, A) return P, K
def model_check(A, B, Q, R, K, op=True, model_type=None): """Check stability of system (A,B) with costs Q, R under the feedback gain K """ # A, B, C = sample_ABCrand(problem_data_true) Qbar = Q + mdot(K.T, R, K) Abar = A + mdot(B, K) # print("Feedback gains test ") if model_type != None: print(model_type) if is_pos_def(dlyap(Abar, Qbar)) == 1: #if pos_def => gain stabilizes if op == True: print( "Policy Iteration on model stabilizes the system - D-Lyap is Positive Definite.\n" ) ret = True else: if op == True: print( "Policy Iteration on model does not stabilize the system - D-Lyap is NOT Positive Definite.\n" ) ret = False return ret
def dare_mult(A, B, a, Aa, b, Bb, Q, R, algo='iterative', show_warn=False): if algo == 'iterative': # Options max_iters = 1000 epsilon = 1e-6 Pelmax = 1e20 n = A.shape[1] m = B.shape[1] p = len(a) q = len(b) # Initialize P = Q iterc = 0 stop_early = False converged = False stop = False while not stop: # Record previous iterate P_prev = P # Certain part APAcer = mdot(A.T, P, A) BPBcer = mdot(B.T, P, B) # Uncertain part APAunc = np.zeros([n, n]) for i in range(p): APAunc += a[i] * mdot(Aa[:, :, i].T, P, Aa[:, :, i]) BPBunc = np.zeros([m, m]) for j in range(q): BPBunc += b[j] * mdot(Bb[:, :, j].T, P, Bb[:, :, j]) APAsum = APAcer + APAunc BPBsum = BPBcer + BPBunc # Recurse P = Q + APAsum - mdot(A.T, P, B, la.solve(R + BPBsum, B.T), P, A) # Check for stopping condition if la.norm(P - P_prev, 'fro') / la.norm(P, 'fro') < epsilon: converged = True if iterc >= max_iters or np.any(np.abs(P) > Pelmax): stop_early = True else: iterc += 1 stop = converged or stop_early # Compute the gains if stop_early: if show_warn: warnings.simplefilter('always', UserWarning) warn( "Recursion failed, ensure system is mean square stabilizable " "or increase maximum iterations") P = None K = None else: K = -mdot(la.solve(R + BPBsum, B.T), P, A) return P, K
def obsvp(self, p): """ Returns a p-step observability matrix Input Parameters: p: Dimension of the required observability matrix """ F = self.F H = self.H n = self.n m = self.m # Build the observability matrix O = np.zeros([m * p, n]) O[0:m] = np.copy(H) for k in range(1, p): O[m * k:m * (k + 1)] = mdot(O[m * (k - 1):m * k], F) return O
def model_based_robust_stabilization_experiment(): seed = 1 npr.seed(seed) problem_data_true, problem_data = gen_double_spring_mass() problem_data_keys = [ 'A', 'B', 'C', 'Ai', 'Bj', 'Ck', 'varAi', 'varBj', 'varCk', 'Q', 'R', 'S' ] A, B, C, Ai, Bj, Ck, varAi, varBj, varCk, Q, R, S = [ problem_data[key] for key in problem_data_keys ] n, m, p = [M.shape[1] for M in [A, B, C]] q, r, s = [M.shape[0] for M in [Ai, Bj, Ck]] # Synthesize controllers using various uncertainty modeling terms # Modify problem data_files # LQR w/ game adversary # Setting varAi, varBj, varCk = 0 => no multiplicative noise on the game problem_data_model_n = copy.deepcopy(problem_data) problem_data_model_n['varAi'] *= 0 problem_data_model_n['varBj'] *= 0 problem_data_model_n['varCk'] *= 0 # LQR w/ multiplicative noise # Setting C = 0 and varCk = 0 => no game adversary problem_data_model_m = copy.deepcopy(problem_data) problem_data_model_m['C'] *= 0 problem_data_model_m['varCk'] *= 0 # Simulation options sim_options = None num_iterations = 50 problem_data_known = True # Policy iteration on LQR w/ game adversary and multiplicative noise K0, L0 = get_initial_gains(problem_data, initial_gain_method='dare') print("LQR w/ game adversary and multiplicative noise") P_pi, K_pi, L_pi, H_pi, P_history_pi, K_history_pi, L_history_pi, c_history_pi, H_history_pi = policy_iteration( problem_data, problem_data_known, K0, L0, sim_options, num_iterations) verify_gare(problem_data, P_pi, algo_str='Policy iteration - Game w/ Multiplicative noise') # Check concavity condition Qvv_pi = -S + mdot(C.T, P_pi, C) + np.sum( [varCk[k] * mdot(Ck[k].T, P_pi, Ck[k]) for k in range(s)], axis=0) if not is_pos_def(-Qvv_pi): raise Exception( 'Problem fails the concavity condition, adjust adversary strength') # Check positive definiteness condition QKL_pi = Q + mdot(K_pi.T, R, K_pi) - mdot(L_pi.T, S, L_pi) if not is_pos_def(QKL_pi): raise Exception( 'Problem fails the positive definiteness condition, adjust adversary strength' ) print(QKL_pi) # Policy Iteration on LQR w/ game adversary K0n, L0n = get_initial_gains(problem_data_model_n, initial_gain_method='dare') print("LQR w/ game adversary") Pn_pi, Kn_pi, Ln_pi, Hn_pi, Pn_history_pi, Kn_history_pi, Ln_history_pi, cn_history_pi, Hn_history_pi = policy_iteration( problem_data_model_n, problem_data_known, K0n, L0n, sim_options, num_iterations) verify_gare(problem_data_model_n, Pn_pi, algo_str='Policy iteration - Game w/o Multiplicative noise') # Policy Iteration on LQR w/ multiplicative noise K0m, L0m = get_initial_gains(problem_data_model_m, initial_gain_method='dare') print("LQR w/ multiplicative noise") Pm_pi, Km_pi, Lm_pi, Hm_pi, Pm_history_pi, Km_history_pi, Lm_history_pi, cm_history_pi, Hm_history_pi = policy_iteration( problem_data_model_m, problem_data_known, K0m, L0m, sim_options, num_iterations) verify_gare(problem_data_model_m, Pm_pi, algo_str='Policy iteration - LQR w/ Multiplicative noise') # LQR on true system A_true, B_true, Q_true, R_true = [ problem_data_true[key] for key in ['A', 'B', 'Q', 'R'] ] n_true, m_true = [M.shape[1] for M in [A_true, B_true]] Pare_true, Kare_true = dare_gain(A_true, B_true, Q_true, R_true) # LQR on nominal system, no explicit robust control design Pce, Kce = dare_gain(A, B, Q, R) # Check if synthesized controllers stabilize the true system K_pi_true = np.hstack([K_pi, np.zeros([m, n])]) Kn_pi_true = np.hstack([Kn_pi, np.zeros([m, n])]) Km_pi_true = np.hstack([Km_pi, np.zeros([m, n])]) Kce_true = np.hstack([Kce, np.zeros([m, n])]) Kol_true = np.zeros_like(Kce_true) control_method_strings = [ 'open-loop ', 'cert equiv ', 'noise ', 'game ', 'noise + game ', 'optimal ' ] K_list = [Kol_true, Kce_true, Km_pi_true, Kn_pi_true, K_pi_true, Kare_true] AK_list = [A_true + np.dot(B_true, K) for K in K_list] QK_list = [Q_true + mdot(K.T, R_true, K) for K in K_list] specrad_list = [specrad(AK) for AK in AK_list] cost_list = [ np.trace(dlyap(AK.T, QK)) if sr < 1 else np.inf for AK, QK, sr in zip(AK_list, QK_list, specrad_list) ] set_numpy_decimal_places(1) output_text_filename = 'results_model_based_robust_stabilization_experiment.txt' output_text_path = os.path.join('..', 'results', output_text_filename) with open(output_text_path, 'w') as f: header_str = 'method | specrad | cost | gains' print(header_str, file=f) for control_method_string, sr, cost, K in zip(control_method_strings, specrad_list, cost_list, K_list): line_str = '%s %.3f %8s %s' % (control_method_string, sr, '%10.0f' % cost, K) print(line_str, file=f)
def rollout(problem_data, K, L, sim_options): """Simulate closed-loop state response""" problem_data_keys = [ 'A', 'B', 'C', 'Ai', 'Bj', 'Ck', 'varAi', 'varBj', 'varCk', 'Q', 'R', 'S' ] A, B, C, Ai, Bj, Ck, varAi, varBj, varCk, Q, R, S = [ problem_data[key] for key in problem_data_keys ] n, m, p = [M.shape[1] for M in [A, B, C]] sim_options_keys = [ 'xstd', 'ustd', 'vstd', 'wstd', 'nt', 'nr', 'group_option' ] xstd, ustd, vstd, wstd, nt, nr, group_option = [ sim_options[key] for key in sim_options_keys ] qfun_estimator = sim_options['qfun_estimator'] if qfun_estimator == 'direct': # First step # Sample initial states, defender control inputs, and attacker control inputs x0, u0, v0 = [ npr.randn(nr, dim) * std for dim, std in zip([n, m, p], [xstd, ustd, vstd]) ] Qval = np.zeros(nr) if group_option == 'single': # Iterate over rollouts for k in range(nr): x0_k, u0_k, v0_k = [var[k] for var in [x0, u0, v0]] # Initialize x = np.copy(x0_k) # Iterate over timesteps for i in range(nt): # Compute controls if i == 0: u, v = np.copy(u0_k), np.copy(v0_k) else: u, v = np.dot(K, x), np.dot(L, x) # Accumulate cost Qval[k] += mdot(x.T, Q, x) + mdot(u.T, R, u) - mdot( v.T, S, v) # Randomly sample state transition matrices using multiplicative noise Arand, Brand, Crand = sample_ABCrand(problem_data) # Additive noise w = npr.randn(n) * wstd # Transition the state using multiplicative and additive noise x = np.dot(Arand, x) + np.dot(Brand, u) + np.dot(Crand, v) + w elif group_option == 'group': # Randomly sample state transition matrices using multiplicative noise Arand_all, Brand_all, Crand_all = sample_ABCrand_multi( problem_data, nt, nr) # Randomly sample additive noise w_all = npr.randn(nt, nr, n) * wstd # Initialize x = np.copy(x0) # Iterate over timesteps for i in range(nt): # Compute controls if i == 0: u = np.copy(u0) v = np.copy(v0) else: u = groupdot(K, x) v = groupdot(L, x) # Accumulate cost Qval += groupquadform(Q, x) + groupquadform( R, u) - groupquadform(S, v) # Look up stochastic dynamics and additive noise Arand, Brand, Crand = Arand_all[i], Brand_all[i], Crand_all[i] w = w_all[i] # Transition the state using multiplicative and additive noise x = groupdot(Arand, x) + groupdot(Brand, u) + groupdot( Crand, v) + w return x0, u0, v0, Qval elif qfun_estimator == 'lsadp' or qfun_estimator == 'lstdq': # Sample initial states, defender control inputs, and attacker control inputs x0 = xstd * npr.randn(nr, n) u_explore_hist = ustd * npr.randn(nr, nt, m) v_explore_hist = vstd * npr.randn(nr, nt, p) x_hist = np.zeros([nr, nt, n]) u_hist = np.zeros([nr, nt, m]) v_hist = np.zeros([nr, nt, p]) c_hist = np.zeros([nr, nt]) if group_option == 'single': # Iterate over rollouts for k in range(nr): # Initialize x = np.copy(x0[k]) # Iterate over timesteps for i in range(nt): # Compute controls u = np.dot(K, x) + u_explore_hist[k, i] v = np.dot(L, x) + v_explore_hist[k, i] # Compute cost c = mdot(x.T, Q, x) + mdot(u.T, R, u) - mdot(v.T, S, v) # Record history x_hist[k, i] = x u_hist[k, i] = u v_hist[k, i] = v c_hist[k, i] = c # Randomly sample state transition matrices using multiplicative noise Arand, Brand, Crand = sample_ABCrand(problem_data) # Additive noise w = npr.randn(n) * wstd # Transition the state using multiplicative and additive noise x = np.dot(Arand, x) + np.dot(Brand, u) + np.dot(Crand, v) + w elif group_option == 'group': # Randomly sample state transition matrices using multiplicative noise Arand_all, Brand_all, Crand_all = sample_ABCrand_multi( problem_data, nt, nr) # Randomly sample additive noise w_all = npr.randn(nt, nr, n) * wstd # Initialize x = np.copy(x0) # Iterate over timesteps for i in range(nt): # Compute controls u = groupdot(K, x) + u_explore_hist[:, i] v = groupdot(L, x) + v_explore_hist[:, i] # Compute cost c = groupquadform(Q, x) + groupquadform(R, u) - groupquadform( S, v) # Record history x_hist[:, i] = x u_hist[:, i] = u v_hist[:, i] = v c_hist[:, i] = c # Look up stochastic dynamics and additive noise Arand, Brand, Crand = Arand_all[i], Brand_all[i], Crand_all[i] w = w_all[i] # Transition the state using multiplicative and additive noise x = groupdot(Arand, x) + groupdot(Brand, u) + groupdot( Crand, v) + w return x_hist, u_hist, v_hist, c_hist
def qfun(problem_data, problem_data_known=None, P=None, K=None, L=None, sim_options=None, output_format=None): """Compute or estimate Q-function matrix""" if problem_data_known is None: problem_data_known = True if output_format is None: output_format = 'list' problem_data_keys = [ 'A', 'B', 'C', 'Ai', 'Bj', 'Ck', 'varAi', 'varBj', 'varCk', 'Q', 'R', 'S' ] A, B, C, Ai, Bj, Ck, varAi, varBj, varCk, Q, R, S = [ problem_data[key] for key in problem_data_keys ] n, m, p = [M.shape[1] for M in [A, B, C]] q, r, s = [M.shape[0] for M in [Ai, Bj, Ck]] if P is None: P = gdlyap(problem_data, K, L) if problem_data_known: APAi = np.sum([varAi[i] * mdot(Ai[i].T, P, Ai[i]) for i in range(q)], axis=0) BPBj = np.sum([varBj[j] * mdot(Bj[j].T, P, Bj[j]) for j in range(r)], axis=0) CPCk = np.sum([varCk[k] * mdot(Ck[k].T, P, Ck[k]) for k in range(s)], axis=0) Qxx = Q + mdot(A.T, P, A) + APAi Quu = R + mdot(B.T, P, B) + BPBj Qvv = -S + mdot(C.T, P, C) + CPCk Qux = mdot(B.T, P, A) Qvx = mdot(C.T, P, A) Qvu = mdot(C.T, P, B) else: nr = sim_options['nr'] nt = sim_options['nt'] qfun_estimator = sim_options['qfun_estimator'] if qfun_estimator == 'direct': # Simulation data_files collection x0, u0, v0, Qval = rollout(problem_data, K, L, sim_options) # Dimensions Qpart_shapes = [[n, n], [m, m], [p, p], [m, n], [p, n], [p, m]] Qvec_part_lengths = [np.prod(shape) for shape in Qpart_shapes] # Least squares estimation xuv_data = np.zeros([nr, np.sum(Qvec_part_lengths)]) for i in range(nr): x = x0[i] u = u0[i] v = v0[i] xuv_data[i] = np.hstack([ kron(x.T, x.T), kron(u.T, u.T), kron(v.T, v.T), 2 * kron(x.T, u.T), 2 * kron(x.T, v.T), 2 * kron(u.T, v.T) ]) # Solve the least squares problem Qvec = la.lstsq(xuv_data, Qval, rcond=None)[0] # Split and reshape the solution vector into the appropriate matrices idxi = [0] Qvec_parts = [] Q_parts = [] for i, part_length in enumerate(Qvec_part_lengths): idxi.append(idxi[i] + part_length) Qvec_parts.append(Qvec[idxi[i]:idxi[i + 1]]) Q_parts.append(np.reshape(Qvec_parts[i], Qpart_shapes[i])) Qxx, Quu, Qvv, Qux, Qvx, Qvu = Q_parts elif qfun_estimator == 'lsadp': # Simulation data_files collection x_hist, u_hist, v_hist, c_hist = rollout(problem_data, K, L, sim_options) # Form the data_files matrices ns = nr * (nt - 1) nz = int(((n + m + p + 1) * (n + m + p)) / 2) mu_hist = np.zeros([nr, nt, nz]) nu_hist = np.zeros([nr, nt, nz]) def phi(x): return svec2(np.outer(x, x)) for i in range(nr): for j in range(nt): z = np.concatenate( [x_hist[i, j], u_hist[i, j], v_hist[i, j]]) w = np.concatenate([ x_hist[i, j], np.dot(K, x_hist[i, j]), np.dot(L, x_hist[i, j]) ]) mu_hist[i, j] = phi(z) nu_hist[i, j] = phi(w) Y = np.zeros(ns) Z = np.zeros([ns, nz]) for i in range(nr): lwr = i * (nt - 1) upr = (i + 1) * (nt - 1) Y[lwr:upr] = c_hist[i, 0:-1] Z[lwr:upr] = mu_hist[i, 0:-1] - nu_hist[i, 1:] # Solve the least squares problem # H_svec = la.lstsq(Z, Y, rcond=None)[0] # H = smat(H_svec) H_svec2 = la.lstsq(Z, Y, rcond=None)[0] H = smat2(H_svec2) Qxx = H[0:n, 0:n] Quu = H[n:n + m, n:n + m] Qvv = H[n + m:, n + m:] Qux = H[n:n + m, 0:n] Qvx = H[n + m:, 0:n] Qvu = H[n + m:, n:n + m] elif qfun_estimator == 'lstdq': # Simulation data_files collection x_hist, u_hist, v_hist, c_hist = rollout(problem_data, K, L, sim_options) # Form the data_files matrices nz = int(((n + m + p + 1) * (n + m + p)) / 2) mu_hist = np.zeros([nr, nt, nz]) nu_hist = np.zeros([nr, nt, nz]) def phi(x): return svec2(np.outer(x, x)) for i in range(nr): for j in range(nt): z = np.concatenate( [x_hist[i, j], u_hist[i, j], v_hist[i, j]]) w = np.concatenate([ x_hist[i, j], np.dot(K, x_hist[i, j]), np.dot(L, x_hist[i, j]) ]) mu_hist[i, j] = phi(z) nu_hist[i, j] = phi(w) Y = np.zeros(nr * nz) Z = np.zeros([nr * nz, nz]) for i in range(nr): lwr = i * nz upr = (i + 1) * nz for j in range(nt - 1): Y[lwr:upr] += mu_hist[i, j] * c_hist[i, j] Z[lwr:upr] += np.outer(mu_hist[i, j], mu_hist[i, j] - nu_hist[i, j + 1]) H_svec2 = la.lstsq(Z, Y, rcond=None)[0] H = smat2(H_svec2) Qxx = H[0:n, 0:n] Quu = H[n:n + m, n:n + m] Qvv = H[n + m:, n + m:] Qux = H[n:n + m, 0:n] Qvx = H[n + m:, 0:n] Qvu = H[n + m:, n:n + m] if output_format == 'list': outputs = Qxx, Quu, Qvv, Qux, Qvx, Qvu elif output_format == 'matrix': outputs = np.block([[Qxx, Qux.T, Qvx.T], [Qux, Quu, Qvu.T], [Qvx, Qvu, Qvv]]) # ABC = np.hstack([A, B, C]) # X = sla.block_diag(Q, R, -S) # Y = mdot(ABC.T, P, ABC) # Z = sla.block_diag(APAi, BPBj, CPCk) # outputs = X + Y + Z return outputs
def get_buffer_size(self): """ Returns the size of the measurement history buffer & corresponding observability matrix """ n = self.n m = self.m ell = self.ell F = self.F H = self.H Mo = 0 p = 0 while not la.matrix_rank( Mo) == n: # valid only if system is observable p += 1 Mo = self.obsvp(p) Mopi = la.pinv(Mo) # Build the stacked H matrix as Mw if p > 1: Mw = np.zeros([m * p, ell * (p - 1)]) Mw[0:m, 0:ell] = np.copy(H) for j in range(1, p - 1): Mw[0:m, ell * j:ell * (j + 1)] = mdot( Mw[0:m, ell * (j - 1):ell * j], F) for i in range(1, p - 1): Mw[m * i:m * (i + 1), ell * i:] = Mw[0:m, 0:ell * (p - i - 1)] # Product matrix: Mopi*Mw MopiMw = mdot(Mopi, Mw) # Form the 'script' A matrix as Anew - Aold Anew = np.hstack([MopiMw, np.eye(ell)]) Aold = np.hstack([np.zeros([ell, ell]), mdot(F, MopiMw)]) A = Anew - Aold # Form the 'script' B matrix as Bnew - Bold Bnew = np.hstack([Mopi, np.zeros([ell, m])]) Bold = np.hstack([np.zeros([ell, m]), mdot(F, Mopi)]) B = Bnew - Bold else: A = np.eye(ell) B = np.hstack([Mopi, -mdot(F, Mopi)]) # Get the A_i and B_i sequences Ai = np.zeros([p, ell, ell]) Bi = np.zeros([p + 1, ell, m]) for i in range(p + 1): if i > 0: Ai[p - i] = A[:, ell * (i - 1):ell * i] Bi[p - i] = B[:, m * i:m * (i + 1)] # Form Kronecker products of the A_i's and B_i's kronA = np.zeros([ell**2, ell**2]) kronB = np.zeros([ell**2, m**2]) for i in range(p + 1): if i < p: kronA += np.kron(Ai[i], Ai[i]) kronB += np.kron(Bi[i], Bi[i]) return p, Mopi, kronA, kronB
def calc_QK(self): self._QK = self.Q + mdot(self.K.T, self.R, self.K) return self._QK
# Define the true and nominal dynamics and LQR costs n, m, A_true, B_true, Q, R, A, B = system_inverted_pendulum() # Define uncertainty directions and magnitudes p = 1 Ai = np.zeros([p, n, n]) Ai[0, 1, 0] = 1 eta_bar = np.ones(p) # Compute robust optimal gains K1, eta1 = algo1(A, B, Ai, Q, R, eta_bar) K2, eta2 = algo2(A, B, Ai, Q, R, eta_bar) # Compute the certainty-equivalent control Pce = dare(A, B, Q, R) Kce = -la.solve((R + mdot(B.T, Pce, B)), mdot(B.T, Pce, A)) def check_random_systems(K, eta, R, bound_type): s = np.zeros(R) if bound_type == "unidirectional": r = np.linspace(0, 1, R) elif bound_type == "bidirectional": r = np.linspace(-1, 1, R) for j in range(R): Arand = np.copy(A) for i in range(p): Arand += eta[i]*r[j]*Ai[i] s[j] = specrad(Arand + B_true@K) return s
def estimate_model(n, m, A, B, SigmaA, SigmaB, nr, ell, x_hist, u_mean_hist, u_covr_hist, display_estimates=False, AB_known=False): muhat_hist = np.zeros([ell + 1, n]) Xhat_hist = np.zeros([ell + 1, n * n]) What_hist = np.zeros([ell + 1, n * m]) # First stage: mean dynamics parameter estimation if AB_known: Ahat = np.copy(A) Bhat = np.copy(B) else: # Form data matrices for least-squares estimation for t in range(ell + 1): muhat_hist[t] = (1 / nr) * np.sum(x_hist[t], axis=0) Xhat_hist[t] = (1 / nr) * vec( np.sum(np.einsum('...i,...j', x_hist[t], x_hist[t]), axis=0)) if t < ell: # What_hist[t] = (1/nr)*vec(np.sum(np.einsum('...i,...j',x_hist[t],u_mean_hist[t]),axis=0)) What_hist[t] = vec(np.outer(muhat_hist[t], u_mean_hist[t])) Y = muhat_hist[1:].T Z = np.vstack([muhat_hist[0:-1].T, u_mean_hist.T]) # Solve least-squares problem # Thetahat = mdot(Y, Z.T, la.pinv(mdot(Z, Z.T))) Thetahat = la.lstsq(Z.T, Y.T, rcond=None)[0].T # Split learned model parameters Ahat = Thetahat[:, 0:n] Bhat = Thetahat[:, n:n + m] AAhat = np.kron(Ahat, Ahat) ABhat = np.kron(Ahat, Bhat) BAhat = np.kron(Bhat, Ahat) BBhat = np.kron(Bhat, Bhat) # Second stage: covariance dynamics parameter estimation # Form data matrices for least-squares estimation C = np.zeros([ell, n * n]).T Uhat_hist = np.zeros([ell, m * m]) for t in range(ell): Uhat_hist[t] = vec(u_covr_hist[t] + np.outer(u_mean_hist[t], u_mean_hist[t])) Cminus = mdot(AAhat, Xhat_hist[t]) + mdot(BAhat, What_hist[t]) + mdot( ABhat, What_hist[t].T) + mdot(BBhat, Uhat_hist[t]) C[:, t] = Xhat_hist[t + 1] - Cminus D = np.vstack([Xhat_hist[0:-1].T, Uhat_hist.T]) # Solve least-squares problem # SigmaThetahat_prime = mdot(C, D.T, la.pinv(mdot(D,D.T))) SigmaThetahat_prime = la.lstsq(D.T, C.T, rcond=None)[0].T # Split learned model parameters SigmaAhat_prime = SigmaThetahat_prime[:, 0:n * n] SigmaBhat_prime = SigmaThetahat_prime[:, n * n:n * (n + m)] # Reshape and project the noise covariance estimates onto the semidefinite cone SigmaAhat = reshaper(SigmaAhat_prime, n, n, n, n) SigmaBhat = reshaper(SigmaBhat_prime, n, m, n, m) SigmaAhat = positive_semidefinite_part(SigmaAhat) SigmaBhat = positive_semidefinite_part(SigmaBhat) if display_estimates: prettyprint(Ahat, "Ahat") prettyprint(A, "A ") prettyprint(Bhat, "Bhat") prettyprint(B, "B ") prettyprint(SigmaAhat, "SigmaAhat") prettyprint(SigmaA, "SigmaA ") prettyprint(SigmaBhat, "SigmaBhat") prettyprint(SigmaB, "SigmaB ") return Ahat, Bhat, SigmaAhat, SigmaBhat
def run(self, T): """ Perform adaptive Kalman filter iterations Input Parameters: T: Integer of maximum filter iterations """ F = self.F H = self.H Q = self.Q R = self.R n = self.n m = self.m p = self.p Mopi = self.Mopi x_mean0 = self.x_mean0 x_covr0 = self.x_covr0 Q_est0 = self.Q_est0 R_est0 = self.R_est0 L0 = self.L0 # Preallocate history data arrays x_hist = np.full((T + 1, n), np.nan) y_hist = np.full((T + 1, m), np.nan) x_pre_hist = np.full((T + 1, n), np.nan) x_post_hist = np.full((T + 1, n), np.nan) P_pre_hist = np.full((T + 1, n, n), np.nan) P_post_hist = np.full((T + 1, n, n), np.nan) K_hist = np.full((T + 1, n, m), np.nan) Q_est_hist = np.full((T + 1, n, n), np.nan) R_est_hist = np.full((T + 1, m, m), np.nan) # Initialize the iterates x_post = x_mean0 P_post = x_covr0 Q_est = Q_est0 R_est = R_est0 L = L0 x = npr.multivariate_normal(x_mean0, x_covr0) x_hist[0] = x x_post_hist[0] = x_post P_post_hist[0] = P_post # Perform dynamic adaptive Kalman filter updates for k in range(T): # Print the iteration number print("k = %9d / %d" % (k + 1, T)) # Generate a new multivariate Gaussian measurement noise v = npr.multivariate_normal(np.zeros(m), R) # Collect and store a new measurement y = mdot(H, x) + v y_hist[k] = y # Noise covariance estimation if k > p - 1: # Collect measurement till 'k-1' time steps Yold = vec(y_hist[np.arange(k - 1, k - p - 1, -1)].T) # Collect measurement till 'k' time steps Ynew = vec(y_hist[np.arange(k, k - p, -1)].T) # Formulate a linear stationary time series Z = mdot(Mopi, Ynew) - mdot(F, Mopi, Yold) # Recursive covariance unbiased estimator L = ((k - 1) / k) * L + (1 / k) * np.outer(Z, Z) # Get the least squares estimate of selected covariances Q_est_new, R_est_new = self.lse(L) # Check positive semidefiniteness of estimated covariances if is_pos_def(Q_est_new): Q_est = np.copy(Q_est_new) if is_pos_def(R_est_new): R_est = np.copy(R_est_new) # Update the covariance estimate history Q_est_hist[k] = Q_est R_est_hist[k] = R_est ## Update state estimates using standard Kalman filter equations # Calculate the a priori state estimate x_pre = mdot(F, x_post) # Calculate the a priori error covariance estimate P_pre = mdot(F, P_post, F.T) + Q_est # Calculate the Kalman gain K = solveb(mdot(P_pre, H.T), mdot(H, P_pre, H.T) + R_est) # Calculate the a posteriori state estimate x_post = x_pre + mdot(K, y - mdot(H, x_pre)) # Calculate the a posteriori error covariance estimate IKH = np.eye(n) - mdot(K, H) P_post = mdot(IKH, P_pre, IKH.T) + mdot(K, R, K.T) # Store the histories x_pre_hist[k + 1] = x_pre x_post_hist[k + 1] = x_post P_pre_hist[k + 1] = P_pre P_post_hist[k + 1] = P_post K_hist[k + 1] = K # True system updates (true state transition and measurement) # Generate process noise w = npr.multivariate_normal(np.zeros(n), Q) # Update and store the state x = mdot(F, x) + w x_hist[k + 1] = x # Tie up loose ends y_hist[-1] = y x_pre_hist[0] = x_post_hist[0] P_pre_hist[0] = P_post_hist[0] K_hist[0] = K_hist[1] Q_est_hist[-1] = Q_est R_est_hist[-1] = R_est # Return data history data_hist = DataHist(T, Q_est_hist, R_est_hist, x_hist, x_pre_hist, x_post_hist, P_pre_hist, P_post_hist) return data_hist
def estimate_model_var_only(n, m, A, B, SigmaA, SigmaB, varAi, varBj, Ai, Bj, nr, ell, x_hist, u_mean_hist, u_covr_hist, display_estimates=False, AB_known=False, detailed_outputs=True): muhat_hist = np.zeros([ell + 1, n]) Xhat_hist = np.zeros([ell + 1, n * n]) What_hist = np.zeros([ell + 1, n * m]) # First stage: mean dynamics parameter estimation if AB_known: Ahat = np.copy(A) Bhat = np.copy(B) else: # Form data matrices for least-squares estimation for t in range(ell + 1): muhat_hist[t] = (1 / nr) * np.sum(x_hist[t], axis=0) Xhat_hist[t] = (1 / nr) * vec( np.sum(np.einsum('...i,...j', x_hist[t], x_hist[t]), axis=0)) if t < ell: # What_hist[t] = (1/nr)*vec(np.sum(np.einsum('...i,...j',x_hist[t],u_mean_hist[t]),axis=0)) What_hist[t] = vec(np.outer(muhat_hist[t], u_mean_hist[t])) Y = muhat_hist[1:].T Z = np.vstack([muhat_hist[0:-1].T, u_mean_hist.T]) # Solve least-squares problem # Thetahat = mdot(Y, Z.T, la.pinv(mdot(Z, Z.T))) Thetahat = la.lstsq(Z.T, Y.T, rcond=None)[0].T # Split learned model parameters Ahat = Thetahat[:, 0:n] Bhat = Thetahat[:, n:n + m] AAhat = np.kron(Ahat, Ahat) ABhat = np.kron(Ahat, Bhat) BAhat = np.kron(Bhat, Ahat) BBhat = np.kron(Bhat, Bhat) # Second stage: covariance dynamics parameter estimation # Form data matrices for least-squares estimation C = np.zeros([ell, n * n]).T Uhat_hist = np.zeros([ell, m * m]) for t in range(ell): Uhat_hist[t] = vec(u_covr_hist[t] + np.outer(u_mean_hist[t], u_mean_hist[t])) Cminus = mdot(AAhat, Xhat_hist[t]) + mdot(BAhat, What_hist[t]) + mdot( ABhat, What_hist[t].T) + mdot(BBhat, Uhat_hist[t]) C[:, t] = Xhat_hist[t + 1] - Cminus C = vec(C) X1 = Xhat_hist[0:-1].T U1 = Uhat_hist.T D1 = np.vstack([ vec(np.dot(np.kron(Ai[i], Ai[i]), X1)) for i in range(np.size(varAi)) ]) D2 = np.vstack([ vec(np.dot(np.kron(Bj[j], Bj[j]), U1)) for j in range(np.size(varBj)) ]) D = np.vstack([D1, D2]) # Solve least-squares problem # var_hat = mdot(C, D.T, la.pinv(mdot(D,D.T))) # var_hat = mdot(la.pinv(mdot(D, D.T)), D, C) var_hat = la.lstsq(D.T, C, rcond=None)[0] varAi_hat = np.maximum(var_hat[0:np.size(varAi)], 0) varBj_hat = np.maximum(var_hat[np.size(varAi):], 0) SigmaAhat = np.sum([ varAi_hat[i] * np.outer(vec(Ai[i]), vec(Ai[i])) for i in range(np.size(varAi)) ], axis=0) SigmaBhat = np.sum([ varBj_hat[j] * np.outer(vec(Bj[j]), vec(Bj[j])) for j in range(np.size(varBj)) ], axis=0) if display_estimates: prettyprint(Ahat, "Ahat") prettyprint(A, "A ") prettyprint(Bhat, "Bhat") prettyprint(B, "B ") prettyprint(SigmaAhat, "SigmaAhat") prettyprint(SigmaA, "SigmaA ") prettyprint(SigmaBhat, "SigmaBhat") prettyprint(SigmaB, "SigmaB ") if detailed_outputs: outputs = Ahat, Bhat, SigmaAhat, SigmaBhat, varAi_hat, varBj_hat else: outputs = Ahat, Bhat, SigmaAhat, SigmaBhat return outputs
def dlyap_mult(A, B, K, a, Aa, b, Bb, Q, R, S0, matrixtype='P', algo='iterative', show_warn=False, check_pd=False, P00=None, S00=None): n = A.shape[1] n2 = n * n p = len(a) q = len(b) AK = A + np.dot(B, K) stable = True stable2 = True if algo == 'linsolve': if matrixtype == 'P': # Intermediate terms Aunc_P = np.zeros([n2, n2]) for i in range(p): Aunc_P = Aunc_P + a[i] * kron(Aa[:, :, i].T) BKunc_P = np.zeros([n2, n2]) for j in range(q): BKunc_P = BKunc_P + b[j] * kron(np.dot(K.T, Bb[:, :, j].T)) # Compute matrix and vector for the linear equation solver Alin_P = np.eye(n2) - kron(AK.T) - Aunc_P - BKunc_P blin_P = vec(Q) + np.dot(kron(K.T), vec(R)) # Solve linear equations xlin_P = la.solve(Alin_P, blin_P) # Reshape P = np.reshape(xlin_P, [n, n]) if check_pd: stable = is_pos_def(P) elif matrixtype == 'S': # Intermediate terms Aunc_S = np.zeros([n2, n2]) for i in range(p): Aunc_S = Aunc_S + a[i] * kron(Aa[:, :, i]) BKunc_S = np.zeros([n2, n2]) for j in range(q): BKunc_S = BKunc_S + b[j] * kron(np.dot(Bb[:, :, j], K)) # Compute matrix and vector for the linear equation solver Alin_S = np.eye(n2) - kron(AK) - Aunc_S - BKunc_S blin_S = vec(S0) # Solve linear equations xlin_S = la.solve(Alin_S, blin_S) # Reshape S = np.reshape(xlin_S, [n, n]) if check_pd: stable = is_pos_def(S) elif matrixtype == 'PS': P = dlyap_mult(A, B, K, a, Aa, b, Bb, Q, R, S0, matrixtype='P', algo='linsolve') S = dlyap_mult(A, B, K, a, Aa, b, Bb, Q, R, S0, matrixtype='S', algo='linsolve') elif algo == 'iterative': # Implicit iterative solution to generalized discrete Lyapunov equation # Inspired by https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7553367 # In turn inspired by https://pdf.sciencedirectassets.com/271503/1-s2.0-S0898122100X0020X/1-s2.0-089812219500119J/main.pdf?x-amz-security-token=AgoJb3JpZ2luX2VjECgaCXVzLWVhc3QtMSJIMEYCIQD#2F00Re8b3wnBnFpZQrjkOeXrNI4bYZ1J6#2F9BcJptZYAAIhAOQjTsZX573uFFEr7QveHx4NaZYWxlZfRN6hr5h1GJWWKuMDCOD#2F#2F#2F#2F#2F#2F#2F#2F#2F#2FwEQAhoMMDU5MDAzNTQ2ODY1IgxqkGe6i8wGmEj6YAwqtwNDKbotYDExP2D6PO8MrlIKYmHCtJhTu1CXLv0N5NKsYT90H2rJTNU0MvqsUsnXtbn6C9t9ed31XTf#2BHc7KrGmpOils7zgrjV1QG4LP0Fu2OcT4#2F#2FOGLWNvVjWY9gOLEHSeG5LhvBbxJiZVrI#2Bm1QAIVz5dxH5DVB27A2e9OmRrswrpPWuxQV#2BUvLkz2dVM4qSkvaDA#2F3KEJk9s0XE74mjO4ZHX7d9Q2aYwxsvFbII6Hms#2FZmB6125tBTwzd0K5xDit5kaoiYadOetp3M#2FvCdaiO0QeQwkV4#2FUaprOIIQGwJaMJuMNe7xInQxF#2B#2FmER81JhWEpBHBmz#2F5p0d2tU7F2oTDc2OR#2BV5dTKab47zgUw648fDT7ays0TQzqTMGnGcX9wIQpxSCam2E8Bhg6tsEs0#2FudddgnsiId368q70xai6ucMfabMSCqnv7O0OZqPVwY5b7qk4mxKIehpIzV6rrtXSAGrH95WGlgGz#2Fhmg9Qq6AUtb8NSqyYw0uZ00E#2FPZmNTnI3nwxjOA5qhyEbw3uXogRwYrv0dLkd50s7oO3mlYFeJDBurhx11t9p94dFqQq7sDY70m#2F4xMNCcmuUFOrMBY1JZuqtQ7QFBVbgzV#2B4xSHV6#2FyD#2F4ezttczZY3eSASJpdC4rjYHXcliiE7KOBHivchFZMIYeF3J4Nvn6UykX5sNfRANC2BDPrgoCQUp95IE5kgYGB8iEISlp40ahVXK62GhEASJxMjJTI9cJ2M#2Ff#2BJkwmqAGjTsBwjxkgiLlHc63rBAEJ2e7xoTwDDql3FSSYcvKzwioLfet#2FvXWvjPzz44tB3#2BTvYamM0uq47XPlUFcTrw#3D&AWSAccessKeyId=ASIAQ3PHCVTYWXNG3EKG&Expires=1554423148&Signature=Ysi80usGGEjPCvw#2BENTSD90NgVs#3D&hash=e5cf30dad62b0b57d7b7f5ba524cccacdbb36d2f747746e7fbebb7717b415820&host=68042c943591013ac2b2430a89b270f6af2c76d8dfd086a07176afe7c76c2c61&pii=089812219500119J&tid=spdf-a9dae0e9-65fd-4f31-bf3f-e0952eb4176c&sid=5c8c88eb95ed9742632ae57532a4a6e1c6b1gxrqa&type=client # Faster for large systems i.e. >50 states # Options max_iters = 1000 epsilon_P = 1e-5 epsilon_S = 1e-5 # Initialize if matrixtype == 'P' or matrixtype == 'PS': if P00 is None: P = np.copy(Q) else: P = P00 if matrixtype == 'S' or matrixtype == 'PS': if S00 is None: S = np.copy(S0) else: S = S00 iterc = 0 converged = False stop = False while not stop: if matrixtype == 'P' or matrixtype == 'PS': P_prev = P APAunc = np.zeros([n, n]) for i in range(p): APAunc += a[i] * mdot(Aa[:, :, i].T, P, Aa[:, :, i]) BPBunc = np.zeros([n, n]) for j in range(q): BPBunc += b[j] * mdot(K.T, Bb[:, :, j].T, P, Bb[:, :, j], K) AAP = AK.T QQP = sympart(Q + mdot(K.T, R, K) + APAunc + BPBunc) P = dlyap(AAP, QQP) if np.any(np.isnan(P)) or np.any( np.isinf(P)) or not is_pos_def(P): stable = False try: converged_P = la.norm(P - P_prev, 2) / la.norm( P_prev, 2) < epsilon_P stable2 = True except: # print(P) # print(P_prev) # print(P-P_prev) # print(la.norm()) stable2 = False # print('') if matrixtype == 'S' or matrixtype == 'PS': S_prev = S ASAunc = np.zeros([n, n]) for i in range(p): ASAunc += a[i] * mdot(Aa[:, :, i], S, Aa[:, :, i].T) BSBunc = np.zeros([n, n]) for j in range(q): BSBunc = b[j] * mdot(Bb[:, :, j], K, S, K.T, Bb[:, :, j].T) AAS = AK QQS = sympart(S0 + ASAunc + BSBunc) S = dlyap(AAS, QQS) if np.any(np.isnan(S)) or not is_pos_def(S): stable = False converged_S = la.norm(S - S_prev, 2) / la.norm(S, 2) < epsilon_S # Check for stopping condition if matrixtype == 'P': converged = converged_P elif matrixtype == 'S': converged = converged_S elif matrixtype == 'PS': converged = converged_P and converged_S if iterc >= max_iters: stable = False else: iterc += 1 stop = converged or not stable or not stable2 # print('\ndlyap iters = %s' % str(iterc)) elif algo == 'finite_horizon': P = np.copy(Q) Pt = np.copy(Q) S = np.copy(Q) St = np.copy(Q) converged = False stop = False while not stop: if matrixtype == 'P' or matrixtype == 'PS': APAunc = np.zeros([n, n]) for i in range(p): APAunc += a[i] * mdot(Aa[:, :, i].T, Pt, Aa[:, :, i]) BPBunc = np.zeros([n, n]) for j in range(q): BPBunc += b[j] * mdot(K.T, Bb[:, :, j].T, Pt, Bb[:, :, j], K) Pt = mdot(AK.T, Pt, AK) + APAunc + BPBunc P += Pt converged_P = np.abs(Pt).sum() < 1e-15 stable = np.abs(P).sum() < 1e10 if matrixtype == 'S' or matrixtype == 'PS': ASAunc = np.zeros([n, n]) for i in range(p): ASAunc += a[i] * mdot(Aa[:, :, i], St, Aa[:, :, i].T) BSBunc = np.zeros([n, n]) for j in range(q): BSBunc = b[j] * mdot(Bb[:, :, j], K, St, K.T, Bb[:, :, j].T) St = mdot(AK, Pt, AK.T) + ASAunc + BSBunc S += St converged_S = np.abs(St).sum() < 1e-15 stable = np.abs(S).sum() < 1e10 if matrixtype == 'P': converged = converged_P elif matrixtype == 'S': converged = converged_S elif matrixtype == 'PS': converged = converged_P and converged_S stop = converged or not stable if not stable: P = None S = None if show_warn: warnings.simplefilter('always', UserWarning) warn('System is possibly not mean-square stable') if matrixtype == 'P': return P elif matrixtype == 'S': return S elif matrixtype == 'PS': return P, S