def verify_gare(problem_data, P, algo_str=None):
"""Verify that the GARE is solved by the solution P"""
if algo_str is None:
algo_str = ''
Qxx, Quu, Qvv, Qux, Qvx, Qvu = qfun(problem_data, P=P)
QxuQxv = np.vstack([Qux, Qvx])
QuuQuvQvuQvv = np.block([[Quu, Qvu.T], [Qvu, Qvv]])
print(algo_str)
print('-' * len(algo_str))
LHS = P
RHS = Qxx - np.dot(QxuQxv.T, la.solve(QuuQuvQvuQvv, QxuQxv))
print(' Left-hand side of the GARE: Positive definite = %s' %
is_pos_def(LHS))
print(LHS)
print('')
print('Right-hand side of the GARE: Positive definite = %s' %
is_pos_def(RHS))
print(RHS)
print('')
print('Difference')
print(LHS - RHS)
print('\n')
return
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 gdlyap(problem_data, K, L, show_warn=False, check_pd=False):
"""
Solve a discrete-time generalized Lyapunov equation
for stochastic linear systems with multiplicative noise.
"""
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 = A.shape[1]
stable = True
# Compute matrix and vector for the linear equation solver
Alin_P = np.eye(n * n) - cost_operator_P(problem_data, K, L)
blin_P = vec(Q) + np.dot(kron(K.T), vec(R)) - np.dot(kron(L.T), vec(S))
# 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)
if not stable:
P = None
if show_warn:
warnings.simplefilter('always', UserWarning)
warn('System is possibly not mean-square stable')
return P
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 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 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 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
def calc_ccare(self):
if is_pos_def(self.Pare):
self._ccare = np.trace(np.dot(self.Pare, self.S0))
else:
self._ccare = np.inf
return self._ccare
def calc_c(self):
if is_pos_def(self.P):
self._c = np.trace(np.dot(self.P, self.S0))
else:
self._c = np.inf
return self._c