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 '-----------------------'
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 [ ]: