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)
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)
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]
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))
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
