Transfer Functions in python

2018-02-27, updated 2019-08-05 Emacs drag-drop pdfs, paste html, custom templatesGroebner Basis for Linear Network Coding in Sage

All of this is based on http://blog.codelv.com/2013/02/control-systems-in-python-part-1.html . From 2013 to 2018, the python control library has improved a lot, so now it is relatively easier to do multiple control operations, such as ploting to root locus. This extends the code from frmdstryr to support discrete time using 'z', instead of only continuous time using 's'.

This also shows an example of how to use it, by solving a problem from a 6.302 lab, a class from MIT.

    INTERACTIVE_PLOT = True
    if INTERACTIVE_PLOT:
        %matplotlib notebook
        pass
    else:
        %matplotlib inline
        pass

    import sympy
    from sympy import *
    sympy.init_printing()
    s = Symbol('s')
    z = Symbol('z')

    from control import matlab
    from control import pzmap

    import matplotlib.pyplot as plt

    DEFAULT_DT = 0.001
    #Converts a polynomial in z to a transfer function
    def tfDiscrete(Ts, dt = DEFAULT_DT, *args, **kwargs):
        tfunc = Ts.simplify() #This is necessary, otherwise you can get float errors (the result is too inexact.)
        num = Poly(tfunc.as_numer_denom()[0],z).all_coeffs()
        den = Poly(tfunc.as_numer_denom()[1],z).all_coeffs()
        tf = matlab.tf(map(float,num),map(float,den), dt)
        return tf

    def tfCont(Ts, *args, **kwargs):
        tfunc = Ts.simplify() #This is necessary, otherwise you can get float errors (the result is too inexact.)
        num = Poly(tfunc.as_numer_denom()[0],s).all_coeffs()
        den = Poly(tfunc.as_numer_denom()[1],s).all_coeffs()
        tf = matlab.tf(map(float,num),map(float,den))
        return tf

    def tf(Ts, dt = DEFAULT_DT, *args, **kwargs):
        if len(Ts.free_symbols) > 1:
            raise ValueError('Too many free variables, ' + str(Ts.free_symbols) +
                             ' A transfer function is a polynomial in only s or z.')
        if len(Ts.free_symbols) < 1:
            raise ValueError('Too few variables.' 
                             'A transfer function is a polynomial in s or z.')
        if z in Ts.free_symbols :
            return tfDiscrete(Ts, dt, *args, **kwargs)
        elif s in Ts.free_symbols:
            return tfCont(Ts, *args, **kwargs)
        else:
            raise ValueError('A transfer function is a polynomial in s or z.'
                            'not one in ' + str(Ts.free_symbols))
    def pole(Ts, dt = DEFAULT_DT, *args,**kwargs):
        return matlab.pole(tf(Ts,dt),*args,**kwargs)

    def rlocus(Ts, dt = DEFAULT_DT, *args,**kwargs):
        plt.figure()
        matlab.rlocus(tf(Ts,dt),*args,**kwargs)
        plt.show()
        
    def bode(Ts, dt = DEFAULT_DT, *args,**kwargs):
        plt.figure()
        matlab.bode(tf(Ts,dt),*args,**kwargs)
        plt.show()
        
    def polezero(Ts, dt = DEFAULT_DT):
        plt.figure()
        pz = pzmap.pzmap(tf(Ts,dt))
        plt.show()
        return pz

    def damp(Ts, dt = DEFAULT_DT, *args,**kwargs):
        return matlab.damp(tf(Ts,dt),*args,**kwargs)
        
    def stepResponse(Ts, dt = DEFAULT_DT, *args,**kwargs):
        plt.figure()
        tfunc = tf(Ts,dt,*args,**kwargs)
        y,t = matlab.step(tfunc,*args,**kwargs)
        if(len(t)==len(y)): # Continuous time
            plt.plot(t,y)
            plt.title("Step Response")
            plt.grid()
            plt.xlabel("time (s)")
            plt.ylabel("y(t)")
            info ="Over Shoot: %f%s"%(round((y.max()/y[-1]-1)*100,2),'%')
            try:
                i10 = next(i for i in range(0,len(y)-1) if y[i]>=y[-1]*.10)
                Tr = round(t[next(i for i in range(i10,len(y)-1) if y[i]>=y[-1]*.90)]-t[i10],2)
            except StopIteration:
                Tr = "unknown"
            try:
                Ts = round(t[next(len(y)-i for i in range(2,len(y)-1) if abs(y[-i]/y[-1])>1.02 or abs(y[-i]/y[-1])<0.98)]-t[0],2)
            except StopIteration:
                Ts = "unknown"

            info += "\nRise Time: %s"%(Tr)
            info +="\nSettling time: %s"%(Ts)
            print info
            plt.legend([info],loc=4)
            plt.show()
        else: #discrete time 
            y = y[0] #unpack value
            t = [x*dt for x in range(len(y))]
            plt.plot(t, y)
            plt.title("Step Response")
            plt.grid()
            plt.xlabel("time (s)")
            plt.ylabel("y[t]")
            info ="Over Shoot: %f%s"%(round((y.max()/y[-1]-1)*100,2),'%')
            try:
                i10 = next(i for i in range(0,len(y)-1) if y[i]>=y[-1]*.10)
                Tr = round(t[next(i for i in range(i10,len(y)-1) if y[i]>=y[-1]*.90)]-t[i10],2)
            except StopIteration:
                Tr = "unknown"
            try:
                Ts = round(t[next(len(y)-i for i in range(2,len(y)-1) if abs(y[-i]/y[-1])>1.02 or abs(y[-i]/y[-1])<0.98)]-t[0],2)
                
            except StopIteration:
                Ts = "unknown"

            info += "\nRise Time: %s"%(Tr)
            info +="\nSettling time: %s"%(Ts)
            plt.legend([info],loc=4)
            plt.show()
    def impulseResponse(Ts, dt = DEFAULT_DT, *args,**kwargs):
        plt.figure()
        tfunc = tf(Ts,dt,*args,**kwargs)
        y,t = matlab.impulse(tfunc,*args,**kwargs)
        if(len(t)==len(y)): # Continuous time
            plt.plot(t,y)
            plt.title("Impulse Response")
            plt.grid()
            plt.xlabel("time (s)")
            plt.ylabel("y(t)")
            plt.show()
        else: #discrete time 
            y = y[0] #unpack value
            t = [x*dt for x in range(len(y))]
            plt.plot(t, y)
            plt.title("Impulse Response")
            plt.grid()
            plt.xlabel("time (s)")
            plt.ylabel("y[t]")
            #info ="Over Shoot: %f%s"%(round((y.max()/y[-1]-1)*100,2),'%')
            #plt.legend([info],loc=4)
            plt.show()
            
    def oscillationPeriod(Ts, dt = DEFAULT_DT, *args,**kwargs):
        a = pole(Ts,0.001)
        return 2*3.1415/max(a, key=lambda x: abs(x)).imag
    Kp = Symbol('Kp')
    Kd = Symbol('Kd')
    Ki = Symbol('Ki')
    p = Symbol('p')
    P = Symbol('P')
    m = Symbol('m')
    gamma = Symbol('y')
    dt = Symbol('dt')

    ha2w = dt/(z-1)
    hc2a = gamma
    hw2th = dt/(z-1)
    hc2ap = (1-p) *(z**-1)/(1-p*z**-1)*gamma

    H = hc2a*ha2w*hw2th
    Hp = hc2ap*ha2w*hw2th

    actuator = H
    actuatorP = Hp

    controller = (Kp+Kd/(dt*m)-Kd/(dt*m)*z**(-m))
    sensor = P

    G=controller*actuator; 
    Gp=controller*actuatorP; 

    sys = G/(1+G*sensor)
    sysP = Gp/(1+Gp*sensor)

    sys = sys.simplify()
    sysP = sysP.simplify()

    pprint(sys)
    print("\n\n ------------------------- \n\n")
    pprint(sysP)
                  ⎛    m                 m⎞         
             dt⋅y⋅⎝Kd⋅z  - Kd + Kp⋅dt⋅m⋅z ⎠         
    ────────────────────────────────────────────────
           ⎛    m                 m⎞      m        2
    P⋅dt⋅y⋅⎝Kd⋅z  - Kd + Kp⋅dt⋅m⋅z ⎠ + m⋅z ⋅(z - 1) 


     ------------------------- 


                              ⎛    m                 m⎞             
                 dt⋅y⋅(p - 1)⋅⎝Kd⋅z  - Kd + Kp⋅dt⋅m⋅z ⎠             
    ────────────────────────────────────────────────────────────────
                   ⎛    m                 m⎞      m                2
    P⋅dt⋅y⋅(p - 1)⋅⎝Kd⋅z  - Kd + Kp⋅dt⋅m⋅z ⎠ + m⋅z ⋅(p - z)⋅(z - 1) 
    #Calculating the value of p
    # N number of steps untill we are 50% done. 1 - p**n = 1/2. p = 1/2.*(1./n)
    n = 65.
    pval = 1/2.**(1/n)
    print pval 

    #We want the period to be 530 when Kp=10 and Kd=1
    #By trial and error:
    subs = {gamma:13.4, 
            Kp:10, 
            Kd:1, 
            Ki:0, 
            m:3, 
            P:1, 
            p:pval,  
            dt:0.001}

    print(oscillationPeriod(sysP.subs(subs),0.001))
    0.989392853996
    530.6524349460201
    # Now we want to find the best Kp and Kd
    # Brute Force approach
    a=[]
    for kp in range(1,40):
        for kd in range(1,40):
            kpp = kp/2.
            kdd = kd/10.
            subs[Kp] = kpp
            subs[Kd] = kdd
            a.append((kpp,kdd,max(abs(pole(sysP.subs(subs),0.001)))))
    kpp, kdd , magnitude = min(a, key=lambda x: x[2])
    print (kpp, kdd , magnitude)

    # Non Bruteforce
    from scipy.optimize import minimize

    def func_min(x):
        kpp = x[0]
        kdd = x[1]
        subs[Kp] = kpp
        subs[Kd] = kdd
        return max(abs(pole(sysP.subs(subs),0.001)))

    x0 = [8, 1]
    res = minimize(func_min, x0, method='nelder-mead', options={'disp': True})
    print res
    (1.5, 0.6, 0.9965544944796353)
        
        Optimization terminated successfully.
             Current function value: 0.996489
             Iterations: 125
             Function evaluations: 237
     final_simplex: (array([[0.30518369, 0.26037023],
           [0.30511368, 0.26035043],
           [0.30511147, 0.26034981]]), array([0.99648941, 0.99648941, 0.99648943]))
               fun: 0.99648940643066
           message: 'Optimization terminated successfully.'
              nfev: 237
               nit: 125
            status: 0
           success: True
                 x: array([0.30518369, 0.26037023])
    # Comparing both results
    print subs
    subs[Kp] = 1.5
    subs[Kd] = 0.6
    stepResponse(sysP.subs(subs), 0.001, T=[x*0.001 for x in range(3000)])
    impulseResponse(sysP.subs(subs), 0.001, T=[x*0.001 for x in range(3000)])
    bode(sysP.subs(subs),0.001)
    print polezero(sysP.subs(subs),0.001)


    subs[Kp] = 0.30533799
    subs[Kd] = 0.26041388
    stepResponse(sysP.subs(subs), 0.001, T=[x*0.001 for x in range(3000)])
    impulseResponse(sysP.subs(subs), 0.001, T=[x*0.001 for x in range(3000)])
    bode(sysP.subs(subs),0.001)
    print polezero(sysP.subs(subs),0.001)
    {Ki: 0, Kd: 0.26041388, m: 3, p: 0.9893928539959366, dt: 0.001, P: 1, Kp: 0.30533799, y: 13.4}

    (array([ 0.99652992+0.00699788j,  0.99652992-0.00699788j,
            0.99650654+0.        j, -0.0297408 +0.        j,
            0.01478363+0.02733623j,  0.01478363-0.02733623j]), array([-0.49875621+0.8638711j, -0.49875621-0.8638711j,
            0.99751243+0.       j]))

    (array([ 0.99648939+0.00019111j,  0.99648939-0.00019111j,
            0.99648941+0.        j, -0.02267371+0.        j,
            0.01129919+0.02054887j,  0.01129919-0.02054887j]), array([-0.49941512+0.86501235j, -0.49941512-0.86501235j,
            0.99883023+0.        j]))

Author: Ivan Tadeu Ferreira Antunes Filho

Date: 2019-11-27 Wed 01:29

Github: github.com/itf

Made with Emacs 27.0.50 (Org mode 9.1.9) and Org export head