def autoScaling(Q, R, N):
""" Compute scaling factors for SDP variables, objective and constraints.
"""
# build hessians
H = [mtools.buildHessian(Q[k], R[k], N[k]) for k in range(len(Q))]
# get mininmum absolute value of eigenvalues
min_eig = 1e10
max_eig = 0.0
for k in range(len(H)):
eigvalsk = np.abs(np.linalg.eigvals(H[k]))
min_eig = min([min_eig, np.min(eigvalsk[np.nonzero(eigvalsk)])])
max_eig = max([max_eig, np.max(eigvalsk[np.nonzero(eigvalsk)])])
# maximum condition number
max_cond = max_eig / min_eig
min_eig = 1e-8 # SDP SCALING!!!
max_cond = 1e8
scaling = {
'alpha': 1 / min_eig,
'beta': max_cond,
'dP': 1 / min_eig,
'F': 1 / min_eig,
'T': 1 / min_eig
}
return scaling
def convexHessianSuppl(A,
B,
Q,
R,
N,
dP,
G=None,
Fg=None,
C=None,
F=None,
T=None):
""" Construct the convexified Hessian Supplement
:param A: system matrix
:param B: input matrix
:param Q: weighting matrix Q (nx,nx)
:param R: weighting matrix R (nu,nu)
:param N: weighting matrix N (nx,nu)
:param dP: ...
:return: convexified Hessian supplement
"""
period = len(dP)
nx = A[0].shape[0] # state dimension
dHc, dQc, dRc, dNc = [], [], [], []
for i in range(period):
# unpack dP
dP1 = dP[i]
dP2 = dP[(i + 1) % period]
# convexify
Qco = np.squeeze(np.dot(np.dot(A[i].T, dP2), A[i]) - dP1)
Rco = np.dot(np.dot(B[i].T, dP2), B[i])
Nco = np.dot(np.dot(B[i].T, dP2.T), A[i]).T
Hco = mtools.buildHessian(Qco, Rco, Nco)
if G:
Hco = Hco + np.dot(np.dot(G[i].T, np.diagflat(Fg[i])), G[i])
if F:
if C[i] is not None:
nc = C[i].shape[0]
Hco = Hco + np.dot(np.dot(C[i].T, np.diagflat(F[i])), C[i])
if T:
Hco = Hco + T[i]
# symmetrize
dHc.append(mtools.symmetrize(Hco))
dQc.append(dHc[i][:nx, :nx])
dRc.append(dHc[i][nx:, nx:])
dNc.append(dHc[i][:nx, nx:])
return dHc, dQc, dRc, dNc
def convexHessianExprPicos(Q,
R,
N,
A,
B,
dP,
alpha,
scaling,
index,
G=None,
C=None,
F=None,
Fg=None):
""" Construct the Picos symbolic expression of the convexified Hessian
:param H: non-convex Hessian
:param A: system matrix
:param B: input matrix
:param dP: ...
:return: Picos Expression of the convexified Hessian
"""
# set-up
period = len(dP)
H = scaling['alpha'] * alpha * mtools.buildHessian(Q[index], R[index],
N[index])
A = A[index]
B = B[index]
if C is not None:
CM = C[index]
if G is not None:
GM = G[index]
dP1 = scaling['dP'] * dP[index]
dP2 = scaling['dP'] * dP[(index + 1) % period]
# convexify
dQ = A.T * dP2 * A - dP1
dN = A.T * dP2 * B
dNT = B.T * dP2.T * A
dR = B.T * dP2 * B
dH = (dQ & dN) // (dN.T & dR)
# add constraints contribution
if G is not None:
dH = dH + GM.T * picos.diag(scaling['F'] * Fg[index]) * GM
if F is not None:
if F[index] is not None:
dH = dH + CM.T * picos.diag(scaling['F'] * F[index]) * CM
return H + dH
def convexify(A,
B,
Q,
R,
N,
C=None,
opts={
'rho': 1e-3,
'solver': 'mosek',
'force': False
}):
""" Convexify the indefinite Hessian "H" of the system with the discrete time dynamics
.. math::
x_{k+1} = A x_k + B u_k
so that the solution of the LQR problem based on the convexified
Hessian "H + dH" yields the same trajectory as the LQR-solution
of the indefinite problem.
:param A: system matrix
:param B: input matrix
:param Q: weighting matrix Q (nx,nx)
:param R: weighting matrix R (nu,nu)
:param N: weighting matrix N (nx,nu)
:param C: jacobian of active constraints at steady state (nc, nx+nu)
:param opts: tuning options
:return: Convexified Hessian supplement "dH".
"""
# perform input checks
arg = {**locals()}
del arg['opts']
# extract steady-state period
period = len(arg['A'])
Logger.logger.info(
'Convexify Hessians along {:d}-periodic steady state trajectory.'.
format(period))
if arg['C'] is None:
del arg['C']
Logger.logger.info(
'Convexifier called w/o active constraints at steady state')
arg = preprocessing.input_checks(arg)
# extract dimensions
nx = arg['A'][0].shape[0]
nu = arg['B'][0].shape[1]
# check if hessian is already convex!
min_eigval = list(
map(
lambda q, r, n: np.min(
np.linalg.eigvals(mtools.buildHessian(q, r, n))), arg['Q'],
arg['R'], arg['N']))
if min(min_eigval) > 0:
Logger.logger.info(
'Provided hessian(s) are already positive definite. No convexification needed!'
)
return np.zeros((nx + nu, nx + nu)), np.zeros((nx, nx)), np.zeros(
(nu, nu)), np.zeros((nx, nu))
# solver verbosity
if Logger.logger.getEffectiveLevel() < 20:
opts['verbose'] = 1
else:
opts['verbose'] = 0
Logger.logger.info('Construct SDP...')
Logger.logger.info('')
Logger.logger.info(50 * '*')
# perform autoscaling
scaling = autoScaling(arg['Q'], arg['R'], arg['N'])
# define model
M = setUpModelPicos(**arg, constr=False)
Logger.logger.info('Step 1: (\u03B7_F = 0), (\u03B7_T = 0)')
# solve
constraint_contribution = False
M = solveSDP(M, opts)
if M.status == 'optimal':
Logger.logger.info('Optimal solution found.')
Logger.logger.info('Maximum condition number: {}'.format(
scaling['beta'] * M.variables['beta'].value))
Logger.logger.info('Smallest eigenvalue: {}'.format(
1 / (scaling['alpha'] * M.variables['alpha'].value)))
Logger.logger.info('EQUIVALENCE TYPE A')
Logger.logger.info(50 * '*')
if M.status != 'optimal' and 'C' in arg:
Logger.logger.info('!! Problem infeasible !!')
Logger.logger.info(50 * '*')
Logger.logger.info('Step 2: (\u03B7_F = 1), (\u03B7_T = 0)')
# create model with active constraint regularisation
M = setUpModelPicos(**arg, rho=opts['rho'], constr=True)
# solve again
constraint_contribution = True
M = solveSDP(M, opts)
if M.status == 'optimal':
Logger.logger.info('Optimal solution found.')
Logger.logger.info('Maximum condition number: {}'.format(
scaling['beta'] * M.variables['beta'].value))
Logger.logger.info('Smallest eigenvalue: {}'.format(
1 / (scaling['alpha'] * M.variables['alpha'].value)))
Logger.logger.info('EQUIVALENCE TYPE B')
Logger.logger.info(50 * '*')
if M.status != 'optimal':
Logger.logger.warning('!! Problem infeasible !!')
Logger.logger.warning(
'!! Strict dissipativity does not hold locally !!')
Logger.logger.warning(
'!! The provided indefinite LQ MPC problem is not stabilising !!')
Logger.logger.warning(50 * '*')
if opts['force']:
Logger.logger.info('Step 3: (\u03B7_F = 1), (\u03B7_T = 1)')
Logger.logger.info('Enforcing convexification...')
raise ValueError('Step 3 not implemented yet.')
Logger.logger.warning(50 * '*')
else:
Logger.logger.warning(
'Consider operating the system at another orbit of different period p'
)
Logger.logger.warning(
'Convexification and stabilization of the MPC scheme can be enforced by enabling "force"-flag.'
)
Logger.logger.warning(
'In this case there are no guarantees of (local, first-order) equivalence.'
)
raise ValueError(
'Convexification is not possible if the system is not optimally operated at the optimal orbit.'
)
# build convex hessian list
dP = [scaling['dP']*np.array(M.variables['dP'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \
for i in range(period)]
if not constraint_contribution:
dHc, dQc, dRc, dNc = convexHessianSuppl(**arg, dP=dP)
else:
F = [scaling['F']*np.array(M.variables['F'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \
for i in range(period)]
dHc, dQc, dRc, dNc = convexHessianSuppl(**arg, dP=dP, F=F)
# check
assert 1 / M.variables[
'alpha'].value > 0, 'convexified hessian(s) should be positive definite'
Logger.logger.info('')
Logger.logger.info('Hessians convexified.')
Logger.logger.info('')
return dHc, dQc, dRc, dNc
def check_convergence(M,
scaling,
A,
B,
Q,
R,
N,
G=None,
Fg=None,
C=None,
constr=False,
force=False):
# build convex hessian list
dP = [scaling['dP']*np.array(M.variables['dP'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \
for i in range(len(A))]
if G is not None:
Fg = [scaling['F']*np.array(M.variables['Fg'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \
for i in range(len(A))]
if not constr and not force:
dHc, dQc, dRc, dNc = convexHessianSuppl(A, B, Q, R, N, dP, G=G, Fg=Fg)
if constr:
F = []
for i in range(len(A)):
if 'F' + str(i) in M.variables:
F.append(scaling['F'] *
np.array(M.variables['F' + str(i)].value) /
(scaling['alpha'] * M.variables['alpha'].value))
else:
F.append(None)
if not force:
dHc, dQc, dRc, dNc = convexHessianSuppl(A,
B,
Q,
R,
N,
dP,
G=G,
Fg=Fg,
C=C,
F=F)
else:
T = [scaling['T']*np.array(M.variables['T'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \
for i in range(len(A))]
dHc, dQc, dRc, dNc = convexHessianSuppl(A,
B,
Q,
R,
N,
dP,
G=G,
Fg=Fg,
C=C,
F=F,
T=T)
else:
if force:
T = [scaling['T']*np.array(M.variables['T'+str(i)].value)/(scaling['alpha']*M.variables['alpha'].value) \
for i in range(len(A))]
dHc, dQc, dRc, dNc = convexHessianSuppl(A,
B,
Q,
R,
N,
dP,
G=G,
Fg=Fg,
T=T)
# add hessian supplements
Hc = [
mtools.buildHessian(Q[k], R[k], N[k]) + dHc[k] for k in range(len(dHc))
]
# compute eigenvalues
min_eigenvalue = min([np.min(np.linalg.eigvals(Hk)) for Hk in Hc])
max_eigenvalue = max([np.max(np.linalg.eigvals(Hk)) for Hk in Hc])
max_cond = max([np.linalg.cond(Hk) for Hk in Hc])
if min_eigenvalue > 0.0:
if M.status == 'optimal':
status = 'Optimal'
else:
status = 'Feasible'
Logger.logger.info('{} solution found.'.format(status))
Logger.logger.info('Maximum condition number: {}'.format(max_cond))
Logger.logger.info('Minimum eigenvalue: {}'.format(min_eigenvalue))
else:
status = 'Infeasible'
Logger.logger.info('SDP solver status: {}'.format(M.status))
Logger.logger.info('Minimum eigenvalue: {}'.format(min_eigenvalue))
Logger.logger.info('!! Problem infeasible !!')
return status, dHc, dQc, dRc, dNc