In [ ]:
import numpy as np
import control as con
from numpy import linalg as LA
import cvxpy
import optim_tools as optim_tools#own file with helper
In [ ]:
In [ ]:
##############################
# Boris Paper UBoot #
##############################
A = np.matrix([[0, 1, 0],
[0, 0, 1],
[0, 0, -0.005]])
a = -A[-1,:].T ### !!!!!
b = np.matrix([[0],[0],[1]])
c = np.matrix([1, 0, 0])
d = np.matrix([0])
u_max = 2.5e-5
n = 3
X0 = [np.matrix([-10.0, -0.05, -0.0046]).T,
np.matrix([-10.0, -0.05, 0.0046]).T,
np.matrix([-10.0, 0.05, -0.0046]).T,
np.matrix([10.0, -0.05, 0.0046]).T,
np.matrix([10.0, -0.05, -0.0046]).T,
np.matrix([10.0, 0.05, 0.0046]).T]
#print "A:\n", A
#print "a:\n", a
#print "b:\n", b
#print "c:\n", c
##### Entwurf parameter #####
beta_val = 2 # beta >=1 !
In [ ]:
from scipy.special import comb as nchoosek # n Choose k (n ueber k)
# n muss be dim(Q) = dim(R1) = dim(A)
# manual approved with equations
def get_S_list(u_max, Q, z, a, n):
S_list = [cvxpy.bmat([[u_max**2 , z.T],
[z , Q]])] # S0 is different !
for i in range(n+1)[1:]:
#print i
#print -a[n-i]
q = Q[:, n-i] # Slicing, (n-i)th column of Q!
#print q
S_list.append(-a[n-i] * cvxpy.bmat([[0, q.T],
[q, np.zeros((n,n))]]))
return S_list
# As seen in A.4 jasniwiecz
# Intervalltransformation (Gleichung (4.36))
# p => [p_l, p_u] (p => [p_min, 1]) wird zu p^ => [-1,1] return neue Matrizen(!) mit diesen p^
#
# S_in ist Liste aus Faktormatrizen für Polynom in p
# S_out ist Liste aus Faktormatrizen für Polynom in p^
# a = (p_u - p_l)
# b = (p_u + p_l)
# manual approved with equations
def trans_S_list(S_in, pl=0.1, pu=1):
a = pu - pl
b = pu + pl
n = len(S_in) # Anzahl der Matrizen in S_in
S_out = [np.zeros(S_in[0].size)] * n
for j in range(0, n): # Bearbeite jede Matrix
for i in range(j, n): # Jeweils mit Summe der anderen
S_out[j] = S_out[j] + 2**(-i) * b**(i-j) * nchoosek(i, j) * S_in[i]
S_out[j] = a**j*S_out[j]
return S_out
def calc_S_Sum(S_list):
l = len(S_list) # number of matrizen in S_list
m = l-1 # Index of Q_m
n = S_list[0].size[0] # shape of each matrix, first element
if m is 0:
S_sum = cvxpy.bmat([[2*S_list[0], np.zeros(n)],
[np.zeros(n), np.zeros(n)]])
elif m is 1:
S_sum = cvxpy.bmat([[2*S_list[0], S_list[1]],
[S_list[1], np.zeros(n)]])
else: # e.g. m is 2 or more
S_sum = cvxpy.bmat([[2*S_list[0], S_list[1]],
[S_list[1], 2*S_list[2]]])
for i1 in range(3, l, 2):
S_new_col = cvxpy.vstack(np.zeros((((i1+1)/2-1)*n, n)), S_list[i1])
if i1 is m:
S_new_row = cvxpy.hstack(np.zeros((n, ((i1+1)/2-1)*n)), S_list[i1], np.zeros((n,n)))
else:
S_new_row = cvxpy.hstack(np.zeros((n, ((i1+1)/2-1)*n)), S_list[i1], 2*S_list[i1+1])
S_sum = cvxpy.bmat([[S_sum, S_new_col],
[S_new_row]])
# Beware, there is NO minus compared with Dilyanas version
S_sum = 0.5*S_sum
return S_sum
# CJ = "Selection matrizes" of (31), l=dimension of P and G
def calc_lmi_cond(S_sum, n):
k = S_sum.size[1] / n
J = np.hstack([np.zeros((n*(k-1), n)), np.eye(n*(k-1))])
C = np.hstack([np.eye(n*(k-1)), np.zeros((n*(k-1), n))])
CJ = np.vstack([C, J])
l = n*(k-1)
return CJ, l
In [ ]:
In [ ]:
##############################
# Convex Problem (35) #
##############################
# Variables
Q = cvxpy.Variable(n,n)
# Q = cvxpy.Semidef(n) implys (27a) (semidefinite for numerical reasons?)
z = cvxpy.Variable(n)
z1 = cvxpy.Variable(n) # z_{star}
# Preparation for constraint (31)
S_list = get_S_list(u_max, Q, z, a, n) # Matrizes of Polynom coefficients: S(p)=S_0 + S_1*p + ... S_n*p^n
S1_list = trans_S_list(S_list, 0, 1) # Intervaltransformation p:[0,1] -> p1:[-1,1] (S1 -> S^tilde)
S1_sum = calc_S_Sum(S1_list) # S^tilde_sum Matrix (30)
CJ, l = calc_lmi_cond(S1_sum, n) # "Selection matrizes" of (31), l=dimension of P and G
# Helper variables of optimization
P = cvxpy.Variable(l,l) #positiv definite (semidefinite for numerical reasons?)
G = cvxpy.Variable(l,l) #skew
# Constants
N = optim_tools._N(n)
A0 = A + b*a.T
# Parameters
beta = cvxpy.Parameter(sign='positive') # Design parameter beta >=1; upper bound of saturation
beta.value = beta_val
gamma = cvxpy.Parameter(sign='positive') # Bisection parameter gamma (underlined beta latex: \b{\gamma})
# Constraints
constraint_27a = Q >> 0
constraint_27b = Q*N+N*Q << 0
constraint_27d = Q*A0.T + A0*Q - z*b.T - b*z.T << 0
# Constraints on Helper variables
constraint_G = G + G.T == 0 # skew symmetry
constraint_S = S == S.T # symmetry
constraint_31 = S1_sum << CJ.T * cvxpy.bmat([[-P, G],
[G.T, P]]) * CJ
constraint_32 = [cvxpy.bmat([[1, X0[i].T],
[X0[i], Q ]]) >> 0
for i in range(0, len(X0))]
constraint_33 = Q*A0.T + A0*Q - z1*b.T - b*z1.T << -2*gamma*Q
constraint_34 = cvxpy.bmat([[beta**2*u_max**2, z1.T-a.T*Q],
[z1-Q*a, Q ]]) >> 0
# Collection of all constraints
constraints = [constraint_27a]
constraints.append(constraint_27b)
constraints.append(constraint_27d)
constraints.append(constraint_G)
constraints.append(constraint_S)
constraints.append(constraint_31)
constraints.extend(constraint_32) ##!! Beware of the "extend" if input is array
constraints.append(constraint_33)
constraints.append(constraint_34)
# Define Objective (Generalized eigenvalue problem?)
#obj = cvxpy.Maximize(beta1)
# Feasibility for bisection:
obj = cvxpy.Minimize(0)
# Form and solve problem.
prob35 = cvxpy.Problem(obj, constraints)
In [ ]:
for i in np.arange(0, 3, 0.1):
beta1.value = i
try:
prob35.solve(solver=cvxpy.SCS, verbose=False, warm_start=True) # Returns the optimal value.
print "beta1=", i, prob35.status
if "infeasible" not in prob35.status:
print LA.eigvals(Q.value)
except:
print("Unexpected error:", sys.exc_info()[0])
print "{} ended with error".format(i)
# Test solution with bisection parameter g=1
beta1.value = 2
#Q.value = LA.inv(R1)
prob35.solve(solver=cvxpy.CVXOPT, verbose=True) # Returns the optimal value.
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]: