``````

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
import shutil
import time

``````
``````

In [ ]:

print cvxpy.installed_solvers()
print cvxpy.__version__

``````
``````

In [ ]:

# Append integrator after velocity model
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

# Gets all permutations (pos, neg) for boundary conditions
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])

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

## Simulation functions
from numpy.linalg import solve, inv

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

``````
##### Only a specific test ##### # List of files to be used as synthesis model fileX = "/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_10/iekf_microssim_nonoise/20181203-200402_cplx.0_real.3.npy" fileY = "/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_10/iekf_microssim_nonoise/20181205-121831_cplx.0_real.3.npy" x = loadtools.getIEKF(fileX) y = loadtools.getIEKF(fileY) assert y.keys() == x.keys() assert x.keys() == y.keys() for v in x.keys(): assert(np.array_equal(y[v], x[v])) print "-> Identical IEKF Data"
``````

In [ ]:

folder_tony_models = "/home/lth/jupyter_nb/optimization/models/"

folder_acc2 = '/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_2/'
folder_acc10 = '/home/lth/catkin_ws/src/pitasc_apps/modelbased-ctrl/sysident/sys_models/iekf_logs/acc_10/'

fnames_iekf = [
folder_acc2+"iekf_microssim_highnoise/20181206-090206_cplx.0_real.3.npy",
folder_acc2+"iekf_microssim_midnoise/20181206-090250_cplx.0_real.3.npy",
folder_acc2+"iekf_microssim_midnoise_nodelay/20181206-090325_cplx.0_real.3.npy",
folder_acc2+"iekf_microssim_nonoise/20181206-085840_cplx.0_real.3.npy",
folder_acc2+"iekf_microssim_nonoise_nodelay/20181206-090511_cplx.0_real.3.npy",
folder_acc10+"iekf_microssim_highnoise/20181206-091132_cplx.0_real.3.npy",
folder_acc10+"iekf_microssim_midnoise/20181206-091201_cplx.0_real.3.npy",
folder_acc10+"iekf_microssim_midnoise_nodelay/20181206-091227_cplx.0_real.3.npy",
folder_acc10+"iekf_microssim_nonoise/20181205-121831_cplx.0_real.3.npy",
folder_acc10+"iekf_microssim_nonoise_nodelay/20181206-091253_cplx.0_real.3.npy",
]

fnames_powell = [
]
### TODO: Iterate over list
#fname = fnames_iekf[0]
###

# Get one sample
#_Ax, _Bx, _Cx, D = loadtools.getIEKFModel(fnames_iekf[0])
#A, B, C = add_position_state(_Ax, _Bx, _Cx)

``````
``````

In [ ]:

# Eigenvalue restrictions
eig_ro = 2000 # Real <=-ro
eig_ni = 0.1  # |Imag| <= ni*Real

# U_max
u_max = 2.0
U_max = [u_max]

# *not* transformed Limitations [pos, vel, acc, jerk]
xx0 = [1.0, 2.0, 5, 1]

store_files = False

``````
``````

In [ ]:

#%%time

for fname in fnames_powell:
try:
####################################
# Load the model                   #
####################################
try:
_Ax, _Bx, _Cx, D = loadtools.getIEKFModel(fname)
except:
_Ax, _Bx, _Cx, D = loadtools.getModel(fname)
A, B, C = add_position_state(_Ax, _Bx, _Cx)

print A, B, C, D
####################################
# Define and transform limits      #
####################################
print 'Limits:', xx0[0:len(B)] # Limit is cut to appropriate length
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]

####################################
# Initialize Optimisation Problem  #
####################################
#### 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 = eig_ro
ni.value = eig_ni

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:", 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)

####################################
# Solve the problem                #
####################################
# 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':50000, 'warm_start':True, 'verbose':True})
])
print "g:", g_o

####################################
# Reconstruct the parameters       #
####################################
# 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))

####################################
# Output the results               #
####################################
print Ak
print Bk
print Ck
print Dk
print Ek
print u_max
#print dT
print
print A
print B
print C
print D

####################################
# Simulation of the step response  #
####################################
# 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)

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)

####################################
# Plot the step response           #
####################################
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')

#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')

#>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()

if store_files:
####################################
# Store in npy file                #
####################################
poles = pc.pole(pc.ss(A, B, C, D))
#print poles

# Creates new file
fname_new = fname[:-4]+'_control_{}.npy'.format(time.strftime("%Y%m%d-%H%M%S"),
len(poles))
# Copy to not destroy something
shutil.copyfile(fname, fname_new)

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

plot_control_step_u=(T, u1),
plot_control_step_s=(T[:], s))
except Exception as e:
print e
print fname

print
print '-----------------'

``````
import os import sdpt3glue import random g.value = 1 print g.value print prob_66.solve(solver=cvxpy.MOSEK, 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 [ ]:

``````
``````

In [ ]:

``````
``````

In [ ]:

``````
``````

In [ ]:

``````