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 cost_operator_P(problem_data, K, L):
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
]
q = Ai.shape[0]
r = Bj.shape[0]
s = Ck.shape[0]
AKL = A + np.dot(B, K) + np.dot(C, L)
Aunc_P = np.sum([varAi[i] * kron(Ai[i].T) for i in range(q)], axis=0)
BKunc_P = np.sum([varBj[j] * kron(np.dot(K.T, Bj[j].T)) for j in range(r)],
axis=0)
CLunc_P = np.sum([varCk[k] * kron(np.dot(L.T, Ck[k].T)) for k in range(s)],
axis=0)
return kron(AKL.T) + Aunc_P + BKunc_P + CLunc_P
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 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