In [ ]:
import numpy as np

import matplotlib.pyplot as plt
from scipy.optimize import minimize

In [ ]:
def sat(v, u_max):
    return np.clip(v, -u_max, u_max)

def simulate(A, B, C, D, regulator_func, s, T, umax=None, x0=0):
    #intitialize y, u
    y = np.matrix(np.zeros((C.shape[0],len(T))))
    u = np.zeros((len(T),np.size(x0,1)))    
    u_sat = np.zeros((len(T),np.size(x0,1)))
    if type(x0) is int:
        xt = np.matrix([x0]*len(A)).T
        print "x0 = \n{}".format(xt)
    else:
        xt = x0
    #print "y.shape = \n",y.shape
    #print "len(T) = \n",len(T)
    #print "A.shape = \n",(A).shape
    #print "xt.shape = \n",(xt).shape
    #print "B.shape = \n",(B).shape
    #print "u.shape = \n",u.shape
    #print "s.shape = \n",s.shape
    #print "C.T.shape = \n",C.T.shape
    #print "D.shape = \n",D.shape
    
    for i, t in enumerate(T):
        u[[i],:] = regulator_func(y[:,i], s[i], xt)
        
        if umax is not None:
            u_sat[[i],:] = sat(u[[i],:], umax)
        else:
            u_sat[[i],:] = u[[i],:]
        
        x_dot = A.dot(xt) + B.dot(u_sat[[i],:])
        y[:,i] = C.dot(xt) + D.dot(u_sat[[i],:])
        #print "u[[i],:].shape = \n",u[[i],:].shape
        #print "xt = \n",xt
        #print "regulator_func = \n",(regulator_func(y[i], s[i],xt))
        #print "x_dot = \n",x_dot
        #print "(C.T).dot(xt) = \n",((C.T).dot(xt)).shape
        #print " D.dot(u[[i],:]) = \n",(D.dot(u[[i],:])).shape
        #print "(C.T).dot(xt) + D.dot(u[[i],:]) = \n",((C.T).dot(xt) + D.dot(u[[i],:])).shape
        #print "y[[i]] = \n",y[[i]]
        if i < len(T)-1:
            xt = xt + x_dot*(T[i+1]-T[i])
    return y, u, u_sat

In [ ]:
# state-based simulation equation:
# x_dot = A.x + B.u
# y = C.x + D.u
# e.g., u = -(k.T).x. u can be a non-linear 
# ref: Inverted Pendulum: System Modeling
# http://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum&section=ControlStateSpace 

# test a Inverted Pendulum function
M = 0.5;
m = 0.2;
b = 0.1;
I = 0.006;
g = 9.8;
l = 0.3;

p = I*(M+m)+M*m*(l**2); #denominator for the A and B matrices

# State space matries: A, B, C, D
A = np.matrix([[0      ,1              ,0           ,0],
     [0 ,-(I+m*(l**2))*b/p  ,(m**2)*g*(l**2)/p   ,0],
     [0      ,0              ,0           ,1],
     [0 ,-(m*l*b)/p       ,m*g*l*(M+m)/p  ,0]]);

B = np.matrix([[     0],
     [(I+m*(l**2))/p],
          [0],
        [m*l/p],]);

C = np.matrix([[1 ,0 ,0 ,0],
     [0 ,0 ,1 ,0],]);

D = np.matrix([[0],
     [0]]);

# initial X
x0 = np.matrix(np.zeros(A.shape[0])).T

#T: time 
T = np.arange(0, 5, 0.01) 

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

In [ ]:
# Regulator function
def closedLoop(y, s, x):
    # fill-in K matrix euation. Below is just a controller matrix for
    # the Inverted Pendulum pendulum problem
    K = np.array([-70.7107  ,-37.8345  ,105.5298   ,20.9238])
    return s-K.dot(x)

In [ ]:
# opened loop 
inputSignal = lambda y,s,x: s
regulator_func = inputSignal
y, u, u_sat = simulate(A, B, C, D, regulator_func, s, T, x0=x0)
plt.figure()
line1, = plt.plot(T, np.array(y[0,:].T), 'b', label='cart position (m)')
line2, = plt.plot(T, np.array(y[1,:].T), 'g', label='pendulum angle (radians)')
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('Step Responce for Open Loop')
plt.show()

In [ ]:
# closed loop 
regulator_func = closedLoop
y, u, u_sat= simulate(A, B, C, D, regulator_func, s, T, x0=x0)
plt.figure()
line1, = plt.plot(T, np.array(y[0,:].T), 'b', label='cart position (m)')
line2, = plt.plot(T, np.array(y[1,:].T), 'g', label='pendulum angle (radians)')
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('Step Responce for Closed Loop With Controller')
plt.show()
Another DEMO (WSCV)

In [ ]:
# uboot
A = np.matrix([[0., 1., 0.],
              [0., 0., 1.],
              [0., 0., -0.005]])

#last line of Regelungsnormalform (characteristical polynom coefficients) in a colunm vector
a = -A[-1,:].T

B = np.matrix([[0], [0], [1.]])

C = np.matrix([1, 0, 0])

D = np.matrix([0]);

u_max = 2.5e-5

#T: time 
T = np.arange(0, 900, 1) 

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

In [ ]:
# Optimation Results for reulator
a_hat      = np.matrix([[4.4469e-8], [2.3073e-5], [4.9148e-3]])
a_hat_star = np.matrix([[1.073e-7], [4.919e-5], [10.4078e-3]])

R1 = np.matrix([[1.6021e-5, 3.26098e-3, 0.4031],
               [3.2698e-3, 1.5666,     163.46],
               [0.4031,    163.46,     40.713]])

In [ ]:
# Defining functions
def get_g(p, x, R1):
    try:
        p = p.squeeze() # This is weird
    except:
        pass
    D_inv = np.diag([p**-3, p**-2, p**-1])
    g = x.T.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.array([[1],[2],[3]]), R1)

def get_kstar(p, a, a_hat_star):
    try:
        p = p.squeeze() # This is weird
    except:
        pass
    D_inv = np.diag([p**-3, p**-2, p**-1])
    kstar = D_inv.dot(a_hat_star) - a
    return kstar
print "Test kstar\n", get_kstar(2, a, a_hat_star)

In [ ]:
assert np.allclose(get_g(0.1, np.array([[1],[2],[3]]), R1), np.matrix(2086332.877))
assert np.allclose(get_kstar(2, a, a_hat_star), np.matrix([[  1.34125000e-08], [  1.22975000e-05], [  2.03900000e-04]]))

In [ ]:
func_kstar = lambda p: get_kstar(p, a, a_hat_star)
assert np.allclose(func_kstar(0.4), np.matrix([[  1.67656250e-06], [  3.07437500e-04], [  2.10195000e-02]]))

In [ ]:
init_p = 0.0255226957823816

pmin = 0.01
# Initial state
x0 = np.array([[0],[0],[-0.004]])

p_t = np.zeros(len(T))

In [ ]:
def contr_func(y, s, x):
    # fill-in K matrix euation.
    ## Calc p
    #print "x:",x
    func_g = lambda p: np.absolute(get_g(p, x, R1))
    res = minimize(func_g, pmin, method='Nelder-Mead')
    # Saturate if too small
    if res.x < pmin:
        p = pmin
    else:
        p = res.x
    #p_t[t] = p
    #print "p:", p
    
    ## Calc K according to p
    K = func_kstar(p).T

    #print "K:", K
    #print "K.shape:", K.shape
    #print "s:", s
    
    # Calc u
    u = s-K.dot(x)
    
    # Saturate u
    u = sat(u, u_max)
    #print "u", u
    
    return u

In [ ]:
y, u, u_sat = simulate(A, B, C, D, contr_func, s, T, u_max, x0)

In [ ]:
y2, u2, u2_sat = simulate(A, B, C, D, lambda y, s, x: s-func_kstar(0.4).T.dot(x), s, T, u_max, x0)

In [ ]:
plt.figure()
line1, = plt.plot(T, np.array(y[0,:].T), 'b', label='y')
line2, = plt.plot(T, np.array(y2[0,:].T), 'r-', label='y2')

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('Step Response for Closed Loop With Controller')
plt.show()

In [ ]:
plt.figure()
line1, = plt.plot(T, u_sat, 'b', label='u')
line2, = plt.plot(T, u2_sat, 'r-', label='u fixed')
first_legend = plt.legend(handles=[line1, line2], loc=1)
plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('T')
plt.ylabel('y')
plt.title('Output values')
plt.show()

In [ ]:


In [ ]: