In [ ]:
import numpy as np
from __future__ import division
In [ ]:
import picos as pic
import cvxopt as cvx
from numpy import linalg as LA
from scipy.special import comb as nchoosek # n Choose k (n ueber k)
In [ ]:
# Helper function to take the calculation time
import time
class Timer(object):
def __init__(self, name=None):
self.name = name
def __enter__(self):
self.tstart = time.time()
def __exit__(self, type, value, traceback):
if self.name:
print '[%s]' % self.name,
print 'Elapsed: %s' % (time.time() - self.tstart)
Some helper functions needed for the implementation of the optimzation problem
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)
### Seite 35; (4.6)
def _D(v, n):
return np.diag([v**x for x in range(1, n+1)])
def _D_inv(v, n):
return np.diag([v**-x for x in range(1, n+1)])
In [ ]:
# Flipping matrixes to fit Adamy definition
def reverse_x_order(T):
return np.flipud(np.fliplr(T))
n = 3
T1 = np.arange(n**2).reshape((n,n))
t1 = np.matrix(np.arange(n))
print "T1:\n", T1
print reverse_x_order(T1)
print "t1:\n", t1
print reverse_x_order(t1)
Create an arbitary example system using python control toolbox
In [ ]:
# Testsystem aufsetzen
import control as con
# pt2 System
K = 1
d = 0.5
T = 10
delay = 0
a0 = 1
a1 = (2 * d * T) #16
a2 = (T**2) #100
b0 = K
# Polynom
tf_1 = con.matlab.tf(K, [a2, a1, a0])
#print tf_1
# Zustandsraum
#ss_1a = con.matlab.tf2ss(tf_1)
#print ss_1a
# Füge Zeitversatz zu
d_num, d_den = con.pade(delay, 1)
tf_delay = con.tf(d_num, d_den)
ss_delay = con.series(tf_delay, tf_1)
ss_1a = con.matlab.tf2ss(ss_delay)
############################################################
############################################################
############################################################
############################################################
# Sammle Systemteile
A0 = ss_1a.A
b0 = ss_1a.B
C0 = ss_1a.C
d0 = ss_1a.D
# Max output
u_max = 0.1
# Einzugsgebiet (convex)
X0 = [np.matrix([-1, -0.1]).T,
np.matrix([-1, 0.1]).T,
np.matrix([1, -0.1]).T,
np.matrix([1, 0.1]).T]
#print len(X0)
# Transformation in Regelungsnormalform
ss, T = con.canonical_form(con.ss(A0, b0, C0, d0), form='reachable')
A = reverse_x_order(np.matrix(ss.A))
a = -A[-1][:].T #!!!!
b = reverse_x_order(np.matrix(ss.B))
C = reverse_x_order(np.matrix(ss.C))
d = reverse_x_order(np.matrix(ss.D)) # == 0!
print "A:\n", A
print "a:\n", a
print "b:\n", b
#print C
In [ ]:
import picos;picos.tools.available_solvers()
Define and solve optimations problem
In [ ]:
###################################################
# Find Matrix Q=R1^{-1}, Vector z=R1^{-1} * a_hat #
# wrt. Q>>0, (!Pos-Definit, not Semi!) #
# .... #
###################################################
# http://picos.zib.de/v101dev/complex.html
# Data to be available: A, a, b, X0, u_max
# Create (Selection) Matrizes
n = b.size
#print n
l_X0 = len(X0) # Number of edges of convex area (4 most of the cases)
# PICOS Convertions
A_cvx = cvx.matrix(A)
a_cvx = cvx.matrix(a)
b_cvx = cvx.matrix(b)
X0_cvx = [cvx.matrix(x) for x in X0]
N_cvx = cvx.matrix(_N(n))
M_cvx = cvx.matrix(_M(n))
I = cvx.matrix(np.eye(n))
### PICOS Definitions
# Constants
AA = pic.new_param('A', A_cvx)
II = pic.new_param('I', I)
III = pic.new_param('I_2', cvx.matrix(np.eye(n+1)))
aa = pic.new_param('a', a_cvx)
bb = pic.new_param('b', b_cvx)
XX0 = pic.new_param('X0', X0_cvx)
NN = pic.new_param('N', N_cvx)
MM = pic.new_param('M', M_cvx)
# Problem
prob = pic.Problem()
# Parameters
QQ = prob.add_variable('Q', (n, n), vtype='symmetric')
zz = prob.add_variable('z', n)
eps = prob.add_variable('eps', 5, lower=0)
ss = prob.add_variable('s', 1)
gamma = prob.add_variable('gamma', 1)
WW = prob.add_variable('W', (n,n))
# Objective
#prob.set_objective('min', pic.trace(QQ))
#prob.set_objective('min', pic.geomean(QQ))
#prob.set_objective('min', pic.sum([eps[i] for i in range(0, 5)], 'eps(i)', '[0,1,..,4]'))
#prob.set_objective('max', ss)
#prob.set_objective('min', gamma)
#prob.add_constraint(ss > 0.1)
#prob.add_constraint(ss < pic.detrootn(QQ)) # equivalent problem as min: -detrootn
#prob.add_constraint(ss < pic.geomean(QQ)) # equivalent problem as min: geomean
# Strict LMI constraints
prob.add_constraint(eps>0)
# Constraints (Yankulova, Seite 55)
#(4.59)
prob.add_constraint(QQ>>0) ## !!!!! TODO!! PROOF THAT THIS IS OK!!!! !!!!!! -> Make the problem stricter!
#(4.60)
prob.add_constraint(QQ*(AA.T+aa*bb.T)+
(AA+bb*aa.T)*QQ-
zz*bb.T-
bb*zz.T<<-eps[1]*II) ## !!!!! TODO!! PROOF THAT THIS IS OK!!!! !!!!!! -> Make the problem stricter!
## Add symmetry constraint, since (4.60) is not necessary symmetric
# A-B==(A-B).T
#prob.add_constraint(QQ*(AA.T+aa*bb.T)+(AA+bb*aa.T)*QQ-zz*bb.T-bb*zz.T ==
# (QQ*(AA.T+aa*bb.T)+(AA+bb*aa.T)*QQ-zz*bb.T-bb*zz.T).T)
# (Alternative 4.60 - p.61)
#prob.add_constraint(WW.T*WW>>0)
#prob.add_constraint(((QQ*(AA.T+aa*bb.T)+(AA+bb*aa.T)*QQ-zz*bb.T-bb*zz.T) & (QQ*WW) //
# (WW.T*QQ) & (-gamma*II) ) <<0)
#(4.61)
prob.add_constraint(QQ*NN+NN*QQ<<-eps[2]*II) ## !!!!! TODO!! PROOF THAT THIS IS OK!!!! !!!!!!-> Make the problem stricter!
#(4.62)
prob.add_list_of_constraints([((1 & XX0[i].T) //
(XX0[i] & QQ ))>>eps[3]*III
for i in range(0, len(X0))]) ## !!!!! TODO!! PROOF THAT THIS IS OK!!!! !!!!!!
#(4.63)
prob.add_constraint(((u_max-aa.T*QQ*aa+2*aa.T*zz & zz.T) //
(zz & QQ))>>eps[4]*III) ## !!!!! TODO!! PROOF THAT THIS IS OK!!!! !!!!!!
#(4.64)
# m ist die Ordnung des Polynoms p
# m <= 2n-1
m=2*n-1 #???
## BEWARE OF SECOND RANGE ####
prob.add_list_of_constraints(
[pic.sum(
[nchoosek(i, k) * aa.T * _P(0, i-k, n) * QQ * NN * _P(0, k, n) * aa - zz.T * NN * _P(n, i, n) * a
for k in range(0,i)], 'k', '[0,1,..,i]')>=0
for i in range(1, m+1)])
## Linear (in)equalities are understood elementwise.
# The strict operators < and > denote weak inequalities (less or equal than and larger or equal than)
# Output
print prob
prob.solve(verbose=0, solver='cvxopt', solve_via_dual = None)
strict = np.all(np.array(eps.value) > 0)
print "All strict definit:", strict,"\n"
if not strict: print "eps:", eps
print "Q:\n", QQ
print "eig:\n", LA.eigvals(QQ.value)
R1 = LA.inv(QQ.value)
print "R1:\n", R1
print "eig:\n", LA.eigvals(R1)
print "z:\n", zz
a_hat = R1*np.matrix(zz.value)
print "a_hat:\n", a_hat
Some checks for validy of solution
In [ ]:
import numpy as np
print "Checking constraints (no output is ok!)"
eig_459 = np.linalg.eigvals(QQ.value)
if np.any(eig_459 <= 0): print("Q not pos definit: {}".format(eig_459))
eig_460 = np.linalg.eigvals((QQ*(AA.T+aa*bb.T)+(AA+bb*aa.T)*QQ-zz*bb.T-bb*zz.T).value)
if np.any(eig_460 >= 0): print("4.60 not neg definit: {}".format(eig_460))
eig_461 = np.linalg.eigvals((QQ*NN+NN*QQ).value)
if np.any(eig_461 >= 0): print("4.61 not neg definit: {}".format(eig_461))
eig_463 = np.linalg.eigvals(((u_max-aa.T*QQ*aa+2*aa.T*zz & zz.T) // (zz & QQ)).value)
if np.any(eig_463 <= 0): print("4.63 not pos definit: {}".format(eig_463))
Helper functions to run simulation
In [ ]:
# Defining functions
def get_g(p, x, R1):
try:
p = p.squeeze() # This is weird
except:
pass
D_inv = _D_inv(p, n)
# print "p:", p
# print "D_inv:", D_inv
# print "x_T:", x.transpose()
# print "x:", x
# print "R1:", R1
g = x.transpose().dot(D_inv).dot(R1).dot(D_inv).dot(x) - 1.0
# Update 2016: As of python 3.5, there is a new matrix_multiply symbol, @:
# g = x' @ D^-1 @ R1 @ D^-1 @ x - 1.0
return g.squeeze()
print "Test g\n", get_g(0.1, np.matrix(range(0,n)).T, R1)
def get_kstar(p, a, a_hat_star):
try:
p = p.squeeze() # This is weird
except:
pass
D_inv = _D_inv(p, n)
kstar = D_inv.dot(a_hat_star) - a
return kstar
print "Test kstar\n", get_kstar(2, a, a_hat)
def sat(v, u_max, u_min=None):
if not u_min:
u_min = -u_max
return np.clip(v, u_min, u_max)
Simulation of WSVC Control
In [ ]:
from IPython.display import clear_output
from __future__ import division
from scipy.optimize import minimize
#Introduced for fun
u_max_sys = u_max
# Numerical approach
max_T = 120 # Seconds
#simulation time
T = 1.0/100.0
max_iter = int(max_T/T)
# Initial state
x0 = np.array([[0.9],[0]])
# Make functionpointer
func_g = lambda p: np.absolute(get_g(p, x_t, R1))
func_kstar = lambda p: get_kstar(p, a, a_hat)
init_p = 0
p_t = np.zeros(max_iter)
p_t2 = np.zeros(max_iter)
u_t = np.zeros(max_iter)
u_t2 = np.zeros(max_iter)
# Timeloop
x_t = x0
y_t = np.zeros(max_iter)
y_t[0] = x0[0]
with Timer():
for t in range(1, max_iter):
#for t in range(0, 10000):
if t%1000 == 1:
clear_output()
print t*T, "seconds done ->", t/max_iter*100, "%"
## Calc p
res = minimize(func_g, p_t2[t-1], method='Nelder-Mead')
#print res.x
#g_poly = sp.Poly(sys, p)
#show(g_poly)
p = sat(res.x, 1, 0.04)
p_t[t] = res.x
p_t2[t] = p
## Calc u
k_t = func_kstar(p)
u = np.dot(np.array(-k_t.T), x_t)
#print u_sym
u_t[t] = u
u = sat(u, u_max)
u_t2[t] = u
## System response
# System saturation u (trivial if u_max and u_max_sys are identical)
# Calc x_dot
x_dot = np.dot(A, x_t) + b * sat(u, u_max_sys)
x_t = x_t + x_dot*T
y_t[t] = x_t[0]
clear_output()
print "done"
Additional simulation of simple SS Control
In [ ]:
ux_t = np.zeros(max_iter)
ux_t2 = np.zeros(max_iter)
yx_t = np.zeros(max_iter)
# Timeloop
x_t = x0
yx_t[0] = x0[0]
with Timer():
for t in range(1, max_iter):
#for t in range(0, 10000):
if t%1000 == 1:
clear_output()
print t*T, "seconds done ->", t/max_iter*100, "%"
## Calc u
k_t = func_kstar(0.5)
u = np.dot(np.array(-k_t.T), x_t)
#print u_sym
ux_t[t] = u
u = sat(u, u_max)
ux_t2[t] = u
## System response
# System saturation u (trivial if u_max and u_max_sys are identical)
# Calc x_dot
x_dot = np.dot(A, x_t) + b * sat(u, u_max_sys)
x_t = x_t + x_dot*T
yx_t[t] = x_t[0]
clear_output()
print "done"
Plotting
In [ ]:
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = (15, 20)
plt.subplot(311)
plt.plot(np.arange(0, len(y_t))*T, y_t, 'b')
plt.plot(np.arange(0, len(yx_t))*T, yx_t, 'b.')
plt.xlabel('t [s]')
plt.ylabel('y(t)')
plt.subplot(312)
plt.plot(np.arange(0, len(y_t))*T, p_t, 'g--')
plt.plot(np.arange(0, len(y_t))*T, p_t2, 'g')
plt.xlabel('t [s]')
plt.ylabel('p(t)')
plt.subplot(313)
axes = plt.gca()
axes.set_ylim([-u_max-1e-5,u_max+1e-5])
plt.plot(np.arange(0, len(y_t))*T, u_t, 'r--')
plt.plot(np.arange(0, len(y_t))*T, u_t2, 'r')
plt.plot(np.arange(0, len(y_t))*T, ux_t, 'y--')
plt.plot(np.arange(0, len(y_t))*T, ux_t2, 'y')
plt.xlabel('t [s]')
plt.ylabel('u(t)')
plt.show()
In [ ]:
print np.array(eps.value) > 0
(Yet failing) visualtions of Ljaponov regions
In [ ]:
# V(x) = x.T * P * x
def V(x,y,P):
x = np.matrix([x, y]).T
return x.T * P * x
arr = [(x,y,V(x,y,R1)) for x in np.arange(-1, 1, 0.1) for y in np.arange(-1, 1, 0.1)]
import matplotlib.pyplot as plt
plt.plot(arr)
plt.show()
print arr
In [ ]: