Algorithms-Python



In [1]:
import numpy as np

In [2]:
## Define the Area Fit Method
def Area_Fit(y,u,t):
    """Returns a FOTD Model from the given data.
    y - array of outputs
    u - array of inputs
    t - array of time values
    """
    # Truncate for Maximum
    i_end = abs(y).argmax(axis=0)
    yp = y[0:i_end+1]
    up = u[0:i_end+1]
    tp = t[0:i_end+1]
    # Get Gain
    KM = (yp[-1]-yp[0])/(up[-1])
    # Get the Residence Time
    Tar = 1/KM * np.trapz(yp[-1]-yp,tp)
    # Time Constant
    T = np.exp(1)/KM*np.trapz(yp[np.where(tp<=Tar)],tp[np.where(tp<=Tar)])
    # Delay
    L = Tar-T
    # Check if all arguments are valid
    if (T < 0):
        print("Error - Negative lag")
        return -1
    if (L < 0):
        if (L < 1e-2):
            L = 0
        else:    
            print("Error - Negative delay")
            return -1
    return KM,T,L

In [3]:
# Define the AMIGO Tuning
def AMIGO_Tune(K,T,L, structure = 'PI'):
    """Computes the PI(D) controller parameter based on AMIGO algorithm;
       Parameter are returned as parallel notation KP,KI,KD and set point;
       Needs first order time delay parameter as input
    """
    # Check for small delay
    if L < 1e-1:
        L = 1e-1
    # PI Controller
    if structure == 'PI':
        # Parameter as Defined in Aström et. al., Advanced PID Control,p.229 
        KP = 0.15/K + (0.35 - L*T /(L+T)**2)*T/(K*L)
        TI = 0.35*L+(13*L*T**2)/(T**2+12*L*T+7*L**2)
        TD = 0.0
        # Set Point Weight, as given on p.235
        if L/(T+L) > 0.5:
            b = 1
        else:
            b = 0.0
        
    elif structure == 'PID':
        KP = 1/K*(0.2+0.45*T/L)
        TI = (0.4*L + 0.8*T)/(L+0.1*T)*L
        TD = (0.5*L*T)/(0.3*L+T)
        # Set Point Weight, Derived from Fig. 7.2, p. 230
        if L/(T+L) < 0.2:
            b = 0.4
        elif L/(T+L) > 0.3:
            b = 1.0
        else:
            # Approximate as Linear Function
            b = 0.4 + (1.0 - 0.4)/(0.3-0.2)*L/(T+L)
    else:
        print("Undefined controller Structure")
        return np.NaN
    KI = KP/TI
    KD = KP*TD
    return [KP,KI,KD],b

# Define AMIGO Detuning
def AMIGO_DETUNE(K,T,L,params,KP, MS = 1.4, structure = 'PI'):
    """Detunes the AMIGO parameter according to Astrom"""
    # Check for small delay
    if L < 1e-1:
        L = 1e-1
    # Needed Parameter
    alpha_D = (MS-1)/MS # See p.255 Eq. 7.19
    beta_D = MS*(MS+np.sqrt(MS**2-1))/2# See p.257 Eq. 7.24
    
    # Define old set of parameter
    KP0 = params[0]
    KI0 = params[1]
    KD0 = params[2]
    
    if structure=='PI':
        # Needed constrain for switch case,See p. 258 Eq. 7.27
        c = KP*K - KP0*K*(L+T)/(beta_D*(alpha_D+KP*K)) - alpha_D
        if c < 0:
            KI = beta_D*(alpha_D+KP*K)**2/(K*(L+T))
        else:
            KI = KI0*(alpha_D+KP*K)/(alpha_D+KP0*K)
        return [KP,KP/KI,0.0]
    if structure == 'PID':
        print("Not implemented")
        return np.NaN
    else:
        print("Undefined controller Structure")
        return np.NaN

In [4]:
def RGA(K,T,L,w=0):
    """Takes a FOTD System and computes the RGA of the system"""
    if (K.shape != T.shape) or (K.shape != L.shape) or (L.shape != T.shape):
        print("Shapes of parameter array are not equal!")
    # Compute the System
    G = np.absolute(FOTD_IMAG(K,T,L,w))
    # Calculate the RGA
    RGA = np.multiply(G, np.transpose(np.linalg.inv(G)))
    return RGA

In [5]:
def FOTD_IMAG(K,T,L,w=0):
    """Creates a FOTD MIMO system from the parameter matrices and the current frequency"""
    # Check if all dimensions match
    if (K.shape != T.shape) or (K.shape != L.shape) or (L.shape != T.shape):
        print("Shapes of parameter array are not equal!")
        return np.NaN
    # System Dimension
    if K.ndim == 1:
        # Using system Identity by multiplying with the complex conjugate
        G = 1/(T**2 * w**2 +1)*(K-1j*T*w)*(np.cos(-L*w)+1j*np.sin(-L*w))
    else:
        outputs,inputs = K.shape
        # Create a system within the complex numbers
        G = np.zeros_like(K)
        for i in range(0,inputs):
            for o in range(0,outputs):
                # Using system Identity by multiplying with the complex conjugate
                G[o][i] = 1 /(T[o][i]**2 * w**2 +1) * ( K[o][i] - 1j*T[o][i]*w) *(np.cos(-L[o][i]*w)+1j*np.sin(-L[o][i]*w))
    return G

In [6]:
def Control_Decentral(K,T,L, w = 0, b=np.empty, structure = 'PI'):
    """ Computes decentralised controller with AMIGO algorithm based on RGA pairing"""
    # Compute SISO Case
    if K.ndim <= 1:
        # Using system Identity by multiplying with the complex conjugate
        params, b0 = AMIGO_Tune(K,T,L)
        # If b is not given, use b from AMIGO
        if b == np.empty:
            Kr = [b0*params[0], params[1], params[2]]
            Ky = params
        else:
            Kr = [b*params[0],params[1],params[2]]
            Ky = params
    # Compute general MIMO Case
    else:
        # Systems dimensions
        outputs,inputs = K.shape
        # Create an empty controller
        Ky = np.zeros([outputs,inputs,3])
        Kr = np.zeros([outputs,inputs,3])
        # Compute RGA -> Checks for Shape
        LG = RGA(K,T,L,w)
        # Get Pairing as an array for every column
        Pairing = np.argmax(LG, axis=0)
        # Iterate through the pairing
        for o in range(0,outputs):
            # Best Pairing
            i = Pairing[o]
            # Compute controller via recursion
            Ky[o][i],Kr[o][i] = Control_Decentral(K[o][i],T[o][i],L[o][i],b)
    return Ky, Kr

In [7]:
def Control_Astrom(K,T,L,H, MS= None, w = 0, b=np.empty, structure = 'PI'):
    """Computes a Decoupling Controller via Aström Algortihm based on FOTD"""
    # Check Input for Maximum Sensitivity
    if MS is None:
        MS = 1.4*np.eye(K.shape[0],K.shape[1])
    # Compute Determinant of Maximum Sensitivity
    ms = np.linalg.det(MS)
    # Compute SISO Case
    if K.ndim <= 1:
        return Control_Decentral(K,T,L,w,b,structure)
    # Compute General MIMO Case
    else:
        # Systems dimensions
        outputs,inputs = K.shape
        # Check dimensions
        if (K.shape != T.shape) or (K.shape != H.shape) or (K.shape != MS.shape) or (K.shape != L.shape) or (L.shape != T.shape):
            print("Shapes of parameter array are not equal!")
            return np.NaN
        
        # Create an empty controller
        Ky = np.empty([outputs,inputs,3])
        Kr = np.empty([outputs,inputs,3])
               
        # Get minimal Delay/ Time Constant for robust limit of crossover frequency, ignore zeros
        if (L[np.where(L>0)].size !=0) or (T[np.where(T>0)].size !=0):
            if (L[np.where(L>0)].size !=0):
                wc_min = np.min([np.min(L[np.nonzero(L)]),np.min(T[np.nonzero(T)])])
            else:
                wc_min = np.min(np.min(T[np.nonzero(T)]))
        else:
            # Use very high frequency
            wc_min = 1e10
        
        # Compute the decoupler
        D = np.linalg.inv(K)
        # Compute the interaction indeces
        # Since d/ds(Q*K) = d/ds(Q)*K = d/ds(G) we can write the Taylor coefficient
        Gamma = np.abs(np.dot(np.multiply(-K,T+L),D))
        # Set main diagonal to zero
        np.fill_diagonal(Gamma,0)
        # Get the maximum of each row
        GMax = np.argmax(Gamma,axis=0)
        # Iterate through the outputs 
        for o in range(0,outputs):
            # Estimate the new system parameter
            # Get the maximal gain
            l = np.max(L[o][:])
            # Add the systems gain -> scaled
            k = np.dot(K[o][:],D[:][o])
            # Get the system time constant
            t = np.sum(np.multiply(np.multiply(K[o][:],D[:][o]),T[o][:]+L[o][:]))/k - l
            
            # Design a controller based on estimated system
            ky,kr = Control_Decentral(k,t,l,w,b,structure)
            
            # Test for Interaction
            
            # Current maximum interaction
            gmax = Gamma[o][GMax[o]]
            # Check for set point weight, either given
            if b == np.empty:
                # Or computed from AMIGO_TUNE
                b = kr[0]/ky[0]
            # Check for structure
            if structure == 'PI':
                # Set counter for while
                counter=0
                # Set shrinking rate in accordance with golden ratio
                shrink_rate = 0.618
                while (np.abs(H[o][o]/(ms*gmax)) - np.sqrt( (b*ky[0]/wc_min)**2 + ky[1]**2 ) < 0):
                    if counter > 10:
                        #print('Maximal Iteration for detuning reached! Abort')
                        break
                    # Detune the controller with the shrinking rate    
                    ky = AMIGO_DETUNE(k,t,l,ky,shrink_rate*ky[0])
                    # Increment counter
                    counter += 1
                # Get the controller parameter
                Ky[o][o][:] = ky
                Kr[o][o][:] = [b*ky[0], ky[1], ky[2]]
        # Rescale controller for real system
        for c in range(0,2):
            Ky[:][:][c] = np.dot(D,Ky[:][:][c])
            Kr[:][:][c] = np.dot(D,Kr[:][:][c])
        return Ky,Kr

Test Area

Testing the Algorithm on robustness etc


In [8]:
# Import the needed packages
# python control for LTI systems
import control as cn
# Import bokeh for Ploting
import bokeh.plotting as bk
import bokeh.io as bi
bi.output_notebook()


Loading BokehJS ...

SISO Test

Testing Identification / AMIGO Tuning / AMIGO Detuning


In [9]:
# Test Identification
# Make a system
num = [1,0,20]
den = [50,10,1]
G = cn.tf(num,den)
G = G*G
# Step response
t = np.linspace(0,100,500)
y,t = cn.matlab.step(G,t)
# Add noise
y = y +np.random.normal(0,0.3,np.size(y))
u = np.ones_like(y)
# Parameter
K,T,L = Area_Fit(y,u,t)
num,den = cn.pade(L,15)
GM = cn.tf([K],[T,1])*cn.tf(num,den)
yM,tM = cn.matlab.step(GM, t)
# Plot
p1 = bk.figure()
p1.line(t,y, line_color= "tomato")
p1.line(tM,yM)
p1.circle(t[abs(y).argmax(axis=0)],y[abs(y).argmax(axis=0)], fill_color='orange', size=10)
bk.show(p1)

# Calculate the controller
params, b = AMIGO_Tune(K,T,L)
ky = cn.tf([params[0]],[1])+cn.tf([params[1]],[1,0])
kr = cn.tf([params[1]],[1,0])
cl =cn.feedback(G,ky)*kr
yc,tc = cn.step(cl)
# Plot
p2 = bk.figure()
p2.line(tc,yc, line_color= "tomato")
bk.show(p2)

# Find the Maximum Sensitivity
# Compute sensitivity
S = 1/(1+G*ky)
# compute the gain of the sensitivity
w = np.logspace(-5,5,1000)
mag, phase, freq = cn.matlab.bode(S,w)
MS = np.max(mag)

# Make a plot
p3= bk.figure(x_axis_type="log")
p3.line(freq,mag)
bk.show(p3)

#Detune the controller -> DOES NOT WORK!!! WHY!?
params = AMIGO_DETUNE(K,T,L,params, params[0])
ky2 = cn.tf([params[0]],[1])+cn.tf([params[1]],[1,0])
kr2 = cn.tf([params[1]],[1,0])
cl2 =cn.feedback(G,ky2)*kr2
yc2,tc2 = cn.step(cl2)
# Plot
p4 = bk.figure()
p4.line(tc,yc)
p4.line(tc2,yc2, line_color= "tomato")
bk.show(p4)


TEST MIMO System


In [10]:
# Make a system
num = [[[3],[10]], [[5],[1]]]
den = [[[10,1],[50,1]],[[20,1],[30,1]]]
#G = cn.tf(num,den)

# Time array
t = np.linspace(0,300,600)
# Store the output
y = np.empty([2,2,t.size])
# Input Array
u = np.ones_like(t)

# Define Parameter
K = np.empty([2,2])
T = np.empty([2,2])
L = np.empty([2,2])
G = [[0 for x in range(2)] for y in range(2)] 

# Identify from step response
for ins in range(0,2):
    for out in range(0,2):
        G[out][ins] = cn.tf(num[out][ins],den[out][ins])
        y[out][ins][:],t = cn.step(G[out][ins],t)
        K[out][ins],T[out][ins],L[out][ins] = Area_Fit(y[out][ins][:],u,t)
    
# Compute the decentralized controller
Ky1,Kr1 = Control_Decentral(K,T,L)
Ky2,Kr2 = Control_Astrom(K,T,L, 0.2*np.eye(2))

#g = G[0][0]
#ky = Ky1[0][0]
#kr = Kr1[0][0]
#ky = cn.tf([ky[0]],[1])+cn.tf([ky[1]],[1,0])
#kr = cn.tf([kr[1]],[1,0])
#cl =cn.feedback(g,ky)*kr
#yc,tc = cn.step(cl)
# Plot
#p4 = bk.figure()
#p4.line(tc,yc)
#bk.show(p4)
#g


/home/alcapitan/anaconda3/lib/python3.5/site-packages/ipykernel/__main__.py:18: ComplexWarning: Casting complex values to real discards the imaginary part

In [11]:
# Test Identification
# Make a system
num = [5,10]
den = [10,20,1]
G = cn.tf(num,den)
# Step response
t = np.linspace(0,100,500)
y,t = cn.matlab.step(G,t)

# Plot
p1 = bk.figure()
p1.line(t,y, line_color= "tomato")
bk.show(p1)



In [4]:
# Modified Detuning
def Control_Decoupled(K,T,L,H, MS= None, w = 0, b=np.empty, structure = 'PI'):
    # Check Input for Maximum Sensitivity
    if MS is None:
        MS = 1.4*np.eye(K.shape[0],K.shape[1])
    
    # Compute Determinant of Maximum Sensitivity
    ms = np.linalg.det(MS)

    # Compute SISO Case
    if K.ndim <= 1:
        return Control_Decentral(K,T,L,w,b,structure)
    
    # Compute General MIMO Case
    else:

        # Compute a decentralized control structure based on RGA
        Ky, Kr = Control_Decentral(K,T,L, w , b, structure)
        
        # Get minimal Delay/ Time Constant for robust limit of crossover frequency, ignore zeros
        # Important note: wc_min is actually 1/w_c !!!
        if (L[np.where(L>0)].size !=0) or (T[np.where(T>0)].size !=0):
            if (L[np.where(L>0)].size !=0):
                wc_min = np.min([np.min(L[np.nonzero(L)]),np.min(T[np.nonzero(T)])])
            else:
                wc_min = np.min(np.min(T[np.nonzero(T)]))
        else:
            # Use very high frequency
            wc_min = 1e10
        
        # Calculate the Pairing
        # Compute RGA -> Checks for Shape
        LG = RGA(K,T,L,w)
        # Get Pairing as an array for every column
        Pairing = np.argmax(LG, axis=0)
        
        # Compute the Taylor Series 
        Gamma =  np.multiply(-K,T+L)

        # Initialize 
        KD = np.zeros_like(Gamma)
        GD = np.zeros_like(Gamma)
        
        # Get the Diagonal entries for decoupling
        for outputs in range(0,K.shape[0]):
            inputs = Pairing[outputs]
            KD[outputs][inputs] = K[outputs][inputs]
            GD[outputs][inputs] = Gamma[outputs][inputs]
    
        # Get the Antidiagonal
        KA = K-KD
        GA = Gamma-GD
        
        # Define the splitter
        S = -np.dot(np.linalg.inv(KD),KA)

        # Get the interaction
        GammaA = np.abs(GA + np.dot(GD,S))
        print(GammaA)
        # Get the maximum of each row 
        GMax = np.argmax(GammaA,axis=0)
        print(GMax)
        #Iterate through the outputs
        for outputs in range(0,K.shape[0]):
            inputs = Pairing[outputs]
            
            # Test the current controller for interaction
            # Every controller has the dimension 3 for kp, ki, kd
            ky = Ky[outputs][inputs]
            kr = Kr[outputs][inputs]
            
            # Get the current parameter
            k = K[outputs][inputs]
            t = T[outputs][inputs]
            l = L[outputs][inputs]
            
            # Check for set point weight, either given
            if b == np.empty:
                # Or computed from AMIGO_TUNE
                b = kr[0]/ky[0]
            print(b)
            gmax = GammaA[outputs][GMax[outputs]]
            print(gmax)
            # Check for PI Structure
            if structure == 'PI':
                # Define the counter
                counter = 0
                # Set shrinking rate 
                shrink_rate = 0.9
                
                print(np.abs(H[outputs][outputs]/(ms*gmax)) - np.sqrt( (b*ky[0]/wc_min)**2 + ky[1]**2 ))
                while (np.abs(H[outputs][outputs]/(ms*gmax)) - np.sqrt( (b*ky[0]/wc_min)**2 + ky[1]**2 ) < 0):
                    if counter > 1e5:
                        #print('Maximal Iteration for detuning reached! Abort')
                        break
                    # Detune the controller with the shrinking rate    
                    ky = AMIGO_DETUNE(k,t,l,ky,shrink_rate*ky[0])
                    # Increment counter
                    counter += 1
                print(counter)
                # Get the controller parameter
                Ky[outputs][inputs][:] = ky
                Kr[outputs][inputs][:] = [b*ky[0], ky[1], ky[2]]

        return Ky,Kr

In [1]:
import Algorithms as alg
import numpy as np
K = np.array([[10,1],[2,11]])
T = np.array([[31,10],[10,51]])
L = np.array([[0,0],[0,0]])
H = np.diag([0.01,0.01])
#Ky,Kr = Control_Decoupled(K,T,L,H)
Ky2,Kr2 = alg.Control_Decoupled(K,T,L,H)

In [4]:
alg.RGA(K,T,L,100)


Out[4]:
array([[-0.28211547,  1.28211547],
       [ 1.28211547, -0.28211547]])

In [5]:
a= np.random.uniform(0,1,(1,1,9))
a


Out[5]:
array([[[ 0.45686713,  0.22132972,  0.89035245,  0.655564  ,  0.67510215,
          0.06877948,  0.86643841,  0.41978976,  0.11611283]]])

In [11]:
a[0][0][:4]


Out[11]:
array([ 0.45686713,  0.22132972,  0.89035245,  0.655564  ])

In [ ]: