notebook.community

Edit and run 





In [ ]:



    


import picos as pic
import cvxopt as cvx
import control as con

import numpy as np
from numpy import linalg as LA
from scipy.special import comb as nchoosek # n Choose k (n ueber k)



    











In [ ]:



    


### Seite 37 (im Text zwischen (4.12) und (4.13))
def _N(n):
    return np.diag([p for p in xrange(-n, 0, 1)])

### Seite 55; (4.64)
def _M(n):
    return np.diag([p for p in xrange(0, n, 1)])

### Seite 55; (4.64)
def _P(l, k, n):
    I = np.eye(n)
    Mn = _M(n)
    P = I
    if k == 0:
        pass # P=I
    else:
        for q in xrange(0, k, 1):
            P = P * ((l-q)*I + Mn)
    return cvx.matrix(P)
############################## # Boris Diss Reactor # ############################## A = np.matrix([[1, 0], [0, -0.5]]) b = np.matrix([[1],[-0.5]]) c = np.matrix([1, 0]) u_max = 1 n = 2 print np.matrix(con.matlab.pole(con.matlab.ss(A, b, c, 0))).T X0 = [np.matrix([-0.9, -2.8]).T, np.matrix([-0.9, 2.8]).T, np.matrix([0.9, -2.8]).T, np.matrix([0.9, 2.8]).T] x_max = [0.9, 2.8] #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 [ ]:



    


###################################################
# Boris Diss Hydraulisches Positioniersystem S.61 #
###################################################
A = np.matrix([[0,      1,        0],
               [0, -0.126,     83.5],
               [0,  -6.89, -0.00707]])
 
b = np.matrix([[0],[0],[-55.51]])
c = np.matrix([1, 0, 0])

print "Poles:\n", np.matrix(con.matlab.pole(con.matlab.ss(A, b, c, 0))).T

u_max = 1
n = len(b)

X0 = [np.matrix([-13.38, -10.7, -2.58]).T,
      np.matrix([-13.38, -10.7,  2.58]).T,
      np.matrix([-13.38,  10.7, -2.58]).T,
      np.matrix([-13.38,  10.7,  2.58]).T,
      np.matrix([ 13.38, -10.7, -2.58]).T,
      np.matrix([ 13.38, -10.7,  2.58]).T,
      np.matrix([ 13.38,  10.7, -2.58]).T,
      np.matrix([ 13.38,  10.7,  2.58]).T]

x_max = [13.38, 10.7, 2.58]

#print "A:\n", A
#print "a:\n", a
#print "b:\n", b
#print "c:\n", c
p_min = 0.2
mu_star = 1.5
zeta = 2.5
zeta_star = 1.5

k0 = 1e-3*np.matrix([-24.23, -0.2122, -10.5]).T
k1 = 1e-3*np.matrix([-40.13, -0.1396, -15.10]).T
k0_star = 1e-3*np.matrix([-11.96, -0.1111, -5.18]).T
k1_star = 1e-3*np.matrix([-83.35, -0.0477, -33.86]).T

##### Entwurf parameter #####
beta=2 # beta >=1 !
############################## # Boris Paper UBoot # ############################## A = np.matrix([[0., 1., 0.], [0., 0., 1.], [0., 0., -0.005]]) ############### TODO ################### ############### CHECK ################## #last line of Regelungsnormalform (characteristical polynom coefficients) in a colunm vector a = -np.matrix(A[np.array(A).shape[1]-1])[np.newaxis].T b = np.matrix([[0], [0], [1.]]) d = 0 c = np.matrix([[1], [0], [0]]).T print np.matrix(con.matlab.pole(con.matlab.ss(A, b, c, d))).T u_max = 2.5e-5 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, np.matrix([ 10, 0.05, -0.0046]).T, np.matrix([ 10, 0.05, 0.0046]).T] x_max = [10, 0.05, 0.0046] #simulation time T = 1.0/200.0 #Introduced for fun u_max_sys = u_max a_hat = np.matrix([[4.4469e-8], [2.3073e-5], [4.9148e-3]]) a_hat_star = np.matrix([[1.073e-7], [4.919e-5], [10.4078e-3]]) R1 = np.matrix([[1.6021e-5, 3.26098e-3, 0.4031], [3.2698e-3, 1.5666, 163.46], [0.4031, 163.46, 40.713]]) ##### Entwurf parameter ##### beta = 2 # beta >=1 ! 




In [ ]:



    


# S. 78 Boris (LMI-Entwurf)
def convex_func(gamma_val, mu_val=1, verbose=0):
    ##############################
    # Convex Problem (35)        #
    ##############################
    prob = pic.Problem()

    # Constants
    AA = pic.new_param('A', A)
    II = pic.new_param('I_n', np.eye(n))
    bb = pic.new_param('b', b)
    XX0 = pic.new_param('X0', X0)
    
    #print "mu_inserted", mu_val
    mu = pic.new_param('mu', mu_val**2)

    ## REMARK THIS!!!! gamma is optimization variable but not convex, thus to be bisected to find "bigg-ish" value
    gamma = pic.new_param('gamma', gamma_val)

    # 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)
    eps = prob.add_variable('eps', 2)
#    x0 = prob.add_variable('x0', n)

    
    # Objective
    prob.set_objective('find', None)

    # Other constraints
    prob.add_constraint(eps > 0)
    
    # Constraints
    prob.add_constraint(QQ >> 0)
    
    #(A.10)
    prob.add_list_of_constraints([((QQ          & XX0[i]) //
                                   (XX0[i].T     & 1      )) >> 0
                                       for i in range(0, len(X0))])

#    prob.add_list_of_constraints([x0[i] << pic.simplex(x_max[i])
#                                       for i in range(0, n)])
    
#    prob.add_constraint(((QQ   & x0) //
#                         (x0.T & 1)) >> 0)
    
    #(A.11)
    prob.add_constraint(((QQ   & zz) //
                         (zz.T & 1)) >> 0)

    #(A.12)
    prob.add_constraint(((QQ        & zz_star) //
                         (zz_star.T & mu)) >> 0)
    
    #(A.13)
    prob.add_constraint(QQ*AA + AA.T*QQ - bb*zz.T - zz*bb.T << eps[0]*II) # STRICT SMALLER THAN!
    
    #(A.14)
    prob.add_constraint(QQ*AA + AA.T*QQ - bb*zz_star.T - zz_star*bb.T << -2*(gamma*QQ)+ eps[1]*II) # STRICT SMALLER THAN!
    
    prob.solve(verbose=verbose, solver='cvxopt')
    if np.any(np.matrix(eps.value) <= 0): print "eps>=0!", eps.value
    
    return prob

#convex_func(gamma_val_max, verbose=1)
try:
    gamma_val_max = 0.11e-16
    convex_func(gamma_val_max, verbose=0)
except Exception:
    print "'gamma_val={}' not feasible -> possible max_val for bisect! ('mu_val=1')".format(gamma_val_max)
else:
    print "Exception:"
    print "Choose a higher gamma_max! ({})".format(gamma_val_max)



    











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, name='unnamed'):
    ### TESTING
    try:
        p0 = func(min_val)
        p0.get_valued_variable('Q')
        p0.get_valued_variable('z')
        p0.get_valued_variable('z_star')
    except Exception as e:
        print "Problem not solved for min_val={}".format(min_val)
        raise Exception("Choose another min_val")
    try:
        p1 = func(max_val)
        p1.get_valued_variable('Q')
        p1.get_valued_variable('z')
        p1.get_valued_variable('z_star')
    except Exception as e:
        pass
    else:
        print "Problem solved for max_val={}".format(max_val)
        print "Taking this value as solution!"
        return max_val
        #raise Exception("Choose another max_val")
    ### Testing ok
    if _iteration > max_iter or (max_val - min_val)/2.0 <= diff:
        print name, "done:", min_val, "with iter:", _iteration, ", Diff:", (max_val - min_val)/2.0
        if min_val == 0:
            print "gamma cannot be 0; further trying"
            bisect_prob(min_val, max_val, func, diff*1e-5, max_iter, 0, name)
        return min_val
    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, name)
        #return min_val, max_val



    











In [ ]:



    


#%%timeit ~4sec
val = bisect_prob(0, 1, convex_func)
print val
print convex_func(val).get_valued_variable('Q')



    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    


## Seite 47 Boris
# Wähle p_min, mu_start, zeta, zeta_star
def step1(p_min, mu_star, zeta, zeta_star, gamma_min=0, gamma_max=1):
    data = {}
    ####################################################
    # Löse (convex_func) mit mu = 1 -> l_star -> lambda_hat_i(1)=lambda_i(A-b*l_star.T)
    
    #val = 0.11109
    val = bisect_prob(gamma_min, gamma_max, convex_func, name='gamma_max_mu_1')
    data['gamma_max_mu_1'] = val
    
    prob = convex_func(val)
    data['prob_mu_1'] = prob
    Q = prob.get_valued_variable('Q')
    z_star = np.matrix(prob.get_valued_variable('z_star'))
#    print z_star
    l_star = LA.inv(Q)*z_star
    data['l_star_mu_1'] = l_star
#    print l_star
#    print A-b*l_star.T
    lambda_hat_1 = LA.eigvals(A-b*l_star.T)
    data['lambda_hat_1'] = lambda_hat_1
    
    # control.acker or control.place for lambda_hat_1 -> k1
    #print "lambda_hat_1:", lambda_hat_1, type(lambda_hat_1)
    k1 = np.matrix(con.place(A, b, lambda_hat_1)).T
    
    lambda_hat_pmin = zeta * lambda_hat_1
    data['lambda_hat_pmin'] = lambda_hat_pmin
    #print "lambda_hat_pmin:", lambda_hat_pmin
    ## TODO: Berechne ki
    # control.acker or control.place for lambda_hat_p_min -> k0
    k0 = np.matrix(con.place(A, b, lambda_hat_pmin)).T
    
    ####################################################
    ## Löse (convex_func) mit mu* >= 1 -> l_star -> lambda_hat_i_star(1)=lambda_i(A-b*l_star.T)
    gamma_val_max = bisect_prob(gamma_min, gamma_max,
                                lambda g: convex_func(g, mu_val=mu_star),
                                name='gamma_max_mu_0')
    data['gamma_max_mu_0'] = gamma_val_max
    #print 'gamma_max:', gamma_val_max
    
    prob2 = convex_func(gamma_val_max, mu_val=mu_star)
    data['prob_mu_0'] = prob2
    Q = prob2.get_valued_variable('Q')
    z_star = np.matrix(prob.get_valued_variable('z_star'))
#    print z_star
    l_star = LA.inv(Q)*z_star
    data['l_star_mu_0'] = l_star
#    print l_star
#    print A-b*l_star.T
    lambda_hat_star_1 = LA.eigvals(A-b*l_star.T)
    data['lambda_hat_star_1'] = lambda_hat_star_1
    
    # control.acker or control.place for lambda_hat_star_1 -> k1_star
    k1_star = np.matrix(con.place(A, b, lambda_hat_star_1)).T
    
    lambda_hat_star_pmin = zeta_star * lambda_hat_star_1
    
    # control.acker or control.place for lambda_hat_star_p_min -> k0_star
    k0_star = np.matrix(con.place(A, b, lambda_hat_star_pmin)).T

    return p_min, k0, k1, k0_star, k1_star, data



    











In [ ]:



    


def step2(p_min, k0, k1, k0_star, k1_star):
    ##############################
    # Convex Problem (4.39)      #
    ##############################
    prob = pic.Problem()

    # Parameters
    R0 = prob.add_variable('R0', (n, n), vtype='symmetric')
    R1 = prob.add_variable('R1', (n, n), vtype='symmetric')

    G = prob.add_variable('G', (n, n), vtype='antisym')
    G_star = prob.add_variable('G_star', (n, n), vtype='antisym')

    #G = prob.add_variable('G', (n, n))
    #G_star = prob.add_variable('G_star', (n, n))

    D = prob.add_variable('D', (n, n), vtype='symmetric')
    D_star = prob.add_variable('D_star', (n, n), vtype='symmetric')
    
    # Constants
    AA = pic.new_param('A', A)
    II = pic.new_param('I', np.eye(n))
    bb = pic.new_param('b', b)
    
    alpha = pic.new_param('alpha', 1-p_min)
    beta = pic.new_param('beta', 1+p_min)
    alpha_2 = pic.new_param('alpha_2', (1-p_min)**2)
    beta_2 = pic.new_param('beta_2', (1+p_min)**2)
    
    kk0 = pic.new_param('k0', k0)
    kk1 = pic.new_param('k1', k1)
    
    kk0_star = pic.new_param('k0_star', k0_star)
    kk1_star = pic.new_param('k1_star', k1_star)
    
    XX0 = pic.new_param('X0', X0)
    
    # Expressions
    # x_dot = A_n * x = (A-b*k_n.T)*x 
    A0 = AA-bb*kk0.T
    A1 = AA-bb*kk1.T
    
    A0_star = AA-bb*kk0_star.T
    A1_star = AA-bb*kk1_star.T

    S0 = A1.T*R1+R1*A1
    S1 = A0.T*R1+R1*A0 + A1.T*R0+R0*A1
    S2 = A0.T*R0+R0*A0
    
    S0_star = A1_star.T*R1+R1*A1_star
    S1_star = A0_star.T*R1+R1*A0_star + A1_star.T*R0+R0*A1_star
    S2_star = A0_star.T*R0+R0*A0_star
    
    # Objective
    prob.set_objective('min', pic.trace(R0 + 1/p_min * R1))

    # Constraints
    prob.add_constraint(R1 >> 0) #(4.39a) --> Can be dropped??????????????????
    prob.add_constraint(R0 + R1 >> 0) #(4.39b)
    
    #(4.39c)
    prob.add_constraint(
        (((-D  & G) // (G.T & D)) - ((S0 + 0.5*beta*S1 + 0.25*beta_2*S2 & 0.25*alpha*(S1 + beta*S2)) //
                                    (0.25*alpha*(S1 + beta*S2)         & 0.25*alpha_2*S2))) >> 0)
    
    #(4.39d)
    #prob.add_list_of_constraints([ 1 - XX0[i].T * (R0 + R1) * XX0[i] > 0 
    #                                   for i in range(0, len(XX0))])

    # Schurkompliment - Alternative (4.39d)
    prob.add_list_of_constraints([ ((1      & XX0[i].T) //
                                    (XX0[i] & (R0 + R1))) >> 0 
                                      for i in range(0, len(XX0))]) 

    #(4.39e)
    prob.add_constraint((((1 & kk0.T) // (kk0 & R0)) + 1/p_min * ((0 & kk1.T)//(kk1 & R1)))>>0)

    #(4.39f)
    prob.add_constraint((((1 & kk0.T) // (kk0 & R0)) + ((0 & kk1.T)//(kk1 & R1)))>>0)
    
    #(4.39g)
    prob.add_constraint(
        (((-D_star  & G_star) // (G_star.T & D_star)) - 
         ((S0_star + 0.5*beta*S1_star + 0.25*beta_2*S2_star & 0.25*alpha*(S1_star + beta*S2_star)) //
          (0.25*alpha*(S1_star + beta*S2_star)              & 0.25*alpha_2*S2_star))) >> 0)
    
    prob.solve(verbose=1, solver='cvxopt', solve_via_dual = None)
    return prob



    











In [ ]:



    


p_min, k0, k1, k0_star, k1_star, data =  step1(0.2, 1.5, 2.5, 1.5, gamma_max=10)
print p_min, k0, k1, k0_star, k1_star
#print data

#print k0
p = step2(p_min, k0, k1, k0_star, k1_star)

#p = step2(p_min, 1e-3*np.matrix([-24.23, -0.2122, -10.5]).T,
#                 1e-3*np.matrix([-40.13, -0.1396, -15.10]).T,
#                 1e-3*np.matrix([-11.96, -0.1111, -5.18]).T,
#                 1e-3*np.matrix([-83.35, -0.0477, -33.86]).T)


print "R0:\n", p.get_valued_variable('R0')
# This fails if R0 is not pos-definit
LA.cholesky(p.get_valued_variable('R0'))
print "R1:\n", p.get_valued_variable('R1')
# This fails if R0+R1 is not pos-definit
LA.cholesky(p.get_valued_variable('R0')+p.get_valued_variable('R1'))
print 'ok'



    











In [ ]:



    


R0 = 1e-3*np.matrix([[-10.14, -0.567, -1.552],
                     [-0.567, 0.0779, -0.113],
                     [-1.552, -0.113, 0.651]])

R1 = 1e-3*np.matrix([[14.603, 0.798, 2.172],
                     [0.798,  0.371, 0.142],
                     [2.172,  0.142, 4.84]])

print "R0+R1"
print LA.eigvals(R0+R1)
print LA.cholesky(R0+R1)

print "R0"
print LA.eigvals(R0)
print  LA.cholesky(R0)
Schurcompliment - not dualized and dualized ----------------------- cvxopt CONELP solver ----------------------- pcost dcost gap pres dres k/t 0: 0.0000e+00 -1.0000e+01 1e+03 2e+00 4e+02 1e+00 1: 0.0000e+00 -8.9896e+00 6e+03 3e+00 7e+02 8e+00 2: 0.0000e+00 6.6140e+01 5e+04 3e+00 7e+02 8e+01 3: 0.0000e+00 9.0381e+01 2e+04 8e-01 2e+02 9e+01 4: 0.0000e+00 2.9553e+01 2e+03 5e-02 1e+01 3e+01 5: 0.0000e+00 2.5862e-01 1e+02 3e-03 6e-01 3e-01 6: 0.0000e+00 2.8811e-03 1e+00 3e-05 6e-03 3e-03 7: 0.0000e+00 2.8803e-05 1e-02 3e-07 6e-05 3e-05 8: 0.0000e+00 2.8803e-07 1e-04 3e-09 6e-07 3e-07 9: 0.0000e+00 2.8803e-09 1e-06 3e-11 6e-09 3e-09 10: 0.0000e+00 2.8803e-11 1e-08 3e-13 6e-11 3e-11 Optimal solution found. cvxopt status: optimal R0: [ 2.75e+02 4.89e+01 4.08e+01] [ 4.89e+01 1.89e+02 1.22e+01] [ 4.08e+01 1.22e+01 2.28e+03] R1: [ 1.24e+03 2.26e+01 1.77e+02] [ 2.26e+01 4.32e+01 3.08e+00] [ 1.77e+02 3.08e+00 5.49e+02]
Elipse - not dualized and dualized ----------------------- cvxopt CONELP solver ----------------------- pcost dcost gap pres dres k/t 0: 0.0000e+00 -1.0000e+01 4e+01 2e+00 2e+03 1e+00 1: 0.0000e+00 -1.3093e+00 3e+00 3e-01 3e+02 2e-01 2: 0.0000e+00 -2.2983e-01 5e-01 5e-02 6e+01 3e-02 3: 0.0000e+00 -4.2742e-02 7e-02 8e-03 1e+01 2e-03 4: 0.0000e+00 -2.5164e-02 3e-02 5e-03 6e+00 2e-03 5: 0.0000e+00 -4.1015e-03 3e-03 7e-04 9e-01 1e-04 6: 0.0000e+00 -3.0018e-04 2e-04 5e-05 7e-02 1e-05 7: 0.0000e+00 -3.3986e-06 2e-06 6e-07 8e-04 1e-07 8: 0.0000e+00 -3.3966e-08 2e-08 6e-09 8e-06 1e-09 9: 0.0000e+00 -3.3966e-10 2e-10 6e-11 8e-08 1e-11 10: 0.0000e+00 -3.3966e-12 2e-12 6e-13 8e-10 1e-13 Optimal solution found. cvxopt status: optimal R0: [-1.79e-01 -3.88e-03 -2.60e-02] [-3.88e-03 -9.61e-03 -7.08e-04] [-2.60e-02 -7.08e-04 -1.20e-01] R1: [ 1.83e-01 4.00e-03 2.67e-02] [ 4.00e-03 1.00e-02 7.31e-04] [ 2.67e-02 7.31e-04 1.25e-01] 




In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:



    






    











In [ ]:

Content source: ipa-lth/jupyter_nb

Similar notebooks:

notebook.community | gallery | about