Exemple #1
0
def compose_transfer_functions(ba0,ba1):
    b0=ba0[0]
    b1=ba1[0]
    a0=ba0[1]
    a1=ba1[1]
    b = scipy.polymul(b0,b1)
    a = scipy.polymul(a0,a1)
    return (b,a)
Exemple #2
0
 def __init__(self, kind):
     assert kind in ["A", "B", "C"]
     if kind == "A":
         f1 = 20.598997
         f2 = 107.65265
         f3 = 737.86223
         f4 = 12194.217
         A1000 = 0.17
         numerator = [(2 * np.pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0, 0]
         denominator = sp.polymul([1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
                                  [1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
         denominator = sp.polymul(
             sp.polymul(denominator, [1, 2 * np.pi * f3]),
             [1, 2 * np.pi * f2])
     if kind == "B":
         f1 = 20.598997
         f2 = 158.5
         f4 = 12194.217
         A1000 = 1.9997
         numerator = [(2 * np.pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0]
         denominator = sp.polymul([1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
                                  [1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
         denominator = sp.polymul(denominator, [1, 2 * np.pi * f2])
     if kind == "C":
         f1 = 20.598997
         f4 = 12194.217
         C1000 = 0.0619
         numerator = [(2 * np.pi * f4)**2 * (10**(C1000 / 20.0)), 0, 0]
         denominator = sp.polymul(
             [1, 4 * np.pi * f4, (2 * np.pi * f4)**2.0],
             [1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
     self.filter = sps.bilinear(numerator, denominator, 16000)
Exemple #3
0
def lint_trap( cx, cy, d, a, b, m1, b1, m2, b2 ) :
    """
    computes the integral :
        \int_{x=a}^b
            int_{y=m1*x+b1}^{m2*x+b2}
                f(x,y) = ( c1*x + c2*y + d )
            dy
        dx
    using scipy polyint / polymul / polyval;
    the closed form given by Wolfram Alpha is surprisingly ugly, so I've broken the integral down into stages
    """
    # polynomial representations (in variable x) of the inner integral bounds
    fx = np.array( [ m1, b1 ] )
    gx = np.array( [ m2, b2 ] )
    # representation of the part of f which is a polynomial in x [alone]
    px = np.array( [ cx, d ] )
    
    # result of inner integral of c2*y
    term1 = .5 * cy * sp.polysub( sp.polymul( gx, gx ), sp.polymul( fx, fx ) )
    term2 = sp.polymul( px, gx - fx )
    Px = sp.polyint( sp.polyadd( term1, term2 ) )
    
    #print fx, gx, px, term1, term2, term1+term2, Px
    return sp.polyval( Px, b ) - sp.polyval( Px, a )
def lint_trap( cx, cy, d, a, b, m1, b1, m2, b2 ) :
    """
    computes the integral :
        \int_{x=a}^b
            int_{y=m1*x+b1}^{m2*x+b2}
                f(x,y) = ( c1*x + c2*y + d )
            dy
        dx
    using scipy polyint / polymul / polyval;
    the closed form given by Wolfram Alpha is surprisingly ugly, so I've broken the integral down into stages
    """
    # polynomial representations (in variable x) of the inner integral bounds
    fx = np.array( [ m1, b1 ] )
    gx = np.array( [ m2, b2 ] )
    # representation of the part of f which is a polynomial in x [alone]
    px = np.array( [ cx, d ] )
    
    # result of inner integral of c2*y
    term1 = .5 * cy * sp.polysub( sp.polymul( gx, gx ), sp.polymul( fx, fx ) )
    term2 = sp.polymul( px, gx - fx )
    Px = sp.polyint( sp.polyadd( term1, term2 ) )
    
    #print fx, gx, px, term1, term2, term1+term2, Px
    return sp.polyval( Px, b ) - sp.polyval( Px, a )
ts = 10e-3

gss1 = ss(A,B,C,D)
gss = ss(A,B,C2,D2)
gz = c2d(gss,ts,'zoh')

# Control design
wn = 10
xi1 = np.sqrt(2)/2 
xi2 = np.sqrt(3)/2 
xi2 = 0.85

cl_p1 = [1,2*xi1*wn,wn**2]
cl_p2 = [1,2*xi2*wn,wn**2]
cl_p3 = [1,wn]
cl_poly1 = sp.polymul(cl_p1,cl_p2)
cl_poly = sp.polymul(cl_poly1,cl_p3)
cl_poles = sp.roots(cl_poly)     # Desired continous poles
cl_polesd = sp.exp(cl_poles*ts)  # Desired discrete poles

# Add discrete integrator for steady state zero error
Phi_f = np.vstack((gz.A,-gz.C*ts))
Phi_f = np.hstack((Phi_f,[[0],[0],[0],[0],[1]]))
G_f = np.vstack((gz.B,zeros((1,1))))

# Pole placement
k = place(Phi_f,G_f,cl_polesd)

# Observer design - reduced order observer
poli_of = 5*sp.roots(cl_poly1)     # Desired continous poles
poli_o = 5*cl_poles[0:2]
Exemple #6
0
d=[0];

sysc=ss(a,b,c,d)                # Continous state space form

Ts=0.01                         # Sampling time
sys = c2d(sysc,Ts,'zoh')       # Get discrete state space form

# Control system design
# State feedback with integral part

wn=6
xi=np.sqrt(2)/2

cl_p1=[1,2*xi*wn,wn**2]
cl_p2=[1,wn]
cl_poly=sp.polymul(cl_p1,cl_p2)
cl_poles=sp.roots(cl_poly);  # Desired continous poles
cl_polesd=sp.exp(cl_poles*Ts)  # Desired discrete poles

sz1=sp.shape(sys.A);
sz2=sp.shape(sys.B);

# Add discrete integrator for steady state zero error
Phi_f=np.vstack((sys.A,-sys.C*Ts))
Phi_f=np.hstack((Phi_f,[[0],[0],[1]]))
G_f=np.vstack((sys.B, np.zeros((1,1))))

k=place(Phi_f,G_f,cl_polesd)

#Reduced order observer
p_oc=-10*max(abs(cl_poles))
Exemple #7
0
d = [0]

sysc = ss(a, b, c, d)  # Continous state space form

Ts = 0.01  # Sampling time
sys = c2d(sysc, Ts, 'zoh')  # Get discrete state space form

# Control system design - Wheel position
# State feedback with integral part

wn = 5
xi = np.sqrt(2) / 2

cl_p1 = [1, 2 * xi * wn, wn**2]
cl_p2 = [1, wn]
cl_poly = sp.polymul(cl_p1, cl_p2)
cl_poles = sp.roots(cl_poly)
# Desired continous poles
cl_polesd = sp.exp(cl_poles * Ts)  # Desired discrete poles

sz1 = sp.shape(sys.A)
sz2 = sp.shape(sys.B)

# Add discrete integrator for steady state zero error
Phi_f = np.vstack((sys.A, -sys.C * Ts))
Phi_f = np.hstack((Phi_f, [[0], [0], [1]]))
G_f = np.vstack((sys.B, np.zeros((1, 1))))

k = rp.place(Phi_f, G_f, cl_polesd)

#Reduced order observer