In [155]:

    

%pylab inline
import control
import scipy.linalg


    

    




Populating the interactive namespace from numpy and matplotlib






    




WARNING: pylab import has clobbered these variables: ['sys', 'gamma']
`%matplotlib` prevents importing * from pylab and numpy

In [156]:

    

eps = 0.000001
A = array([[0, 1], [-1, -1]])
B = array([[0],[1]])
C = eye(2)
D = array([[0],[0]])
sys = control.ss(A, B, C, D)
sys.damp()


    

    
Out[156]:





(array([ 1.,  1.]),
 array([ 0.5,  0.5]),
 array([-0.5+0.8660254j, -0.5-0.8660254j]))

In [157]:

    

K = control.care(sys.A, sys.B, eye(2), eye(1))[2]
sys_closed = control.feedback(sys, control.ss(0,[0,0],0,K))
sys_closed.damp()


    

    
Out[157]:





(array([ 1.18920712,  1.18920712]),
 array([ 0.70710678,  0.70710678]),
 array([-0.84089642+0.84089642j, -0.84089642-0.84089642j]))

In [176]:

    

Q = diag([1, -1])
R = 0.01*eye(1)
P = scipy.linalg.solve_continuous_are(sys.A, sys.B, Q, R)
K_destab = -linalg.inv(R).dot(B.T).dot(P)
sys_destab = control.feedback(
    sys_closed, control.ss(0,[0,0],0,K_destab))
t, y, x = control.forced_response(
    sys_destab,
    T=linspace(0,10, 1000), X0=[0.001,0])
plot(t, x.T)
u = K_destab.dot(x)
plot(t, u.T, '--')
sys_destab.damp()


    

    
Out[176]:





(array([ 1.96850431,  0.71328852]),
 array([ 1.,  1.]),
 array([-1.96850431, -0.71328852]))






    

In [5]:

    

Ac = sys_closed.A
Bc = sys_closed.B
Ac.T.dot(P) + P.dot(Ac) - P.dot(Bc).dot(linalg.inv(R)).dot(Bc.T).dot(P) + Q


    

    
Out[5]:





matrix([[ -1.44328993e-15,  -1.11022302e-16],
        [  2.88657986e-15,   8.88178420e-16]])

In [10]:

    

P = scipy.linalg.solve_continuous_are(Ar, Br, Q, R)
P


    

    
Out[10]:





array([[  5.99999200e-09,  -3.99999800e-03],
       [  1.10000025e-14,  -4.00001000e-09]])

In [11]:

    

Ar.T.dot(P) + P.dot(Ar) - P.dot(Br).dot(linalg.inv(R)).dot(Br.T).dot(P) + Q


    

    
Out[11]:





array([[ -5.39801537e-12,   1.28056854e-09],
       [  3.00001109e-12,   2.22044605e-15]])

In [153]:

    

A = array([[-1]])
B = array([[1]])
C = array([[1]])
D = array([[1]])
Q=array([[-1]])
R=array([[1]])
sys = control.ss(A, B, C, D)
P = scipy.linalg.solve_continuous_are(A, B, Q, R)
A.T.dot(P) + P.dot(A) - P.dot(B).dot(linalg.inv(R)).dot(B.T).dot(P) + Q


    

    
Out[153]:





array([[-4.]])

In [154]:

    

linalg.inv(array([[1]]))


    

    
Out[154]:





array([[ 1.]])

In [151]:

    

%pylab inline
import picos as pic
import cvxopt as cvx
import numpy as np
import scipy.linalg
import scipy.optimize
import scipy.integrate
def solve_bounded_disturbance(A, B, C, D):
    
    def solve_lmi(A_data, B_data, C_data, D_data, alpha, verbose=False):
        sdp = pic.Problem()

        # shape
        n_x = A_data.shape[0]
        n_u = B_data.shape[1]
        
        # variables
        P = sdp.add_variable('P', (n_x, n_x), vtype='symmetric')
        alpha = pic.new_param('alpha', alpha)
        mu_1 = sdp.add_variable('mu_1')
        mu_2 = sdp.add_variable('mu_2')

        # parameters
        A = pic.new_param('A', cvx.matrix(A_data))
        B = pic.new_param('B', cvx.matrix(B_data))
        C = pic.new_param('C', cvx.matrix(C_data))
        D = pic.new_param('D', cvx.matrix(D_data))
        I_n_u = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(n_u))))
        I_n_x = pic.new_param('I', cvx.sparse(cvx.matrix(np.eye(n_x))))

        eps = 1e-10
        sdp.add_constraint(
            (P*A + A.T*P + 2*alpha*P & P*B) //
            (B.T*P &  -2*alpha*mu_1*I_n_u)  << -eps)
        sdp.add_constraint(
            (C.T*C - P & C.T*D) //
            (D.T*C & D.T*D - mu_2*I_n_u)  << -eps)
        sdp.add_constraint(P >> eps*I_n_x)
        sdp.add_constraint(mu_1 << 0.01)
        sdp.add_constraint(mu_2 << 0.01)
        sdp.set_objective('max', mu_1 + mu_2)
        try:
            sdp.solve(verbose=verbose)
            mu_1 =  sdp.variables['mu_1'].value[0,0]
            mu_2 =  sdp.variables['mu_2'].value[0,0]
            gamma = np.sqrt(mu_1 + mu_2)
        except Exception as e:
            print e
            gamma = -1
            
        return gamma, sdp
    
    # we use fmin to solve a line search problem in alpha for minimum gamma
    print('line search')
    alpha_opt  = scipy.optimize.fmin(lambda alpha: -solve_lmi(A, B, C, D, alpha)[0], x0=-1)
    
    gamma, sdp = solve_lmi(A, B, C, D, alpha_opt)
    print sdp
    
    if sdp.status == 'optimal':
        P = sdp.variables['P'].value
        mu_1 =  sdp.variables['mu_1'].value[0,0]
        mu_2 =  sdp.variables['mu_2'].value[0,0]
        print 'optimal alpha: ', alpha_opt
        print 'gamma: ', gamma
        print 'mu_1: ', mu_1
        print 'mu_2: ', mu_2
        print 'P: ',  P
    else:
        sdp = solve_lmi(A, B, C, D, alpha_opt, verbose=True)
        raise RuntimeError('Optimization failed')
        
    return sdp, P, alpha_opt, gamma


    

    




Populating the interactive namespace from numpy and matplotlib






    




WARNING: pylab import has clobbered these variables: ['gamma']
`%matplotlib` prevents importing * from pylab and numpy

In [152]:

    

A = array([[0,1],[-1,-1]])
B = array([[0],[1]])
C = eye(2)
D = array([[0], [0]])
sdp, P, alpha, gamma = solve_bounded_disturbance(A, B, C, D)


    

    




line search
Warning: Maximum number of function evaluations has been exceeded.
---------------------
optimization problem  (SDP):
5 variables, 0 affine constraints, 17 vars in 5 SD cones

P 	: (2, 2), symmetric
mu_2 	: (1, 1), continuous
mu_1 	: (1, 1), continuous

	maximize mu_1 + mu_2
such that
  [P*A + A.T*P + 2.0*alpha*P,P*B;B.T*P,( ( ( -2.0 )*alpha )*mu_1 )*I] ≼ |-1e-10|
  [C.T*C -P,C.T*D;D.T*C,D.T*D -mu_2*I] ≼ |-1e-10|
  P ≽ ( 1e-10 )*I
  mu_1 ≼ 0.01
  mu_2 ≼ 0.01
---------------------
optimal alpha:  [-1.]
gamma:  nan
mu_1:  -0.119521546231
mu_2:  0.0099999994918
P:  [ 5.74e+00 -1.43e+00]
[-1.43e+00  1.43e+00]

In [ ]: