"""
"""
import os
import numpy as np
import matplotlib.pyplot as plt
import control
import scipy.linalg
from buzz import ssutil
from wield.bunch import Bunch
from wield.control import MIMO
from wield.utilities.np import logspaced
from wield.control.ss_bare.ssprint import print_dense_nonzero, print_dense_nonzero_M
from wield.control import ss_bare
# from Final_Func import *
from . import solvers
[docs]
def BH1solverset(
ss,
z2=["S", "O"],
zinf=["Zinf"],
y=["D"],
w=["F1scaled", "F2scaled"],
u=['T'],
igsq_capture=None,
# print=lambda *a: None,
print=print,
minpole=1e-5,
minpole_jump0=1e-6,
include_non_optimal=False,
):
"""
Takes a wield.controls.MIMO.ss object.
The arguments are the names of the inputs and outputs for a mixed H2, Hinfinity problem.
Assumes that the sshas already been transposed.
TODO, allow it to not assume this, and transpose it inside?
starting 1/gamma^2 value to use
end 1/gamma^2 value to seek. May terminate before that!
igsq_end=1
list of inverse gammas to refine the search on and return in the set
igsq_capture=None
if it is None, it will return them every decade
TODO, make the transition score and the refine score less magical numbers.
Not sure how to auto-scale them yet
"""
print()
# dprint(ss.input_dissections)
# dprint(ss.output_dissections)
# ss = ssutil.wieldSS(ss.A, ss.B, ss.C, ss.D, iod=ss.iod)
ss = ss.dissect(
ilists=[w, u],
inames=['w', 'u'],
olists=[zinf, z2, y],
onames=['zinf', 'z2', 'y']
)
# TODO, integrate this functionality into wield.control
# this method is hacky, but does help scale problems
Od = ss.output_dissections_byname['z2']
C = ss.C.copy()
D = ss.D.copy()
D_z2_u = ss.dissectD(iname='u', oname='z2')
Linf_z2_scale = scipy.linalg.norm(D_z2_u, 2)
# print("Linf Z2 rescaling: ", Linf_z2_scale)
# additional scale factor, if desired
rescale = 1e0
C[Od.idx_start:Od.idx_end, :] *= rescale / Linf_z2_scale
D[Od.idx_start:Od.idx_end, :] *= rescale / Linf_z2_scale
ss = ss.__build_similar__(
ss_bare.BareStateSpace(
A=ss.A,
B=ss.B,
C=C,
D=D,
E=ss.E,
)
)
ss = ssutil.balance_sys_gain(ss)
ss = ss.dissect(
ilists=[w, u],
inames=['w', 'u'],
olists=[zinf, z2, y],
onames=['zinf', 'z2', 'y']
)
retset = []
# takes the dissected statespace and sets up the parameter space
bh = BH1setup(ss)
local_plist = []
minpole_plist = []
for pole, cplx, pidx in bh.plist:
if abs(np.real(pole)) < minpole:
local_plist.append((pole, cplx, pidx))
minpole_plist.append((-minpole, cplx, pidx))
print("LPL", local_plist)
print("MPL", minpole_plist)
# minpole_plist = []
Zero = np.zeros((bh.A.shape[-2], bh.A.shape[-2]))
bht = BH1solverQZQh(
bh,
Q=None,
igsq=0,
modpole_plist=minpole_plist,
print=print
)
# bht2 = BH1solverQZQh(
# bh,
# Q=None,
# igsq=0,
# modpole_plist=[],
# print=print
# )
# print("Q-diff", (bht.Q - bht2.Q) / bht.Q)
Linf, _ = bht.ss_u_z2.Linf_norm()
Linf_full, _ = bht.ss_u_z2_full.Linf_norm()
Linf_D = scipy.linalg.norm(bht.ss_u_z2_full.D, 2)
print("LINF: {:.1e}".format( Linf))
print("LINF full: {:.1e}, vs: {:.1e}".format(Linf_full, Linf_D))
Qprime = bht.Q
# # return the H2 solve
# Bret = Bunch(igsq=0, optimal=True)
# Bret.Ac, Bret.Bc, Bret.Cc = BH1controllerQZQh(bh, bht, Q, Z, Qh)
# retset.append(Bret)
if igsq_capture is None:
igsq_capture = [
0, # H2 constraint
1.8e-2, # 1 deg
1e-1, # 6 deg
0.20, # 11.5 deg
0.3, # 17 deg
0.5, # 30 deg
0.7653668647301797, # 45 deg
0.85, # 50.0 deg
0.90, # 53.5 deg
0.95, # 56.5 deg
# above this it gets very hairy
# these are 10 solutions
]
igsq_capture = np.asarray(igsq_capture)
if igsq_capture.size == 0:
return retset
igsq_capture = sorted(igsq_capture)
# reverse it to pop
# igsq_capture = list(igsq_capture)[::-1]
asq_eff_max = Linf_full**2 / Linf_D**2
for igsq in igsq_capture:
# print("IGSQ: ", igsq)
bht = BH1solverQZQh(
bh,
Q=Qprime,
igsq=igsq,
modpole_plist=minpole_plist,
print=lambda *a: None
)
Z_tst = bht.eq52_Z(Z=Zero, Qh=Zero, asq_eff=asq_eff_max)
Qh_tst = bht.eq53_Qh(Z=Z_tst, Qh=Zero)
optimal = False
mp_copy = list(minpole_plist)
good_mplist = minpole_plist
good_bht = bht
while mp_copy:
# print("MP_copy: ", mp_copy)
pole, cplx, pidx = mp_copy.pop()
pole_target, _, _ = local_plist[len(mp_copy)]
if cplx:
Nsteps = 40
else:
Nsteps = 20
if pole_target <= 0:
sequence = np.concatenate([-logspaced(-pole, minpole_jump0, Nsteps), [pole_target]])
else:
sequence = -logspaced(-pole, -pole_target, Nsteps)
for ptarg in sequence:
# print("TRYING POLE", ptarg, 'towards', pole_target)
try:
mplist = mp_copy + [(ptarg, cplx, pidx)]
bht = BH1solverQZQh(
bh,
Q=Zero,
igsq=igsq,
modpole_plist=mplist,
print=lambda *a: None
)
Z_tst = bht.eq52_Z(Z=Z_tst, Qh=Qh_tst, asq_eff=asq_eff_max)
score = bht.eval_eq52(Z=Z_tst, Qh=Qh_tst)
# Z_tst = Z / igsq**0.5
Qh_tst = bht.eq53_Qh(Z=Z_tst, Qh=Qh_tst)
score = bht.eval_eq53(Z=Z_tst, Qh=Qh_tst)
good_mplist = mplist
good_bht = bht
Q = bht.Q
except Exception as e:
print("Failed on igsq: ", igsq, e)
isgood = False
break
else:
good_mplist = []
else:
Q = good_bht.Q
bht = good_bht
asq_eff = asq_eff_max
logstep = (np.log(asq_eff_max) - np.log(1)) / 100
goodset = (Z_tst, Qh_tst, asq_eff)
N_reconverge = 0
N_step = -1
N_resets = 0
N_1 = 0
while True:
N_step += 1
# print("ASQ_EFF: ", asq_eff)
try:
# score_prev = bht.eval_eq52(Z=Z_tst, Qh=Qh_tst)
Z_tst = bht.eq52_Z(Z=Z_tst, Qh=Qh_tst, asq_eff=asq_eff)
# score = bht.eval_eq52(Z=Z_tst, Qh=Qh_tst)
# Z_tst = Z / igsq**0.5
Qh_tst = bht.eq53_Qh(Z=Z_tst, Qh=Qh_tst)
# score = bht.eval_eq53(Z=Z_tst, Qh=Qh_tst)
isgood = True
s, v = scipy.linalg.eigh(Qh_tst)
s = s[::-1]
# print("IGSQs", igsq, "score: ", score)
# print(s[0], s[-1])
if s[-1] < -1e-4:
isgood = False
s, v = scipy.linalg.eigh(Z_tst)
s = s[::-1]
# print("Z IGSQs", s[-1])
if s[-1] < -1e-4:
isgood = False
except Exception as e:
print("Failed on igsq: ", igsq, e)
isgood = False
if not isgood:
# check that we have not just lost convergence
if N_reconverge > 4:
optimal = False
break
elif N_reconverge == 0:
# reset and try again
(Z_tst, Qh_tst, asq_eff) = goodset
# take a half step backward, this also resets the N_converge clock
asq_eff = asq_eff * np.exp(logstep / 2)
logstep = max(np.log(1.02), logstep / 2)
N_resets += 1
print(f"Reset #{N_resets}, asq_eff={asq_eff:.1e}, igsq={igsq}", )
N_reconverge += 1
continue
else:
goodset = (Z_tst, Qh_tst, asq_eff)
if asq_eff == 1:
# now check that the solution is stable for three cycles
if N_1 > 9 or N_reconverge > 3:
optimal = True
break
N_1 += 1
N_reconverge += 1
continue
N_reconverge = 0
asq_eff = asq_eff / np.exp(logstep)
if asq_eff < 1:
asq_eff = 1
Nsteps_remaining = (np.log(asq_eff) - np.log(1)) / logstep
if N_step % 30 == 0:
print(f"Steps remaining: {round(Nsteps_remaining)}, asq_eff={asq_eff:.1e}, igsq={igsq}", )
if Nsteps_remaining > 1e3 or N_step > 300:
# not converging
optimal = False
break
# Qh_tst2 = bht.eq53_Qh_lyap(Z=Z, Qh=Zero)
# score = bht.eval_eq53(Z=Z, Qh=Qh_tst2)
# print("IGSQs lyap", igsq, "score: ", score)
# s, v = scipy.linalg.eigh(Qh_tst2)
# s = s[::-1]
# print(s[0], s[-1])
if not optimal:
print(f"Non-optimal solve at igsq={igsq:.2e} and asq={asq_eff:.2e}, N_step={N_step}")
Z_tst, Qh_tst, asq_eff = goodset
Bret = Bunch(igsq=igsq, optimal=optimal, asq_eff=asq_eff)
Bret.Ac, Bret.Bc, Bret.Cc, = BH1controllerQZQh(bh, bht, Q, Z_tst, Qh_tst)
if optimal or include_non_optimal:
retset.append(Bret)
return retset
[docs]
def BH1solverQZQh(
bh,
Q,
igsq,
asq_eff=1,
modpole_plist=[],
print=print,
vprint=print,
):
"""
This solver assumes that Einf is null and this impacts a number of choices
"""
A = bh.A.copy()
for pole_r, cplx, pidx in modpole_plist:
A[pidx, pidx] = pole_r
if cplx:
A[pidx+1, pidx+1] = pole_r
# show the poles moving
# print(np.diag(A))
# print(np.diag(bh.A))
B = bh.B
C = bh.C
ss_u_z2_full = ss_bare.BareStateSpace(
A=bh.A,
B=bh.B_u,
C=bh.C_z2,
D=bh.D_z2_u,
E=None
)
ss_u_z2 = ss_bare.BareStateSpace(
A=A,
B=bh.B_u,
C=bh.C_z2,
D=np.zeros((bh.C_z2.shape[-2], bh.B_u.shape[-1])),
E=None
)
R1 = bh.R1
V1i = bh.V1i
V12i = bh.V12i
R2hat = bh.R2hat
# print("R2hat!", R2hat) # proportional to sensing gain squared
Sigma = bh.Sigma # inversely propto
invR2hat = bh.invR2hat # inversely
betasq = bh.betasq # inversely
def eval_eq37(Q, text=""):
Qa = Q @ C.T + V12i
# with restrictions, equal to eq 3.7 in BH1
eq37 = (
A @ Q + Q @ A.T
+ V1i
- Qa @ bh.invV2i @ Qa.T
)
eq37 = (eq37 + eq37.T)/2
Tfrob = np.trace(eq37 @ eq37.T)**0.5
print("eq37 {} S:".format(text), Tfrob)
return Tfrob
def eval_eq52(Z, Qh, text=""):
Astar52 = (A - betasq / asq_eff * igsq * Qh @ R1)
eq52 = (Astar52.T @ Z + Z @ Astar52 + R1
- Z.T @ (
Sigma / asq_eff
- (betasq**2 / asq_eff**2 * igsq**2) * Qh @ R1 @ Qh
- (betasq / asq_eff * igsq) * Qa @ bh.invV2i @ Qa.T
) @ Z)
eq52 = (eq52 + eq52.T)/2
Tfrob = np.trace(eq52 @ eq52.T)**0.5
print("eq52 {} S:".format(text), Tfrob)
return Tfrob
def eval_eq53(Z, Qh, text=""):
AstarP53 = (A - bh.Sigma @ Z)
# with restrictions, equal to eq 3.9 in BH1
eq53 = (
AstarP53 @ Qh + Qh @ AstarP53.T
# + Q @ bh.SigmaBar @ Q.T Actually should be the next line
+ Qa @ bh.invV2i @ Qa.T
+ Qh @ ((igsq * betasq) * Z @ bh.Sigma @ Z) @ Qh # Note the plus!
)
eq53 = (eq53 + eq53.T)/2
Tfrob = np.trace(eq53 @ eq53.T)**0.5
print("eq53 {} trace:".format(text), Tfrob)
return Tfrob
def eq37_Q():
# NOTE there is a discrepancy between the BH papers
# one uses V1 and the other V1i in the Q calculation 3.7 vs 4.1
# Solves the continuous-time algebraic Riccati equation (CARE).
# E^HXA + A^HXE - (E^HXB + S) R^{-1} (B^HXE + S^H) + Q = 0
Q = solvers.solve_continuous_are_scipy(
a=A.T,
b=C.T,
q=V1i,
r=bh.D2 @ bh.D2.T,
e=None,
s=V12i,
balanced=True
)
eval_eq37(Q=Q*0, text="Before")
eval_eq37(Q=Q)
return Q
if Q is None:
Q = eq37_Q()
Qa = Q @ C.T + V12i
def eq52_Z(Z, Qh, asq_eff=asq_eff):
Astar52 = (A - betasq / asq_eff * igsq * Qh @ R1)
BB = (bh.Sigma / asq_eff
- (betasq**2 / asq_eff**2 * igsq**2) * Qh @ R1 @ Qh
- (betasq / asq_eff * igsq) * Qa @ bh.invV2i @ Qa.T
)
BB = (BB + BB.T)/2
s, v = scipy.linalg.eigh(BB)
si = np.argsort(1/abs(s))
s = s[si]
v = v[:, si]
snorm = s / s[0]
# print("S", snorm)
idx_mx = len(s) - np.searchsorted(abs(snorm[::-1]), 1e-10)
# Solves the continuous-time algebraic Riccati equation (CARE).
# E^HXA + A^HXE - (E^HXB + S) R^{-1} (B^HXE + S^H) + Q = 0
Znew = solvers.solve_continuous_are_special(
a=Astar52,
q=R1,
b=v[:, :idx_mx] @ np.diag(abs(s[:idx_mx])**0.5),
r=np.diag(s[:idx_mx] / abs(s[:idx_mx])),
e=None,
s=None,
balanced=True,
positive_definite=False,
symmettrization_method="mirror",
) / asq_eff
return Znew
def eq53_Qh(Z, Qh):
# Solves the continuous-time algebraic Riccati equation (CARE).
# E^HXA + A^HXE - (E^HXB + S) R^{-1} (B^HXE + S^H) + Q = 0
AstarP53 = (A - bh.Sigma @ Z)
Qh = solvers.solve_continuous_are_special(
a=AstarP53.T,
q=Qa @ bh.invV2i @ Qa.T,
b=(igsq * betasq)**0.5 * Z @ B,
r=-R2hat,
e=None,
s=None,
balanced=True,
positive_definite=False,
symmettrization_method="mirror",
)
# print(Qh)
# print("eig", scipy.linalg.eigvals(AstarP43))
# Qh = scipy.linalg.solve_continuous_lyapunov(AstarP43, -Qa @ invV2i @ Qa.T)
# print(Qh)
# score = eval_eq53(Z=Z, Qh=Qh, text='#2')
return Qh
def eq53_Qh_lyap(Z, Qh):
# Solves the continuous-time algebraic Riccati equation (CARE).
# E^HXA + A^HXE - (E^HXB + S) R^{-1} (B^HXE + S^H) + Q = 0
AstarP53 = (A - bh.Sigma @ Z + Qh @ ((igsq * betasq) * Z @ bh.Sigma @ Z / 2))
Qh_new = solvers.solve_continuous_are_special(
a=AstarP53.T,
q=Qa @ bh.invV2i @ Qa.T,
b=0 * (igsq * betasq)**0.5 * Z @ B,
r=-R2hat,
e=None,
s=None,
balanced=True,
positive_definite=False,
symmettrization_method="mirror",
)
return Qh_new
# just return the toolset
return Bunch(locals())
[docs]
def BH1controllerQZQh(
bh,
bht,
Q,
Z,
Qh,
):
Qa = Q @ bh.C.T + bh.V12i
# eq 5.4 in BH1
# eq 3.4 in BH1
Bc = Qa @ bh.invV2i
# eq 5.5 in BH1
Cc = -bh.invR2hat @ bh.B.T @ Z
# print("D22, D!", bh.D)
final = Qa @ bh.invV2i @ bh.D @ bh.invR2hat @ bh.B.T @ Z
final = Bc @ bh.D @ Cc
# print(final.shape)
Ac = bht.A - bh.B @ bh.invR2hat @ bh.B.T @ Z - Qa @ bh.invV2i @ bh.C - final
return Ac, Bc, Cc
[docs]
def BH1setup(ss):
ss = ss.schur_form()
B_w = ss.dissectB(iname='w')
B_u = ss.dissectB(iname='u')
C_zi = ss.dissectC(oname='zinf')
C_z2 = ss.dissectC(oname='z2')
C_yy = ss.dissectC(oname='y')
D_zi_w = ss.dissectD(iname='w', oname='zinf')
D_z2_w = ss.dissectD(iname='w', oname='z2')
D_yy_w = ss.dissectD(iname='w', oname='y')
D_zi_u = ss.dissectD(iname='u', oname='zinf')
D_z2_u = ss.dissectD(iname='u', oname='z2')
D_yy_u = ss.dissectD(iname='u', oname='y')
poles = np.diag(ss.A)
subDpoles = np.diag(ss.A, -1)
idx = 0
plist = []
while idx < len(poles):
if idx >= len(subDpoles) or subDpoles[idx] == 0:
plist.append((poles[idx], False, idx))
idx += 1
else:
# plist.append((poles[idx] + subDpoles[idx]*1j, True, idx))
plist.append((poles[idx], True, idx))
idx += 2
# BH_variables from fig 1 of W. M. Haddad and D. S. Bernstein, “Generalized Riccati equations for the full- and reduced-order mixed-norm H/sub 2//H/sub infinity / standard problem,” in Proceedings of the 28th IEEE Conference on Decision and Control, Dec. 1989, pp. 397–402 vol.1. doi: 10.1109/CDC.1989.70145.
bh = Bunch()
bh.plist = plist
bh.A = ss.A
bh.B_w = B_w
bh.B_u = B_u
bh.C_zi = C_zi
bh.C_z2 = C_z2
bh.C_yy = C_yy
bh.D_zi_w = D_zi_w
bh.D_z2_w = D_z2_w
bh.D_yy_w = D_yy_w
bh.D_zi_u = D_zi_u
bh.D_z2_u = D_z2_u
bh.D_yy_u = D_yy_u
bh.n = ss.A.shape[-1]
bh.In = np.eye(bh.n)
bh.D1 = B_w
bh.B = B_u
bh.E1 = C_z2
bh.E1i = C_zi
bh.C = C_yy
# should be zero
bh.O = D_z2_w
# print("SHOULD BE ZERO in BH")
# print_dense_nonzero_M(bh.O)
bh.Einf = D_zi_w
bh.D2 = D_yy_w
bh.E2 = D_z2_u
bh.E2i = D_zi_u
bh.D = D_yy_u
bh.V1 = bh.D1 @ bh.D1.T
bh.V2 = bh.D2 @ bh.D2.T
bh.V12 = bh.D1 @ bh.D2.T
bh.R1 = bh.E1.T @ bh.E1
bh.R2 = bh.E2.T @ bh.E2
bh.R12 = bh.E1.T @ bh.E2
# BH1 does adjust M and N on igsq, so it can be included in setup
# this is the inverse gamma square value.
alpha_scale = np.max(abs(bh.R2))
bh.M = np.eye(bh.Einf.shape[-2])
bh.iM = np.linalg.inv(bh.M)
bh.R1i = bh.E1i.T @ bh.iM @ bh.E1i
bh.R2i = bh.E2i.T @ bh.iM @ bh.E2i
bh.R12i = bh.E1i.T @ bh.iM @ bh.E2i
bh.R01i = bh.Einf.T @ bh.iM @ bh.E1i
bh.R02i = bh.Einf.T @ bh.iM @ bh.E2i
bh.N = np.eye(bh.Einf.shape[-1])
bh.V1i = bh.D1 @ bh.N @ bh.D1.T
bh.V2i = bh.D2 @ bh.N @ bh.D2.T
bh.V12i = bh.D1 @ bh.N @ bh.D2.T
# print("R2!!", bh.R2)
# print("R2i!!", bh.R2i)
bh.invV2i = np.linalg.inv(bh.V2i)
beta_scale = np.max(abs(bh.R2i))
np.testing.assert_allclose(
bh.R2 * beta_scale,
bh.R2i * alpha_scale,
rtol=1e-10,
atol=1e-10
)
if False and beta_scale**2 > alpha_scale**2:
assert (beta_scale**2 > 0)
bh.R2hat = bh.R2i
bh.alphasq = (alpha_scale / beta_scale)
bh.betasq = 1
bh.igsqINT_scale = 1
else:
# choose this normalization even though alpha is small
# this is needed for BH1 which does not use alpha
assert (alpha_scale**2 > 0)
bh.R2hat = bh.R2
bh.alphasq = 1
bh.betasq = (beta_scale / alpha_scale)
bh.igsqINT_scale = alpha_scale
# print("BETASQ", bh.betasq, bh.R2 * bh.betasq)
bh.invR2hat = np.linalg.inv(bh.R2hat)
bh.Sigma = bh.B @ bh.invR2hat @ bh.B.T
bh.SigmaBar = bh.C.T @ bh.invV2i @ bh.C # actually invV2 but that is the same under the restrictions
# check that the system and model obey the BH1 restrictions
BH1checker(bh)
return bh
[docs]
def BH1checker(bh):
assert (np.all(bh.E1i == 0))
assert (np.all(bh.Einf == 0))
assert (np.all(bh.R12i == 0))
if not np.all(bh.R12 == 0):
print('R12 is not zero! This would normally throw an AssertionError')
print('R12: ', bh.R12)
# assert (np.all(bh.R12 == 0))
assert (np.all(bh.M - np.eye(bh.M.shape[-2]) == 0))
assert (np.all(bh.R1i == 0))
assert (np.all(bh.R01i == 0))
assert (np.all(bh.R02i == 0))
#assert (np.all(bh.V12i == 0))
assert (np.all(bh.N - np.eye(bh.N.shape[-2]) == 0))
np.testing.assert_allclose(
bh.V1,
bh.V1i,
rtol=1e-10,
atol=1e-10
)
assert (bh.alphasq == 1)
# these are all tested to be zero
# E1i = 0
# Einf = 0
# R1i = 0
# R12i = 0
# R01i = 0
# R02i = 0
# R12 = 0
return