In [ ]:
import numpy as np
#from numpy import linalg as LA
from scipy import linalg as LA


import cvxpy
import optim_tools #own file with helper

import control as pc
import matplotlib.pyplot as plt
import itertools

from sysident import loadtools
%pylab inline

In [ ]:
print cvxpy.installed_solvers()
print cvxpy.__version__

In [ ]:
def add_position_state(A0, B0, C0):
    A = np.vstack((C0, A0))
    A = np.hstack((np.zeros((A.shape[0],1)), A))
    
    B = np.vstack((np.zeros((1, B0.shape[1])), B0))
    
    C = np.matrix(np.zeros(B.shape).T)
    C[0,0] = 1
    return A, B, C

#add_position_state(A, B, C)

#def load_with_delay(fname):
#    A, B, C, D = loadtools.getModel(fname)
#    return A, B, C, D
#
#def load_without_delay(fname):
#    res, _ = loadtools.loadNPY(fname)
#    return res['A'], res['B'], res['C'], res['D'], res['delay']

def getX00(x0):
    arrays = []
    X00 = []
    for x in x0:
        arrays.append((x, -x))
    for per in list(itertools.product(*arrays)):
        X00.append(np.matrix(per).T)
    return X00
    
#getX00([1.0, 0.5, 0.025])

In [ ]:
folder = "/home/lth/jupyter_nb/optimization/models/"

ss_id = 3

if ss_id is 1:
    fnames = [folder+"ss1_20180724-082630_poles1_ident_nodelay.npy",
              folder+"ss1_20180724-082630_poles2_ident_pade1_0:036.npy"]
    _Ax, _Bx, _Cx, D = loadtools.getModel(fnames[1])
    A, B, C = add_position_state(_Ax, _Bx, _Cx)
elif ss_id is 2:
    fnames = [folder+"ss2_20180724-082631_poles1_ident_nodelay.npy",
              folder+"ss2_20180724-082631_poles3_ident_pade2_0:036.npy"]
    _Ax, _Bx, _Cx, D = loadtools.getModel(fnames[1])
    A, B, C = add_position_state(_Ax, _Bx, _Cx)
elif ss_id is 5:
    fnames = [folder+"ss5_20180724-082628_poles2_ident_nodelay.npy",
              folder+"ss5_20180724-082628_poles3_ident_pade1_0:032.npy"]
    _Ax, _Bx, _Cx, D = loadtools.getModel(fnames[1])
    A, B, C = add_position_state(_Ax, _Bx, _Cx)
elif ss_id is 6:
    fnames = [folder+"ss6_20180724-082629_poles2_ident_nodelay.npy",
              folder+"ss6_20180724-082629_poles4_ident_pade2_0:032.npy"]
    _Ax, _Bx, _Cx, D = loadtools.getModel(fnames[1])
    A, B, C = add_position_state(_Ax, _Bx, _Cx)
elif ss_id is 3:
    ####################### BEWARE THIS ONE IS DIFFERENT #################################
    fnames = [folder+"ss5_20180724-082628_poles2_ident_nodelay.npy",
              folder+"ss3_position_20180717-104106_poles3_ident_pade1_0:032.npy"]
    A, B, C, D = loadtools.getModel(fnames[1])
    ######################################################################################

_A0x, _B0x, _C0x, D0, delay = loadtools.getDelayModel(fnames[0])
A0, B0, C0 = add_position_state(_A0x, _B0x, _C0x)

u_max = 2.0
U_max = [u_max]

xx0 = [1.0, 2.0,   0.5,  0.025, 0.01]
print 'Limits:', xx0[0:len(B)]

X00 = getX00(xx0[0:len(B)])

# Transform into reachable form to specify boundaries useful
(A1, B1, C1, D1), T1, Q1 = optim_tools.get_Steuerungsnormalform(A, B, C.T, D)
X0 = [T1.dot(x0) for x0 in X00]
#print X0

In [ ]:
## Helper functions for optimization problem

def Tri(i, n_u):
    if n_u>1:
        print "not implemented"
        print "alle permutationen mit einsen auf der hauptdiagonalen!"
        raise Error()
        
    if i==1:
        return np.eye(n_u)
    else:
        # only true for n_u == 1
        return np.zeros((n_u, n_u))

def negTri(i, n_u):
    return np.eye(n_u) - Tri(i, n_u)

In [ ]:
%%time
# Satz 6.6

# Init
n = B.shape[0] # get dim of system
n_u = B.shape[1] 

# Variables
X = cvxpy.Variable((n, n), PSD=True)
Y = cvxpy.Variable((n, n), PSD=True)
W = cvxpy.Variable((n_u, n_u))

Ak_h = cvxpy.Variable(A.shape, PSD=True)
Bk_h = cvxpy.Variable(B.shape)
Ck_h = cvxpy.Variable(C.shape)
Dk_h = cvxpy.Variable(D.shape)

Ch = cvxpy.Variable(C.shape)
Dh = cvxpy.Variable(D.shape)

# Substitutionen
C_hat = lambda i: Tri(i, n_u)*Ck_h + negTri(i, n_u)*Ch
D_hat = lambda i: Tri(i, n_u)*Dk_h + negTri(i, n_u)*Dh

Xi = cvxpy.bmat([[ A*X + B*Ck_h, A + B*Dk_h*C ],
                 [ Ak_h,         Y*A + Bk_h*C ]])

I = np.eye(n)

# Bisection parameter
g = cvxpy.Parameter(nonneg=True)

# Pole restriction
ro = cvxpy.Parameter(nonneg=True) # Real <=-ro
ni = cvxpy.Parameter(nonneg=True) # |Imag| <= ni*Real

#####################################################
ro.value = 100
#ro.value = 50
ni.value = 1
#####################################################
poles = pc.pole(pc.ss(A, B, C, D))
if any(poles.real < -ro.value) or any(poles.imag < ni.value*poles.real):
    print "WARNING: Pole restriction too tight for model?"
    print poles
    
    raw_input("Press Enter to continue...")


# Define Constraints

# (6.39a)
const_639a = cvxpy.bmat([[X, I],
                         [I, Y]]) >> 0
                         #[I, Y]]) == cvxpy.Variable((2*n, 2*n), PSD=True)



# (6.39b)
const_639b = cvxpy.bmat([[ X*A.T + A*X + B*Ck_h + (B*Ck_h).T, Ak_h.T + A + B*Dk_h*C            ],
                         [ cvxpy.Variable((n, n)),          A.T*Y + Y*A + Bk_h*C + (Bk_h*C).T]]) + \
             2*g*cvxpy.bmat([[X, I],
                             [I, Y]]) << 0
                             #[I, Y]]) == -cvxpy.Variable((2*n, 2*n), PSD=True)
    
# (6.39c)
const_639c = [cvxpy.bmat([[ X*A.T + A*X + B*C_hat(i) + (B*C_hat(i)).T, Ak_h.T + A + B*D_hat(i)*C            ],
                          [ cvxpy.Variable((n, n)),                      A.T*Y + Y*A + Bk_h*C + (Bk_h*C).T]]) << 0 for i in range(2, (2**n_u)+1)]
                          #[ cvxpy.Variable((n, n)),                      A.T*Y + Y*A + Bk_h*C + (Bk_h*C).T]]) == -cvxpy.Variable((2*n, 2*n), PSD=True) for i in range(2, (2**n_u)+1)]
    
    
# (6.39d)
const_639d = cvxpy.bmat([[ X,  I,    Ch.T     ],
                         [ I,  Y,    (Dh*C).T ],
                         [ Ch, Dh*C, W        ]]) >> 0
                         #[ Ch, Dh*C, W        ]]) == cvxpy.Variable((2*n+n_u, 2*n+n_u), PSD=True)

const_639e = [W[j,j] <= U_max[j]**2 for j in range(0, n_u)]

const_639f = [ X0[k].T*Y*X0[k] <= 1.0
                            for k in range(0, len(X0))]

const_621a = Xi.T + Xi + ro*cvxpy.bmat([[X, I],
                                        [I, Y]]) >> 0
                                        #[I, Y]]) == cvxpy.Variable((2*n, 2*n), PSD=True)

const_621b = cvxpy.bmat([[ ni*(Xi.T + Xi), Xi.T - Xi ],
                         [ -Xi.T + Xi,     ni*(Xi.T + Xi) ]]) << 0
                         #[ -Xi.T + Xi,     ni*(Xi.T + Xi) ]]) == -cvxpy.Variable((4*n, 4*n), PSD=True)

    
# Collect all constraints
constraints_639 = []
constraints_639.append(const_639a)
constraints_639.append(const_639b)
constraints_639.extend(const_639c)
constraints_639.append(const_639d)
constraints_639.extend(const_639e)
constraints_639.extend(const_639f)

constraints_639.append(const_621a)
constraints_639.append(const_621b)


# Form problem.
prob_66 = cvxpy.Problem(cvxpy.Minimize(0), constraints_639)

# bisection with one solver
#[[X_o, Y_o, W_o,
#  Ak_h_o, Bk_h_o, Ck_h_o, Dk_h_o,
#  Ch_o, Dh_o], g_o] = optim_tools.bisect_max(0, None, prob_66, g, [X, Y, W, Ak_h, Bk_h, Ck_h, Dk_h, Ch, Dh], bisect_verbose=True,
#                                                      bisection_tol=0.01,
#                                                      #solver=cvxpy.CVXOPT, verbose=False,  max_iters=50000)
#                                                      solver=cvxpy.MOSEK, verbose=False)
#                                                      #solver=cvxpy.SCS, max_iters=50000)

# bisection alternative with list of multiple solvers
[[X_o, Y_o, W_o,
  Ak_h_o, Bk_h_o, Ck_h_o, Dk_h_o,
  Ch_o, Dh_o], g_o] = optim_tools.bisect_max2(0, None, prob_66, g, [X, Y, W, Ak_h, Bk_h, Ck_h, Dk_h, Ch, Dh],
                                                      bisect_verbose=True,
                                                      bisection_tol=0.1,
                                                      solvers=[
                                                               (cvxpy.CVXOPT, {'verbose':False}),
                                                               (cvxpy.MOSEK, {'verbose':False}),
                                                               #(cvxpy.SUPER_SCS, {'max_iters':50000, 'verbose':True}),
                                                               #(cvxpy.SCS, {'max_iters':5000, 'warm_start':True, 'verbose':True})
                                                              ])
print "g:", g_o

print
print '-----------------------'
import os import sdpt3glue import random #print g.value print prob_66.solve(solver=cvxpy.CVXOPT, verbose=False) ran = random.randint(0, 100) folder ='/tmp/' # Generate filenames matfile_target = os.path.join(folder, 'matfile_{}.mat'.format(ran)) # Where to save the .mat file to print matfile_target output_target = os.path.join(folder, 'output_{}.txt'.format(ran)) # Where to save the output log print output_target #g.value = 1 result = sdpt3glue.sdpt3_solve_problem(prob_66, sdpt3glue.OCTAVE, matfile_target, output_target=output_target)

In [ ]:
# QR Decomposition for M and N
M, NT = LA.qr(I - X_o.dot(Y_o))
N = NT.T
#assert(np.allclose(I - X_o.dot(Y_o), M.dot(N.T)))

# LU Decomposition for M and N
#M, L, U = LA.lu(I - X_o.dot(Y_o))
#N = L.dot(U).T

# Reconstruction
Ek = -LA.solve(N, Y_o.dot(B))
Dk = np.matrix(Dk_h_o)

Ck = LA.solve(M, (Ck_h_o - Dk.dot(C).dot(X_o)).T).T
#Ck_1 = (Ck_h_o - Dk.dot(C).dot(X_o)).dot(LA.inv(M).T) #Check
#assert(np.allclose(Ck_1, Ck))

Bk = LA.solve(N, Bk_h_o)

temp_alpha = LA.solve(N, (Ak_h_o-Y_o.dot(A).dot(X_o)))
temp_beta = Bk.dot(C).dot(X_o)
Ak = (LA.solve(M, temp_alpha.T) - LA.solve(M, temp_beta.T)).T

#Ak_1 = LA.solve(N, (Ak_h_o-Y_o.dot(A).dot(X_o))).dot(LA.inv(M).T) - Bk.dot(C).dot(X_o).dot(LA.inv(M).T) #Check
#assert(np.allclose(Ak_1, Ak))

In [ ]:
print Ak
print Bk
print Ck
print Dk
print Ek
print u_max
#print dT
print
print A
print B
print C
print D

In [ ]:
import time
import shutil

poles = pc.pole(pc.ss(A, B, C, D))

# Creates new file
fname = fnames[1][:-4]+'_control_{}.npy'.format(time.strftime("%Y%m%d-%H%M%S"),
                                    len(poles))

shutil.copyfile(fnames[1], fname)

loadtools.saveModel(fname, A, B, C, D)
loadtools.saveControl(fname, Ak, Bk, Ck, Dk, Ek, u_max)
loadtools.saveNPY(fname, ro=ro.value, ni=ni.value, X0=X0)

print
#print 'Model:'
#print loadtools.getModel(fname)
#print 'Control:'
#print loadtools.getControl(fname)
#res, _ = loadtools.loadNPY(fname)
#print res

In [ ]:
from numpy.linalg import solve, inv

def control_func(y, s, x, k, A, b, c):
    N = -c.dot(solve(A-b.dot(k.T), b))
    u = solve(N, np.array([s]))-k.T.dot(x)
    return u

class control_func_dyn_out():
    def __init__(self, Ak, Bk, Ck, Dk, Ek, A, B, C, D, umax=None, dT=1e-3):
        self.Ak = Ak
        self.Bk = Bk
        self.Ck = Ck
        self.Dk = Dk
        self.Ek = Ek
        self.umax = umax
        self.dT = dT

        self.z = np.zeros(B.shape)
                
        # Construct Prefilter
        C2 = np.hstack((-C, np.zeros(Ck.shape)))

        A2_u = np.hstack(((A+B.dot(Dk).dot(C)), B.dot(Ck)))
        A2_d = np.hstack((Bk.dot(C), Ak))

        A2 = np.vstack((A2_u, A2_d))
        B2 = np.vstack((B, np.zeros(Bk.shape)))

        self.N = inv(C2.dot(inv(A2)).dot(B2))
        #print self.N
        
    def estimate(self, y, u):
        # already saturated in this case
        #if self.umax is not None:
        #    u = optim_tools.sat(u, self.umax)
        
        z_dot = self.Ak.dot(self.z) + self.Bk.dot(y) + self.Ek.dot(u)
        return self.z + z_dot*self.dT
        
    def regulate(self, y, s, x):
        u = self.N.dot(s)+self.Ck.dot(self.z) + self.Dk.dot(y)

        # Saturate 
        if self.umax is not None:
            u = optim_tools.sat(u, self.umax)
            
        self.z = self.estimate(y, u)
        return u

In [ ]:
# Timeline
T = np.arange(0, 2, 1e-3) 

#s: input, e.g., step function with amplitude of 0.2
#s = np.zeros(len(T));
s = np.ones(len(T))*1;


# Initial state
x0 = np.zeros(B.shape)

In [ ]:
from functools import partial

y0, u0, u0_sat = optim_tools.simulate(A, B, C, D, 
                                   partial(optim_tools.openLoop),
                                   s, T, delay=None, umax=u_max, x0=x0)

y1, u1, u1_sat = optim_tools.simulate(A, B, C, D, 
                                   control_func_dyn_out(Ak, Bk, Ck, Dk, Ek,
                                                        A, B, C, D,
                                                        umax=u_max).regulate,
                                   s, T, delay=None, umax=u_max, x0=x0)

In [ ]:
y2, u2, u2_sat = optim_tools.simulate(A0, B0, C0, D0, 
                                   control_func_dyn_out(Ak, Bk, Ck, Dk, Ek,
                                                        A, B, C, D,
                                                        umax=u_max).regulate,
                                   s, T, delay=delay, umax=u_max, x0=np.zeros(B0.shape))

In [ ]:
pylab.rcParams['figure.figsize'] = (10, 6)

line_s, = plt.plot(T[:], s, 'b', label='reference')
line0, = plt.plot(T[:], np.array(y1[0,:].T), 'r', label='dyn_out')
line1, = plt.plot(T[:], np.array(y2[0,:].T), 'g--', label='dyn_out.delay system')


#first_legend = plt.legend(handles=[line1], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('y')
plt.title('Closed Loop Step Response')
plt.show()


line0, = plt.plot(T, u1, 'r--', label='u1')
line2, = plt.plot(T, u2, 'g--', label='u2')



#>first_legend = plt.legend(handles=[line1, line2, line1b, line2b], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('u')
plt.title('Output values')
plt.show()


loadtools.saveNPY(fname, plot_control_step_y=(T[:], np.array(y1[0,:].T)),
                         plot_control_step_u=(T, u1),
                         plot_control_step_delay_y=(T[:], np.array(y2[0,:].T)),
                         plot_control_step_delay_u=(T, u2),
                         plot_control_step_s=(T[:], s))

In [ ]:


In [ ]:


In [ ]: