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 [ ]:
##############################
# 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])
u_max = 2.5e-5
n = 3
X0 = [np.matrix([-10, -0.05, -0.0046]).T,
np.matrix([-10, -0.05, 0.0046]).T,
np.matrix([-10, 0.05, -0.0046]).T,
np.matrix([10, -0.05, 0.0046]).T,
np.matrix([10, -0.05, -0.0046]).T,
np.matrix([10, 0.05, 0.0046]).T]
#print "A:\n", A
#print "a:\n", a
#print "b:\n", b
#print "c:\n", c
N = cvx.matrix(_N(n))
M = cvx.matrix(_M(n))
##### Entwurf parameter #####
beta = 2 # beta >=1 !
In [ ]:
######################################################
# Helper for Constraint variant (4.65) -> Q_sum #
######################################################
# TODO: Was ist m? Kann man das berechnen oder wird das festgelegt m<=2n-1, meistens n+1?
from scipy.special import comb as nchoosek # n Choose k (n ueber k)
def get_a_list(a, Q, z, m):
n = len(a) # dim of a, z, Q(nxn)
m = m # Is this to choose or somehow given?
l = m+1 # length of the array with all a_i in it i:{0,m}
H = lambda k: optim_tools._H(k, n) # Convinence function to fix n and make specialized H(k)
N = optim_tools._N(n)
a_list = [np.zeros((n, n))] * l
for i in range(0, l): # m eingeschlossen
if i <= (m-1)/2.0: # 0 <= i <= (m-1)/2
for k in range(1, i+1):
a_list[i] += a.T * H(n+k-i)*Q*N*H(n-k+1)*a - z.T*N*H(n-i)*a
elif i <= m: # (m-1)/2 < i <=m
for k in range(1, 2*n-i+1):
a_list[i] += a.T * H(k)*Q*N*H(2*n-i-k+1)*a
else:
# this branch is currently not possible in this program
print "i={} < m={}".format(i, m)
a_list[i] = 0
return a_list
def trans_a_list(a_list, eps):
l = len(a_list)
m = l-1 #biggest index in a_list
a1_list = [np.zeros(a_list[0].shape)] * l
for j in range(0, l): #for each in a_list
for i in range(j, l): # for each coefficient including m
a1_list[j] += nchoosek(i, i-j) * ((1.0+eps)/(1.0-eps))**(i-j) * ((1.0-eps)/(2.0))**i * a_list[i]
return a1_list
def calc_a_Sum(a_list):
l = len(a_list) # number of matrizen in a_list
m = l-1 # Index of Q_m
n = a_list[0].size[0] # shape of each matrix, first element
if m is 0:
a_sum = cvxpy.bmat([[2*a_list[0], np.zeros(n)],
[np.zeros(n), np.zeros(n)]])
elif m is 1:
a_sum = cvxpy.bmat([[2*a_list[0], a_list[1]],
[a_list[1], np.zeros(n)]])
else: # e.g. m is 2 or more
a_sum = cvxpy.bmat([[2*a_list[0], a_list[1]],
[a_list[1], 2*a_list[2]]])
for i1 in range(3, l, 2):
S_new_col = cvxpy.vstack(np.zeros((((i1+1)/2-1)*n, n)), a_list[i1])
if i1 is m:
S_new_row = cvxpy.hstack(np.zeros((n, ((i1+1)/2-1)*n)), a_list[i1], np.zeros((n,n)))
else:
S_new_row = cvxpy.hstack(np.zeros((n, ((i1+1)/2-1)*n)), a_list[i1], 2*a_list[i1+1])
a_sum = cvxpy.bmat([[a_sum, S_new_col],
[S_new_row]])
a_sum = -0.5*a_sum
return a_sum
def calc_lmi_cond(a_sum, n):
k = a_sum.size[1] / n # This dimension is from Boris. Dilyana does not specifiy the dimension though
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))])
return np.vstack([C, J]), n*(k-1)
In [ ]:
# S. 78 Boris (LMI-Entwurf)
def convex_problem(gamma, mu=1):
##############################
# Convex Problem (35) #
##############################
prob = pic.Problem()
# Constants
AA = pic.new_param('A', A)
II = pic.new_param('I_n', np.eye(n))
III = pic.new_param('I_n+1', np.eye(n+1))
aa = pic.new_param('a', a)
bb = pic.new_param('b', b)
XX0 = pic.new_param('X0', X0)
NN = pic.new_param('N', N)
MM = pic.new_param('M', M)
AA0 = pic.new_param('A0', AA+bb*aa.T)
## REMARK THIS!!!! gamma is optimization variable but not convex, thus to be bisected to find "bigg-ish" value
gamma = pic.new_param('gamma', gamma)
# Problem
prob = pic.Problem()
# Parameters
QQ = prob.add_variable('Q', (n, n), vtype='symmetric')
zz = prob.add_variable('z', n)
zz_star = prob.add_variable('z_star', n)
# Objective
prob.set_objective('find', None)
# Constraints
#prob.add_constraint(QQ >> 0)
#prob.add_constraint(QQ*NN + NN*QQ << 0)
prob.add_constraint(QQ*AA0.T + AA0*QQ - zz*bb.T - bb*zz.T << 0)
## (31) ???
prob.add_list_of_constraints([((1 & XX0[i].T) //
(XX0[i] & QQ )) >> 0
for i in range(0, len(X0))])
prob.add_constraint(QQ*AA0.T + AA0*QQ - zz_star*bb.T - bb*zz_star.T << -2*(gamma*QQ))
prob.solve(verbose=0, solver='cvxopt')
return prob
In [ ]:
## Lets bisect
# Expects min_val to be valid, and max_val to be not valid
# func only taking on (scalar) argument -> gamma
def bisect_prob(min_val, max_val, func, diff=1e-5, max_iter=50, _iteration=0):
if _iteration > max_iter:
print "Iter:", _iteration
print "Diff:", (max_val - min_val)/2.0
return min_val, prob
elif (max_val - min_val)/2.0 <= diff:
print "Iter:", _iteration
print "Diff:", (max_val - min_val)/2.0
return min_val, prob
else:
mid_val = min_val+(max_val - min_val)/2.0
#print "1. Evaluating: ", mid_val
try:
prob = func(mid_val)
Q = prob.get_valued_variable('Q')
z = prob.get_valued_variable('z')
z_star = prob.get_valued_variable('z_star')
except Exception as e:
#print "Problem not solved!"
#print e
max_val = mid_val
else:
#print "Problem solved!"
#print Q, z, z_star
min_val = mid_val
finally:
#print "2. Recursing: ", min_val, max_val
return bisect_prob(min_val, max_val, func, diff, max_iter,_iteration+1)
#return min_val, max_val
In [ ]:
#%%timeit ~4sec
val, prob = bisect_prob(1, 50, convex_problem)
print val
print convex_problem(val).get_valued_variable('Q')
In [ ]:
In [ ]:
prob = convex_problem(5.1)
Q = None
z = None
z_star = None
try:
Q = prob.get_valued_variable('Q')
z = prob.get_valued_variable('z')
z_star = prob.get_valued_variable('z_star')
except Exception as e:
print "Problem not solved!"
print e
else:
print "Problem solved!"
print Q, z, z_star
#print prob.get_valued_variable('z')
#print prob.get_valued_variable('z_star')
#print "P:\n", PP
#print "eig:\n", LA.eigvals(PP.value)
#print
#print "AP+PA:\n",(AA.T*PP + PP*AA).value
#print "eig:\n", LA.eigvals((AA.T*PP + PP*AA).value)
In [ ]:
In [ ]:
In [ ]:
## Seite 47 Boris
# Wähle p_min, mu_start, zeta, zeta_start
def step1(p_min, mu_start, zeta, zeta_start):
# Löse (A.15) mit mu = 1 -> l_star -> lambda_hat_i(1)=lambda_i(A-b*l_star.T)
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]: