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 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 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 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