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