diff --git a/external/controls.py b/external/controls.py deleted file mode 100644 index 4e63beb5a..000000000 --- a/external/controls.py +++ /dev/null @@ -1,1508 +0,0 @@ -# controls.py - Ryan Krauss's control module -# $Id: controls.py 30 2010-11-06 16:26:19Z murrayrm $ - -"""This module is for analyzing linear, time-invariant dynamic systems -and feedback control systems using the Laplace transform. The heart -of the module is the TransferFunction class, which represents a -transfer function as a ratio of numerator and denominator polynomials -in s. TransferFunction is derived from scipy.signal.lti.""" - -import glob, pdb -from math import atan2, log10 - -from scipy import * -from scipy import signal -from scipy import interpolate, integrate -from scipy.linalg import inv as inverse -from scipy.optimize import newton, fmin, fminbound -#from scipy.io import read_array, save, loadmat, write_array -from scipy import signal -from numpy.linalg import LinAlgError - -from IPython.Debugger import Pdb - -import sys, os, copy, time - -from matplotlib.ticker import LogFormatterMathtext - -version = '1.1.4' - -class MyFormatter(LogFormatterMathtext): - def __call__(self, x, pos=None): - if pos==0: return '' # pos=0 is the first tick - else: return LogFormatterMathtext.__call__(self, x, pos) - - -def shift(vectin, new): - N = len(vectin)-1 - for n in range(N,0,-1): - vectin[n]=vectin[n-1] - vectin[0]=new - return vectin - -def myeq(p1,p2): - """Test the equality of the of two polynomials based on - coeffiecents.""" - if hasattr(p1, 'coeffs') and hasattr(p2, 'coeffs'): - c1=p1.coeffs - c2=p2.coeffs - else: - return False - if len(c1)!=len(c2): - return False - else: - testvect=c1==c2 - if hasattr(testvect,'all'): - return testvect.all() - else: - return testvect - -def build_fit_matrix(output_vect, input_vect, numorder, denorder): - """Build the [A] matrix used in least squares curve fitting - according to - - output_vect = [A]c - - as described in fit_discrete_response.""" - A = zeros((len(output_vect),numorder+denorder+1))#the +1 accounts - #for the fact that both the numerator and the denominator - #have zero-order terms (which would give +2), but the - #zero order denominator term is actually not used in the fit - #(that is the output vector) - curin = input_vect - A[:,0] = curin - for n in range(1, numorder+1): - curin = r_[[0.0], curin[0:-1]]#prepend a 0 to curin and drop its - #last element - A[:,n] = curin - curout = -output_vect#this is the first output column, but it not - #actually used - firstden = numorder+1 - for n in range(0, denorder): - curout = r_[[0.0], curout[0:-1]] - A[:,firstden+n] = curout - return A - - -def fit_discrete_response(output_vect, input_vect, numorder, denorder): - """Find the coefficients of a digital transfer function that give - the best fit to output_vect in a least squares sense. output_vect - is the output of the system and input_vect is the input. The - input and output vectors are shifted backward in time a maximum of - numorder and denorder steps respectively. Each shifted vector - becomes a column in the matrix for the least squares curve fit of - the form - - output_vect = [A]c - - where [A] is the matrix whose columns are shifted versions of - input_vect and output_vect and c is composed of the numerator and - denominator coefficients of the transfer function. numorder and - denorder are the highest power of z in the numerator or - denominator respectively. - - In essence, the approach is to find the coefficients that best fit - related the input_vect and output_vect according to the difference - equation - - y(k) = b_0 x(k) + b_1 x(k-1) + b_2 x(k-2) + ... + b_m x(k-m) - - a_1 y(k-1) - a_2 y(k-2) - ... - a_n y(k-n) - - where x = input_vect, y = output_vect, m = numorder, and - n = denorder. The unknown coefficient vector is then - - c = [b_0, b_1, b_2, ... , b_m, a_1, a_2, ..., a_n] - - Note that a_0 is forced to be 1. - - The matrix [A] is then composed of [A] = [X(k) X(k-1) X(k-2) - ... Y(k-1) Y(k-2) ...] where X(k-2) represents the input_vect - shifted 2 elements and Y(k-2) represents the output_vect shifted - two elements.""" - A = build_fit_matrix(output_vect, input_vect, numorder, denorder) - fitres = linalg.lstsq(A, output_vect) - x = fitres[0] - numz = x[0:numorder+1] - denz = x[numorder+1:] - denz = r_[[1.0],denz] - return numz, denz - -def prependzeros(num, den): - nd = len(den) - if isscalar(num): - nn = 1 - else: - nn = len(num) - if nn < nd: - zvect = zeros(nd-nn) - numout = r_[zvect, num] - else: - numout = num - return numout, den - -def in_with_tol(elem, searchlist, rtol=1e-5, atol=1e-10): - """Determine whether or not elem+/-tol matches an element of - searchlist.""" - for n, item in enumerate(searchlist): - if allclose(item, elem, rtol=rtol, atol=atol): - return n - return -1 - - - -def PolyToLatex(polyin, var='s', fmt='%0.5g', eps=1e-12): - N = polyin.order - clist = polyin.coeffs - outstr = '' - for i, c in enumerate(clist): - curexp = N-i - curcoeff = fmt%c - if curexp > 0: - if curexp == 1: - curs = var - else: - curs = var+'^%i'%curexp - #Handle coeffs of +/- 1 in a special way: - if 1-eps < c < 1+eps: - curcoeff = '' - elif -1-eps < c < -1+eps: - curcoeff = '-' - else: - curs='' - curstr = curcoeff+curs - if c > 0 and outstr: - curcoeff = '+'+curcoeff - if abs(c) > eps: - outstr+=curcoeff+curs - return outstr - - -def polyfactor(num, den, prepend=True, rtol=1e-5, atol=1e-10): - """Factor out any common roots from the polynomials represented by - the vectors num and den and return new coefficient vectors with - any common roots cancelled. - - Because poly1d does not think in terms of z^-1, z^-2, etc. it may - be necessary to add zeros to the beginning of the numpoly coeffs - to represent multiplying through be z^-n where n is the order of - the denominator. If prependzeros is Trus, the numerator and - denominator coefficient vectors will have the same length.""" - numpoly = poly1d(num) - denpoly = poly1d(den) - nroots = roots(numpoly).tolist() - droots = roots(denpoly).tolist() - n = 0 - while n < len(nroots): - curn = nroots[n] - ind = in_with_tol(curn, droots, rtol=rtol, atol=atol) - if ind > -1: - nroots.pop(n) - droots.pop(ind) - #numpoly, rn = polydiv(numpoly, poly(curn)) - #denpoly, rd = polydiv(denpoly, poly(curn)) - else: - n += 1 - numpoly = poly(nroots) - denpoly = poly(droots) - nvect = numpoly - dvect = denpoly - if prepend: - nout, dout = prependzeros(nvect, dvect) - else: - nout = nvect - dout = dvect - return nout, dout - - -def polysubstitute(polyin, numsub, densub): - """Substitute one polynomial into another to support Tustin and - other c2d algorithms of a similar approach. The idea is to make - it easy to substitute - - a z-1 - s = - ----- - T z+1 - - or other forms involving ratios of polynomials for s in a - polynomial of s such as the numerator or denominator of a transfer - function. - - For the tustin example above, numsub=a*(z-1) and densub=T*(z+1), - where numsub and densub are scipy.poly1d instances. - - Note that this approach seems to have substantial floating point - problems.""" - mys = TransferFunction(numsub, densub) - out = 0.0 - no = polyin.order - for n, coeff in enumerate(polyin.coeffs): - curterm = coeff*mys**(no-n) - out = out+curterm - return out - - -def tustin_sub(polyin, T, a=2.0): - numsub = a*poly1d([1.0,-1.0]) - densub = T*poly1d([1.0,1.0]) - out = polysubstitute(polyin, numsub, densub) - out.myvar = 'z' - return out - - -def create_swept_sine_input(maxt, dt, maxf, minf=0.0, deadtime=2.0): - t = arange(0, maxt, dt) - u = sweptsine(t, minf=minf, maxf=maxf) - if deadtime: - deadt = arange(0,deadtime, dt) - zv = zeros_like(deadt) - u = r_[zv, u, zv] - return u - -def create_swept_sine_t(maxt, dt, deadtime=2.0): - t = arange(0, maxt, dt) - if deadtime: - deadt = arange(0,deadtime, dt) - t = t+max(deadt)+dt - tpost = deadt+max(t)+dt - return r_[deadt, t, tpost] - else: - return t - -def ADC(vectin, bits=9, vmax=2.5, vmin=-2.5): - """Simulate the sampling portion of an analog-to-digital - conversion by outputing an integer number of counts associate with - each voltage in vectin.""" - dv = (vmax-vmin)/2**bits - vect2 = clip(vectin, vmin, vmax) - counts = vect2/dv - return counts.astype(int) - - -def CountsToFloat(counts, bits=9, vmax=2.5, vmin=-2.5): - """Convert the integer output of ADC to a floating point number by - mulitplying by dv.""" - dv = (vmax-vmin)/2**bits - return dv*counts - - -def epslist(listin, eps=1.0e-12): - """Make a copy of listin and then check each element of the copy - to see if its absolute value is greater than eps. Set to zero all - elements in the copied list whose absolute values are less than - eps. Return the copied list.""" - listout = copy.deepcopy(listin) - for i in range(len(listout)): - if abs(listout[i])= len(num): - realizable = True - return realizable - - -def shape_u(uvect, slope): - u_shaped = zeros_like(uvect) - u_shaped[0] = uvect[0] - - N = len(uvect) - - for n in range(1, N): - diff = uvect[n] - u_shaped[n-1] - if diff > slope: - u_shaped[n] = u_shaped[n-1] + slope - elif diff < -1*slope: - u_shaped[n] = u_shaped[n-1] - slope - else: - u_shaped[n] = uvect[n] - return u_shaped - - -class TransferFunction(signal.lti): - def __setattr__(self, attr, val): - realizable = False - if hasattr(self, 'den') and hasattr(self, 'num'): - realizable = _realizable(self.num, self.den) - if realizable: - signal.lti.__setattr__(self, attr, val) - else: - self.__dict__[attr] = val - - - def __init__(self, num, den, dt=0.01, maxt=5.0, myvar='s', label='G'): - """num and den are either scalar constants or lists that are - passed to scipy.poly1d to create a list of coefficients.""" - #print('in TransferFunction.__init__, dt=%s' % dt) - if _realizable(num, den): - num = atleast_1d(num) - den = atleast_1d(den) - start_num_ind = nonzero(num)[0][0] - start_den_ind = nonzero(den)[0][0] - num_ = num[start_num_ind:] - den_ = den[start_den_ind:] - signal.lti.__init__(self, num_, den_) - else: - z, p, k = signal.tf2zpk(num, den) - self.gain = k - self.num = poly1d(num) - self.den = poly1d(den) - self.dt = dt - self.myvar = myvar - self.maxt = maxt - self.label = label - - - def print_poles(self, label=None): - if label is None: - label = self.label - print(label +' poles =' + str(self.poles)) - - - def __repr__(self, labelstr='controls.TransferFunction'): - nstr=str(self.num)#.strip() - dstr=str(self.den)#.strip() - nstr=nstr.replace('x',self.myvar) - dstr=dstr.replace('x',self.myvar) - n=len(dstr) - m=len(nstr) - shift=(n-m)/2*' ' - nstr=nstr.replace('\n','\n'+shift) - tempstr=labelstr+'\n'+shift+nstr+'\n'+'-'*n+'\n '+dstr - return tempstr - - - def __call__(self,s,optargs=()): - return self.num(s)/self.den(s) - - - def __add__(self,other): - if hasattr(other,'num') and hasattr(other,'den'): - if len(self.den.coeffs)==len(other.den.coeffs) and \ - (self.den.coeffs==other.den.coeffs).all(): - return TransferFunction(self.num+other.num,self.den) - else: - return TransferFunction(self.num*other.den+other.num*self.den,self.den*other.den) - elif isinstance(other, int) or isinstance(other, float): - return TransferFunction(other*self.den+self.num,self.den) - else: - raise ValueError, 'do not know how to add TransferFunction and '+str(other) +' which is of type '+str(type(other)) - - def __radd__(self,other): - return self.__add__(other) - - - def __mul__(self,other): - if isinstance(other, Digital_P_Control): - return self.__class__(other.kp*self.num, self.den) - elif hasattr(other,'num') and hasattr(other,'den'): - if myeq(self.num,other.den) and myeq(self.den,other.num): - return 1 - elif myeq(self.num,other.den): - return self.__class__(other.num,self.den) - elif myeq(self.den,other.num): - return self.__class__(self.num,other.den) - else: - gain = self.gain*other.gain - new_num, new_den = polyfactor(self.num*other.num, \ - self.den*other.den) - newtf = self.__class__(new_num*gain, new_den) - return newtf - elif isinstance(other, int) or isinstance(other, float): - return self.__class__(other*self.num,self.den) - - - def __pow__(self, expon): - """Basically, go self*self*self as many times as necessary. I - haven't thought about negative exponents. I don't think this - would be hard, you would just need to keep dividing by self - until you got the right answer.""" - assert expon >= 0, 'TransferFunction.__pow__ does not yet support negative exponents.' - out = 1.0 - for n in range(expon): - out *= self - return out - - - def __rmul__(self,other): - return self.__mul__(other) - - - def __div__(self,other): - if hasattr(other,'num') and hasattr(other,'den'): - if myeq(self.den,other.den): - return TransferFunction(self.num,other.num) - else: - return TransferFunction(self.num*other.den,self.den*other.num) - elif isinstance(other, int) or isinstance(other, float): - return TransferFunction(self.num,other*self.den) - - - def __rdiv__(self, other): - print('calling TransferFunction.__rdiv__') - return self.__div__(other) - - - def __truediv__(self,other): - return self.__div__(other) - - - def _get_set_dt(self, dt=None): - if dt is not None: - self.dt = float(dt) - return self.dt - - - def simplify(self, rtol=1e-5, atol=1e-10): - """Return a new TransferFunction object with poles and zeros - that nearly cancel (within real or absolutie tolerance rtol - and atol) removed.""" - gain = self.gain - new_num, new_den = polyfactor(self.num, self.den, prepend=False) - newtf = self.__class__(new_num*gain, new_den) - return newtf - - - def ToLatex(self, eps=1e-12, fmt='%0.5g', ds=True): - mynum = self.num - myden = self.den - npart = PolyToLatex(mynum) - dpart = PolyToLatex(myden) - outstr = '\\frac{'+npart+'}{'+dpart+'}' - if ds: - outstr = '\\displaystyle '+outstr - return outstr - - - def RootLocus(self, kvect, fig=None, fignum=1, \ - clear=True, xlim=None, ylim=None, plotstr='-'): - """Calculate the root locus by finding the roots of 1+k*TF(s) - where TF is self.num(s)/self.den(s) and each k is an element - of kvect.""" - if fig is None: - import pylab - fig = pylab.figure(fignum) - if clear: - fig.clf() - ax = fig.add_subplot(111) - mymat = self._RLFindRoots(kvect) - mymat = self._RLSortRoots(mymat) - #plot open loop poles - poles = array(self.den.r) - ax.plot(real(poles), imag(poles), 'x') - #plot open loop zeros - zeros = array(self.num.r) - if zeros.any(): - ax.plot(real(zeros), imag(zeros), 'o') - for col in mymat.T: - ax.plot(real(col), imag(col), plotstr) - if xlim: - ax.set_xlim(xlim) - if ylim: - ax.set_ylim(ylim) - ax.set_xlabel('Real') - ax.set_ylabel('Imaginary') - return mymat - - - def _RLFindRoots(self, kvect): - """Find the roots for the root locus.""" - roots = [] - for k in kvect: - curpoly = self.den+k*self.num - curroots = curpoly.r - curroots.sort() - roots.append(curroots) - mymat = row_stack(roots) - return mymat - - - def _RLSortRoots(self, mymat): - """Sort the roots from self._RLFindRoots, so that the root - locus doesn't show weird pseudo-branches as roots jump from - one branch to another.""" - sorted = zeros_like(mymat) - for n, row in enumerate(mymat): - if n==0: - sorted[n,:] = row - else: - #sort the current row by finding the element with the - #smallest absolute distance to each root in the - #previous row - available = range(len(prevrow)) - for elem in row: - evect = elem-prevrow[available] - ind1 = abs(evect).argmin() - ind = available.pop(ind1) - sorted[n,ind] = elem - prevrow = sorted[n,:] - return sorted - - - def opt(self, kguess): - pnew = self._RLFindRoots(kguess) - pnew = self._RLSortRoots(pnew)[0] - if len(pnew)>1: - pnew = _checkpoles(self.poleloc,pnew) - e = abs(pnew-self.poleloc)**2 - return sum(e) - - - def rlocfind(self, poleloc): - self.poleloc = poleloc - kinit,pinit = _k_poles(self,poleloc) - k = fmin(self.opt,[kinit])[0] - poles = self._RLFindRoots([k]) - poles = self._RLSortRoots(poles) - return k, poles - - - def PlotTimeResp(self, u, t, fig, clear=True, label='model', mysub=111): - ax = fig.add_subplot(mysub) - if clear: - ax.cla() - try: - y = self.lsim(u, t) - except: - y = self.lsim2(u, t) - ax.plot(t, y, label=label) - return ax - - -## def BodePlot(self, f, fig, clear=False): -## mtf = self.FreqResp( -## ax1 = fig.axes[0] -## ax1.semilogx(modelf,20*log10(abs(mtf))) -## mphase = angle(mtf, deg=1) -## ax2 = fig.axes[1] -## ax2.semilogx(modelf, mphase) - - - def SimpleFactor(self): - mynum=self.num - myden=self.den - dsf=myden[myden.order] - nsf=mynum[mynum.order] - sden=myden/dsf - snum=mynum/nsf - poles=sden.r - residues=zeros(shape(sden.r),'D') - factors=[] - for x,p in enumerate(poles): - polearray=poles.copy() - polelist=polearray.tolist() - mypole=polelist.pop(x) - tempden=1.0 - for cp in polelist: - tempden=tempden*(poly1d([1,-cp])) - tempTF=TransferFunction(snum,tempden) - curres=tempTF(mypole) - residues[x]=curres - curTF=TransferFunction(curres,poly1d([1,-mypole])) - factors.append(curTF) - return factors,nsf,dsf - - def factor_constant(self, const): - """Divide numerator and denominator coefficients by const""" - self.num = self.num/const - self.den = self.den/const - - def lsim(self, u, t, interp=0, returnall=False, X0=None, hmax=None): - """Find the response of the TransferFunction to the input u - with time vector t. Uses signal.lsim. - - return y the response of the system.""" - try: - out = signal.lsim(self, u, t, interp=interp, X0=X0) - except LinAlgError: - #if the system has a pure integrator, lsim won't work. - #Call lsim2. - out = self.lsim2(u, t, X0=X0, returnall=True, hmax=hmax) - #override returnall because it is handled below - if returnall:#most users will just want the system output y, - #but some will need the (t, y, x) tuple that - #signal.lsim returns - return out - else: - return out[1] - -## def lsim2(self, u, t, returnall=False, X0=None): -## #tempsys=signal.lti(self.num,self.den) -## if returnall: -## return signal.lsim2(self, u, t, X0=X0) -## else: -## return signal.lsim2(self, u, t, X0=X0)[1] - - def lsim2(self, U, T, X0=None, returnall=False, hmax=None): - """Simulate output of a continuous-time linear system, using ODE solver. - - Inputs: - - system -- an instance of the LTI class or a tuple describing the - system. The following gives the number of elements in - the tuple and the interpretation. - 2 (num, den) - 3 (zeros, poles, gain) - 4 (A, B, C, D) - U -- an input array describing the input at each time T - (linear interpolation is assumed between given times). - If there are multiple inputs, then each column of the - rank-2 array represents an input. - T -- the time steps at which the input is defined and at which - the output is desired. - X0 -- (optional, default=0) the initial conditions on the state vector. - - Outputs: (T, yout, xout) - - T -- the time values for the output. - yout -- the response of the system. - xout -- the time-evolution of the state-vector. - """ - # system is an lti system or a sequence - # with 2 (num, den) - # 3 (zeros, poles, gain) - # 4 (A, B, C, D) - # describing the system - # U is an input vector at times T - # if system describes multiple outputs - # then U can be a rank-2 array with the number of columns - # being the number of inputs - - # rather than use lsim, use direct integration and matrix-exponential. - if hmax is None: - hmax = T[1]-T[0] - U = atleast_1d(U) - T = atleast_1d(T) - if len(U.shape) == 1: - U = U.reshape((U.shape[0],1)) - sU = U.shape - if len(T.shape) != 1: - raise ValueError, "T must be a rank-1 array." - if sU[0] != len(T): - raise ValueError, "U must have the same number of rows as elements in T." - if sU[1] != self.inputs: - raise ValueError, "System does not define that many inputs." - - if X0 is None: - X0 = zeros(self.B.shape[0],self.A.dtype) - - # for each output point directly integrate assume zero-order hold - # or linear interpolation. - - ufunc = interpolate.interp1d(T, U, kind='linear', axis=0, \ - bounds_error=False) - - def fprime(x, t, self, ufunc): - return dot(self.A,x) + squeeze(dot(self.B,nan_to_num(ufunc([t])))) - - xout = integrate.odeint(fprime, X0, T, args=(self, ufunc), hmax=hmax) - yout = dot(self.C,transpose(xout)) + dot(self.D,transpose(U)) - if returnall: - return T, squeeze(transpose(yout)), xout - else: - return squeeze(transpose(yout)) - - - def residue(self, tol=1e-3, verbose=0): - """from scipy.signal.residue: - - Compute residues/partial-fraction expansion of b(s) / a(s). - - If M = len(b) and N = len(a) - - b(s) b[0] s**(M-1) + b[1] s**(M-2) + ... + b[M-1] - H(s) = ------ = ---------------------------------------------- - a(s) a[0] s**(N-1) + a[1] s**(N-2) + ... + a[N-1] - - r[0] r[1] r[-1] - = -------- + -------- + ... + --------- + k(s) - (s-p[0]) (s-p[1]) (s-p[-1]) - - If there are any repeated roots (closer than tol), then the - partial fraction expansion has terms like - - r[i] r[i+1] r[i+n-1] - -------- + ----------- + ... + ----------- - (s-p[i]) (s-p[i])**2 (s-p[i])**n - - returns r, p, k - """ - r,p,k = signal.residue(self.num, self.den, tol=tol) - if verbose>0: - print('r='+str(r)) - print('') - print('p='+str(p)) - print('') - print('k='+str(k)) - - return r, p, k - - - def PartFrac(self, eps=1.0e-12): - """Compute the partial fraction expansion based on the residue - command. In the final polynomials, coefficients whose - absolute values are less than eps are set to zero.""" - r,p,k = self.residue() - - rlist = r.tolist() - plist = p.tolist() - - N = len(rlist) - - tflist = [] - eps = 1e-12 - - while N > 0: - curr = rlist.pop(0) - curp = plist.pop(0) - if abs(curp.imag) < eps: - #This is a purely real pole. The portion of the partial - #fraction expansion corresponding to this pole is curr/(s-curp) - curtf = TransferFunction(curr,[1,-curp]) - else: - #this is a complex pole and we need to find its conjugate and - #handle them together - cind = plist.index(curp.conjugate()) - rconj = rlist.pop(cind) - pconj = plist.pop(cind) - p1 = poly1d([1,-curp]) - p2 = poly1d([1,-pconj]) - #num = curr*p2+rconj*p1 - Nr = curr.real - Ni = curr.imag - Pr = curp.real - Pi = curp.imag - numlist = [2.0*Nr,-2.0*(Nr*Pr+Ni*Pi)] - numlist = epslist(numlist, eps) - num = poly1d(numlist) - denlist = [1, -2.0*Pr,Pr**2+Pi**2] - denlist = epslist(denlist, eps) - den = poly1d(denlist) - curtf = TransferFunction(num,den) - tflist.append(curtf) - N = len(rlist) - return tflist - - - def FreqResp(self, f, fignum=1, fig=None, clear=True, \ - grid=True, legend=None, legloc=1, legsub=1, \ - use_rad=False, **kwargs): - """Compute the frequency response of the transfer function - using the frequency vector f, returning a complex vector. - - The frequency response (Bode plot) will be plotted on - figure(fignum) unless fignum=None. - - legend should be a list of legend entries if a legend is - desired. If legend is not None, the legend will be placed on - the top half of the plot (magnitude portion) if legsub=1, or - on the bottom half with legsub=2. legloc follows the same - rules as the pylab legend command (1 is top right and goes - counter-clockwise from there.)""" - testvect=real(f)==0 - if testvect.all(): - s=f#then you really sent me s and not f - else: - if use_rad: - s = 1.0j*f - else: - s=2.0j*pi*f - self.comp = self.num(s)/self.den(s) - self.dBmag = 20*log10(abs(self.comp)) - rphase = unwrap(angle(self.comp)) - self.phase = rphase*180.0/pi - - if fig is None: - if fignum is not None: - import pylab - fig = pylab.figure(fignum) - - if fig is not None: - if clear: - fig.clf() - ax1 = fig.add_subplot(2,1,1) - ax2 = fig.add_subplot(2,1,2, sharex=ax1) - else: - ax1 = fig.axes[0] - ax2 = fig.axes[1] - - if fig is not None: - myargs=['linetype','linewidth'] - subkwargs={} - for key in myargs: - if kwargs.has_key(key): - subkwargs[key]=kwargs[key] - #myind=ax1._get_lines.count - mylines=_PlotMag(f, self, axis=ax1, **subkwargs) - ax1.set_ylabel('Mag. Ratio (dB)') - ax1.xaxis.set_major_formatter(MyFormatter()) - if grid: - ax1.grid(1) - if legend is not None and legsub==1: - ax1.legend(legend, legloc) - mylines=_PlotPhase(f, self, axis=ax2, **subkwargs) - ax2.set_ylabel('Phase (deg.)') - if use_rad: - ax2.set_xlabel('$\\omega$ (rad./sec.)') - else: - ax2.set_xlabel('Freq. (Hz)') - ax2.xaxis.set_major_formatter(MyFormatter()) - if grid: - ax2.grid(1) - if legend is not None and legsub==2: - ax2.legend(legend, legloc) - return self.comp - - - def CrossoverFreq(self, f): - if not hasattr(self, 'dBmag'): - self.FreqResp(f, fignum=None) - t1 = squeeze(self.dBmag > 0.0) - t2 = r_[t1[1:],t1[0]] - t3 = (t1 & -t2) - myinds = where(t3)[0] - if not myinds.any(): - return None, [] - maxind = max(myinds) - return f[maxind], maxind - - - def PhaseMargin(self,f): - fc,ind=self.CrossoverFreq(f) - if not fc: - return 180.0 - return 180.0+squeeze(self.phase[ind]) - - - def create_tvect(self, dt=None, maxt=None): - if dt is None: - dt = self.dt - else: - self.dt = dt - assert dt is not None, "You must either pass in a dt or call create_tvect on an instance with a self.dt already defined." - if maxt is None: - if hasattr(self,'maxt'): - maxt = self.maxt - else: - maxt = 100*dt - else: - self.maxt = maxt - tvect = arange(0,maxt+dt/2.0,dt) - self.t = tvect - return tvect - - - def create_impulse(self, dt=None, maxt=None, imp_time=0.5): - """Create the input impulse vector to be used in least squares - curve fitting of the c2d function.""" - if dt is None: - dt = self.dt - indon = int(imp_time/dt) - tvect = self.create_tvect(dt=dt, maxt=maxt) - imp = zeros_like(tvect) - imp[indon] = 1.0 - return imp - - - def create_step_input(self, dt=None, maxt=None, indon=5): - """Create the input impulse vector to be used in least squares - curve fitting of the c2d function.""" - tvect = self.create_tvect(dt=dt, maxt=maxt) - mystep = zeros_like(tvect) - mystep[indon:] = 1.0 - return mystep - - - def step_response(self, t=None, dt=None, maxt=None, \ - step_time=None, fignum=1, clear=True, \ - plotu=False, amp=1.0, interp=0, fig=None, \ - fmts=['-','-'], legloc=0, returnall=0, \ - legend=None, **kwargs): - """Find the response of the system to a step input. If t is - not given, then the time vector will go from 0 to maxt in - steps of dt i.e. t=arange(0,maxt,dt). If dt and maxt are not - given, the parameters from the TransferFunction instance will - be used. - - step_time is the time when the step input turns on. If not - given, it will default to 0. - - If clear is True, the figure will be cleared first. - clear=False could be used to overlay the step responses of - multiple TransferFunction's. - - plotu=True means that the step input will also be shown on the - graph. - - amp is the amplitude of the step input. - - return y unless returnall is set then return y, t, u - - where y is the response of the transfer function, t is the - time vector, and u is the step input vector.""" - if t is not None: - tvect = t - else: - tvect = self.create_tvect(dt=dt, maxt=maxt) - u = zeros_like(tvect) - if dt is None: - dt = self.dt - if step_time is None: - step_time = 0.0 - #step_time = 0.1*tvect.max() - if kwargs.has_key('indon'): - indon = kwargs['indon'] - else: - indon = int(step_time/dt) - u[indon:] = amp - try: - ystep = self.lsim(u, tvect, interp=interp)#[1]#the outputs of lsim are (t, y,x) - except: - ystep = self.lsim2(u, tvect)#[1] - - if fig is None: - if fignum is not None: - import pylab - fig = pylab.figure(fignum) - - if fig is not None: - if clear: - fig.clf() - ax = fig.add_subplot(111) - if plotu: - leglist =['Input','Output'] - ax.plot(tvect, u, fmts[0], linestyle='steps', **kwargs)#assume step input wants 'steps' linestyle - ofmt = fmts[1] - else: - ofmt = fmts[0] - ax.plot(tvect, ystep, ofmt, **kwargs) - ax.set_ylabel('Step Response') - ax.set_xlabel('Time (sec)') - if legend is not None: - ax.legend(legend, loc=legloc) - elif plotu: - ax.legend(leglist, loc=legloc) - #return ystep, ax - #else: - #return ystep - if returnall: - return ystep, tvect, u - else: - return ystep - - - - def impulse_response(self, dt=None, maxt=None, fignum=1, \ - clear=True, amp=1.0, fig=None, \ - fmt='-', **kwargs): - """Find the impulse response of the system using - scipy.signal.impulse. - - The time vector will go from 0 to maxt in steps of dt - i.e. t=arange(0,maxt,dt). If dt and maxt are not given, the - parameters from the TransferFunction instance will be used. - - If clear is True, the figure will be cleared first. - clear=False could be used to overlay the impulse responses of - multiple TransferFunction's. - - amp is the amplitude of the impulse input. - - return y, t - - where y is the impulse response of the transfer function and t - is the time vector.""" - - tvect = self.create_tvect(dt=dt, maxt=maxt) - temptf = amp*self - tout, yout = temptf.impulse(T=tvect) - - if fig is None: - if fignum is not None: - import pylab - fig = pylab.figure(fignum) - - if fig is not None: - if clear: - fig.clf() - ax = fig.add_subplot(111) - ax.plot(tvect, yout, fmt, **kwargs) - ax.set_ylabel('Impulse Response') - ax.set_xlabel('Time (sec)') - - return yout, tout - - - def swept_sine_response(self, maxf, minf=0.0, dt=None, maxt=None, deadtime=2.0, interp=0): - u = create_swept_sine_input(maxt, dt, maxf, minf=minf, deadtime=deadtime) - t = create_swept_sine_t(maxt, dt, deadtime=deadtime) - ysweep = self.lsim(u, t, interp=interp) - return t, u, ysweep - - - def _c2d_sub(self, numsub, densub, scale): - """This method performs substitutions for continuous to - digital conversions using the form: - - numsub - s = scale* -------- - densub - - where scale is a floating point number and numsub and densub - are poly1d instances. - - For example, scale = 2.0/T, numsub = poly1d([1,-1]), and - densub = poly1d([1,1]) for a Tustin c2d transformation.""" - m = self.num.order - n = self.den.order - mynum = 0.0 - for p, coeff in enumerate(self.num.coeffs): - mynum += poly1d(coeff*(scale**(m-p))*((numsub**(m-p))*(densub**(n-(m-p))))) - myden = 0.0 - for p, coeff in enumerate(self.den.coeffs): - myden += poly1d(coeff*(scale**(n-p))*((numsub**(n-p))*(densub**(n-(n-p))))) - return mynum.coeffs, myden.coeffs - - - def c2d_tustin(self, dt=None, a=2.0): - """Convert a continuous time transfer function into a digital - one by substituting - - a z-1 - s = - ----- - T z+1 - - into the compensator, where a is typically 2.0""" - #print('in TransferFunction.c2d_tustin, dt=%s' % dt) - dt = self._get_set_dt(dt) - #print('in TransferFunction.c2d_tustin after _get_set_dt, dt=%s' % dt) - scale = a/dt - numsub = poly1d([1.0,-1.0]) - densub = poly1d([1.0,1.0]) - mynum, myden = self._c2d_sub(numsub, densub, scale) - mynum = mynum/myden[0] - myden = myden/myden[0] - return mynum, myden - - - - def c2d(self, dt=None, maxt=None, method='zoh', step_time=0.5, a=2.0): - """Find a numeric approximation of the discrete transfer - function of the system. - - The general approach is to find the response of the system - using lsim and fit a discrete transfer function to that - response as a least squares problem. - - dt is the time between discrete time intervals (i.e. the - sample time). - - maxt is the length of time for which to calculate the system - respnose. An attempt is made to guess an appropriate stopping - time if maxt is None. For now, this defaults to 100*dt, - assuming that dt is appropriate for the system poles. - - method is a string describing the c2d conversion algorithm. - method = 'zoh refers to a zero-order hold for a sampled-data - system and follows the approach outlined by Dorsey in section - 14.17 of - "Continuous and Discrete Control Systems" summarized on page - 472 of the 2002 edition. - - Other supported options for method include 'tustin' - - indon gives the index of when the step input should switch on - for zoh or when the impulse should happen otherwise. There - should probably be enough zero entries before the input occurs - to accomidate the order of the discrete transfer function. - - a is used only if method = 'tustin' and it is substituted in the form - - a z-1 - s = - ----- - T z+1 - - a is almost always equal to 2. - """ - if method.lower() == 'zoh': - ystep = self.step_response(dt=dt, maxt=maxt, step_time=step_time)[0] - myimp = self.create_impulse(dt=dt, maxt=maxt, imp_time=step_time) - #Pdb().set_trace() - print('You called c2d with "zoh". This is most likely bad.') - nz, dz = fit_discrete_response(ystep, myimp, self.den.order, self.den.order+1)#we want the numerator order to be one less than the denominator order - the denominator order +1 is the order of the denominator during a step response - #multiply by (1-z^-1) - nz2 = r_[nz, [0.0]] - nzs = r_[[0.0],nz] - nz3 = nz2 - nzs - nzout, dzout = polyfactor(nz3, dz) - return nzout, dzout - #return nz3, dz - elif method.lower() == 'tustin': - #The basic approach for tustin is to create a transfer - #function that represents s mapped into z and then - #substitute this s(z)=a/T*(z-1)/(z+1) into the continuous - #transfer function - return self.c2d_tustin(dt=dt, a=a) - else: - raise ValueError, 'c2d method not understood:'+str(method) - - - - def DigitalSim(self, u, method='zoh', bits=9, vmin=-2.5, vmax=2.5, dt=None, maxt=None, digitize=True): - """Simulate the digital reponse of the transfer to input u. u - is assumed to be an input signal that has been sampled with - frequency 1/dt. u is further assumed to be a floating point - number with precision much higher than bits. u will be - digitized over the range [min, max], which is broken up into - 2**bits number of bins. - - The A and B vectors from c2d conversion will be found using - method, dt, and maxt. Note that maxt is only used for - method='zoh'. - - Once A and B have been found, the digital reponse of the - system to the digitized input u will be found.""" - B, A = self.c2d(dt=dt, maxt=maxt, method=method) - assert A[0]==1.0, "A[0]!=1 in c2d result, A="+str(A) - uvect = zeros(len(B), dtype='d') - yvect = zeros(len(A)-1, dtype='d') - if digitize: - udig = ADC(u, bits, vmax=vmax, vmin=vmin) - dv = (vmax-vmin)/(2**bits-1) - else: - udig = u - dv = 1.0 - Ydig = zeros(len(u), dtype='d') - for n, u0 in enumerate(udig): - uvect = shift(uvect, u0) - curY = dot(uvect,B) - negpart = dot(yvect,A[1:]) - curY -= negpart - if digitize: - curY = int(curY) - Ydig[n] = curY - yvect = shift(yvect, curY) - return Ydig*dv - -TF = TransferFunction - -class Input(TransferFunction): - def __repr__(self): - return TransferFunction.__repr__(self, labelstr='controls.Input') - - -class Compensator(TransferFunction): - def __init__(self, num, den, *args, **kwargs): - #print('in Compensator.__init__') - #Pdb().set_trace() - TransferFunction.__init__(self, num, den, *args, **kwargs) - - - def c2d(self, dt=None, a=2.0): - """Compensators should use Tustin for c2d conversion. This - method is just and alias for TransferFunction.c2d_tustin""" - #print('in Compensators.c2d, dt=%s' % dt) - #Pdb().set_trace() - return TransferFunction.c2d_tustin(self, dt=dt, a=a) - - def __repr__(self): - return TransferFunction.__repr__(self, labelstr='controls.Compensator') - - - -class Digital_Compensator(object): - def __init__(self, num, den, input_vect=None, output_vect=None): - self.num = num - self.den = den - self.input = input_vect - self.output = output_vect - self.Nnum = len(self.num) - self.Nden = len(self.den) - - - def calc_out(self, i): - out = 0.0 - for n, bn in enumerate(self.num): - out += self.input[i-n]*bn - - for n in range(1, self.Nden): - out -= self.output[i-n]*self.den[n] - out = out/self.den[0] - return out - - -class Digital_PI(object): - def __init__(self, kp, ki, input_vect=None, output_vect=None): - self.kp = kp - self.ki = ki - self.input = input_vect - self.output = output_vect - self.esum = 0.0 - - - def prep(self): - self.esum = zeros_like(self.input) - - - def calc_out(self, i): - self.esum[i] = self.esum[i-1]+self.input[i] - out = self.input[i]*self.kp+self.esum[i]*self.ki - return out - - -class Digital_P_Control(Digital_Compensator): - def __init__(self, kp, input_vect=None, output_vect=None): - self.kp = kp - self.input = input_vect - self.output = output_vect - self.num = poly1d([kp]) - self.den = poly1d([1]) - self.gain = 1 - - def calc_out(self, i): - self.output[i] = self.kp*self.input[i] - return self.output[i] - - -def dig_comp_from_c_comp(c_comp, dt): - """Convert a continuous compensator into a digital one using Tustin - and sampling time dt.""" - b, a = c_comp.c2d_tustin(dt=dt) - return Digital_Compensator(b, a) - - -class FirstOrderCompensator(Compensator): - def __init__(self, K, z, p, dt=0.004): - """Create a first order compensator whose transfer function is - - K*(s+z) - D(s) = ----------- - (s+p) """ - Compensator.__init__(self, K*poly1d([1,z]), [1,p]) - - - def __repr__(self): - return TransferFunction.__repr__(self, labelstr='controls.FirstOrderCompensator') - - - def ToPSoC(self, dt=0.004): - b, a = self.c2d(dt=dt) - outstr = 'v = %f*e%+f*ep%+f*vp;'%(b[0],b[1],-a[1]) - print('PSoC str:') - print(outstr) - return outstr - - -def sat(vin, vmax=2.0): - if vin > vmax: - return vmax - elif vin < -1*vmax: - return -1*vmax - else: - return vin - -class ButterworthFilter(Compensator): - def __init__(self,fc,mag=1.0): - """Create a compensator that is a second order Butterworth - filter. fc is the corner frequency in Hz and mag is the low - frequency magnitude so that the transfer function will be - mag*wn**2/(s**2+2*z*wn*s+wn**2) where z=1/sqrt(2) and - wn=2.0*pi*fc.""" - z=1.0/sqrt(2.0) - wn=2.0*pi*fc - Compensator.__init__(self,mag*wn**2,[1.0,2.0*z*wn,wn**2]) - -class Closed_Loop_System_with_Sat(object): - def __init__(self, plant_tf, Kp, sat): - self.plant_tf = plant_tf - self.Kp = Kp - self.sat = sat - - - def lsim(self, u, t, X0=None, include_sat=True, \ - returnall=0, lsim2=0, verbosity=0): - dt = t[1]-t[0] - if X0 is None: - X0 = zeros((2,len(self.plant_tf.den.coeffs)-1)) - N = len(t) - y = zeros(N) - v = zeros(N) - x_n = X0 - for n in range(1,N): - t_n = t[n] - if verbosity > 0: - print('t_n='+str(t_n)) - e = u[n]-y[n-1] - v_n = self.Kp*e - if include_sat: - v_n = sat(v_n, vmax=self.sat) - #simulate for one dt using ZOH - if lsim2: - t_nn, y_n, x_n = self.plant_tf.lsim2([v_n,v_n], [t_n, t_n+dt], X0=x_n[-1], returnall=1) - else: - t_nn, y_n, x_n = self.plant_tf.lsim([v_n,v_n], [t_n, t_n+dt], X0=x_n[-1], returnall=1) - - y[n] = y_n[-1] - v[n] = v_n - self.y = y - self.v = v - self.u = u - if returnall: - return y, v - else: - return y - - - - - -def step_input(): - return Input(1,[1,0]) - - -def feedback(olsys,H=1): - """Calculate the closed-loop transfer function - - olsys - cltf = -------------- - 1+H*olsys - - where olsys is the transfer function of the open loop - system (Gc*Gp) and H is the transfer function in the feedback - loop (H=1 for unity feedback).""" - clsys=olsys/(1.0+H*olsys) - return clsys - - - -def Usweep(ti,maxt,minf=0.0,maxf=10.0): - """Return the current value (scalar) of a swept sine signal - must be used - with list comprehension to generate a vector. - - ti - current time (scalar) - minf - lowest frequency in the sweep - maxf - highest frequency in the sweep - maxt - T or the highest value in the time vector""" - if ti<0.0: - return 0.0 - else: - curf=(maxf-minf)*ti/maxt+minf - if ti<(maxt*0.95): - return sin(2*pi*curf*ti) - else: - return 0.0 - - -def sweptsine(t,minf=0.0, maxf=10.0): - """Generate a sweptsine vector by calling Usweep for each ti in t.""" - T=max(t)-min(t) - Us = [Usweep(ti,T,minf,maxf) for ti in t] - return array(Us) - - -mytypes=['-','--',':','-.'] -colors=['b','y','r','g','c','k']#['y','b','r','g','c','k'] - -def _getlinetype(ax=None): - if ax is None: - import pylab - ax = pylab.gca() - myind=ax._get_lines.count - return {'color':colors[myind % len(colors)],'linestyle':mytypes[myind % len(mytypes)]} - - -def create_step_vector(t, step_time=0.0, amp=1.0): - u = zeros_like(t) - dt = t[1]-t[0] - indon = int(step_time/dt) - u[indon:] = amp - return u - - -def rate_limiter(uin, du): - uout = zeros_like(uin) - N = len(uin) - for n in range(1,N): - curchange = uin[n]-uout[n-1] - if curchange > du: - uout[n] = uout[n-1]+du - elif curchange < -du: - uout[n] = uout[n-1]-du - else: - uout[n] = uin[n] - return uout - - - - diff --git a/external/yottalab.py b/external/yottalab.py deleted file mode 100644 index fcef0e2a1..000000000 --- a/external/yottalab.py +++ /dev/null @@ -1,689 +0,0 @@ -""" -This is a procedural interface to the yttalab library - -roberto.bucher@supsi.ch - -The following commands are provided: - -Design and plot commands - dlqr - Discrete linear quadratic regulator - d2c - discrete to continous time conversion - full_obs - full order observer - red_obs - reduced order observer - comp_form - state feedback controller+observer in compact form - comp_form_i - state feedback controller+observer+integ in compact form - set_aw - introduce anti-windup into controller - bb_dcgain - return the steady state value of the step response - placep - Pole placement (replacement for place) - bb_c2d - Continous to discrete conversion - - Old functions now corrected in python control - bb_dare - Solve Riccati equation for discrete time systems - -""" -from numpy import hstack, vstack, rank, imag, zeros, eye, mat, \ - array, shape, real, sort, around -from scipy import poly -from scipy.linalg import inv, expm, eig, eigvals, logm -import scipy as sp -from slycot import sb02od -from matplotlib.pyplot import * -from control import * -from supsictrl import _wrapper - -def d2c(sys,method='zoh'): - """Continous to discrete conversion with ZOH method - - Call: - sysc=c2d(sys,method='log') - - Parameters - ---------- - sys : System in statespace or Tf form - method: 'zoh' or 'bi' - - Returns - ------- - sysc: continous system ss or tf - - - """ - flag = 0 - if isinstance(sys, TransferFunction): - sys=tf2ss(sys) - flag=1 - - a=sys.A - b=sys.B - c=sys.C - d=sys.D - Ts=sys.dt - n=shape(a)[0] - nb=shape(b)[1] - nc=shape(c)[0] - tol=1e-12 - - if method=='zoh': - if n==1: - if b[0,0]==1: - A=0 - B=b/sys.dt - C=c - D=d - else: - tmp1=hstack((a,b)) - tmp2=hstack((zeros((nb,n)),eye(nb))) - tmp=vstack((tmp1,tmp2)) - s=logm(tmp) - s=s/Ts - if norm(imag(s),inf) > sqrt(sp.finfo(float).eps): - print "Warning: accuracy may be poor" - s=real(s) - A=s[0:n,0:n] - B=s[0:n,n:n+nb] - C=c - D=d - elif method=='foh': - a=mat(a) - b=mat(b) - c=mat(c) - d=mat(d) - Id = mat(eye(n)) - A = logm(a)/Ts - A = real(around(A,12)) - Amat = mat(A) - B = (a-Id)**(-2)*Amat**2*b*Ts - B = real(around(B,12)) - Bmat = mat(B) - C = c - D = d - C*(Amat**(-2)/Ts*(a-Id)-Amat**(-1))*Bmat - D = real(around(D,12)) - elif method=='bi': - a=mat(a) - b=mat(b) - c=mat(c) - d=mat(d) - poles=eigvals(a) - if any(abs(poles-1)<200*sp.finfo(float).eps): - print "d2c: some poles very close to one. May get bad results." - - I=mat(eye(n,n)) - tk = 2 / sqrt (Ts) - A = (2/Ts)*(a-I)*inv(a+I) - iab = inv(I+a)*b - B = tk*iab - C = tk*(c*inv(I+a)) - D = d- (c*iab) - else: - print "Method not supported" - return - - sysc=StateSpace(A,B,C,D) - if flag==1: - sysc=ss2tf(sysc) - return sysc - -def dlqr(*args, **keywords): - """Linear quadratic regulator design for discrete systems - - Usage - ===== - [K, S, E] = dlqr(A, B, Q, R, [N]) - [K, S, E] = dlqr(sys, Q, R, [N]) - - The dlqr() function computes the optimal state feedback controller - that minimizes the quadratic cost - - J = \sum_0^\infty x' Q x + u' R u + 2 x' N u - - Inputs - ------ - A, B: 2-d arrays with dynamics and input matrices - sys: linear I/O system - Q, R: 2-d array with state and input weight matrices - N: optional 2-d array with cross weight matrix - - Outputs - ------- - K: 2-d array with state feedback gains - S: 2-d array with solution to Riccati equation - E: 1-d array with eigenvalues of the closed loop system - """ - - # - # Process the arguments and figure out what inputs we received - # - - # Get the system description - if (len(args) < 3): - raise ControlArgument("not enough input arguments") - - elif (ctrlutil.issys(args[0])): - # We were passed a system as the first argument; extract A and B - A = array(args[0].A, ndmin=2, dtype=float); - B = array(args[0].B, ndmin=2, dtype=float); - index = 1; - if args[0].dt==0.0: - print "dlqr works only for discrete systems!" - return - else: - # Arguments should be A and B matrices - A = array(args[0], ndmin=2, dtype=float); - B = array(args[1], ndmin=2, dtype=float); - index = 2; - - # Get the weighting matrices (converting to matrices, if needed) - Q = array(args[index], ndmin=2, dtype=float); - R = array(args[index+1], ndmin=2, dtype=float); - if (len(args) > index + 2): - N = array(args[index+2], ndmin=2, dtype=float); - Nflag = 1; - else: - N = zeros((Q.shape[0], R.shape[1])); - Nflag = 0; - - # Check dimensions for consistency - nstates = B.shape[0]; - ninputs = B.shape[1]; - if (A.shape[0] != nstates or A.shape[1] != nstates): - raise ControlDimension("inconsistent system dimensions") - - elif (Q.shape[0] != nstates or Q.shape[1] != nstates or - R.shape[0] != ninputs or R.shape[1] != ninputs or - N.shape[0] != nstates or N.shape[1] != ninputs): - raise ControlDimension("incorrect weighting matrix dimensions") - - if Nflag==1: - Ao=A-B*inv(R)*N.T - Qo=Q-N*inv(R)*N.T - else: - Ao=A - Qo=Q - - #Solve the riccati equation - (X,L,G) = dare(Ao,B,Qo,R) -# X = bb_dare(Ao,B,Qo,R) - - # Now compute the return value - Phi=mat(A) - H=mat(B) - K=inv(H.T*X*H+R)*(H.T*X*Phi+N.T) - L=eig(Phi-H*K) - return K,X,L - -def full_obs(sys,poles): - """Full order observer of the system sys - - Call: - obs=full_obs(sys,poles) - - Parameters - ---------- - sys : System in State Space form - poles: desired observer poles - - Returns - ------- - obs: ss - Observer - - """ - if isinstance(sys, TransferFunction): - "System must be in state space form" - return - a=mat(sys.A) - b=mat(sys.B) - c=mat(sys.C) - d=mat(sys.D) - L=placep(a.T,c.T,poles) - L=mat(L).T - Ao=a-L*c - Bo=hstack((b-L*d,L)) - n=shape(Ao) - m=shape(Bo) - Co=eye(n[0],n[1]) - Do=zeros((n[0],m[1])) - obs=StateSpace(Ao,Bo,Co,Do,sys.dt) - return obs - -def red_obs(sys,T,poles): - """Reduced order observer of the system sys - - Call: - obs=red_obs(sys,T,poles) - - Parameters - ---------- - sys : System in State Space form - T: Complement matrix - poles: desired observer poles - - Returns - ------- - obs: ss - Reduced order Observer - - """ - if isinstance(sys, TransferFunction): - "System must be in state space form" - return - a=mat(sys.A) - b=mat(sys.B) - c=mat(sys.C) - d=mat(sys.D) - T=mat(T) - P=mat(vstack((c,T))) - invP=inv(P) - AA=P*a*invP - ny=shape(c)[0] - nx=shape(a)[0] - nu=shape(b)[1] - - A11=AA[0:ny,0:ny] - A12=AA[0:ny,ny:nx] - A21=AA[ny:nx,0:ny] - A22=AA[ny:nx,ny:nx] - - L1=placep(A22.T,A12.T,poles) - L1=mat(L1).T - - nn=nx-ny - - tmp1=mat(hstack((-L1,eye(nn,nn)))) - tmp2=mat(vstack((zeros((ny,nn)),eye(nn,nn)))) - Ar=tmp1*P*a*invP*tmp2 - - tmp3=vstack((eye(ny,ny),L1)) - tmp3=mat(hstack((P*b,P*a*invP*tmp3))) - tmp4=hstack((eye(nu,nu),zeros((nu,ny)))) - tmp5=hstack((-d,eye(ny,ny))) - tmp4=mat(vstack((tmp4,tmp5))) - - Br=tmp1*tmp3*tmp4 - - Cr=invP*tmp2 - - tmp5=hstack((zeros((ny,nu)),eye(ny,ny))) - tmp6=hstack((zeros((nn,nu)),L1)) - tmp5=mat(vstack((tmp5,tmp6))) - Dr=invP*tmp5*tmp4 - - obs=StateSpace(Ar,Br,Cr,Dr,sys.dt) - return obs - -def comp_form(sys,obs,K): - """Compact form Conroller+Observer - - Call: - contr=comp_form(sys,obs,K) - - Parameters - ---------- - sys : System in State Space form - obs : Observer in State Space form - K: State feedback gains - - Returns - ------- - contr: ss - Controller - - """ - nx=shape(sys.A)[0] - ny=shape(sys.C)[0] - nu=shape(sys.B)[1] - no=shape(obs.A)[0] - - Bu=mat(obs.B[:,0:nu]) - By=mat(obs.B[:,nu:]) - Du=mat(obs.D[:,0:nu]) - Dy=mat(obs.D[:,nu:]) - - X=inv(eye(nu,nu)+K*Du) - - Ac = mat(obs.A)-Bu*X*K*mat(obs.C); - Bc = hstack((Bu*X,By-Bu*X*K*Dy)) - Cc = -X*K*mat(obs.C); - Dc = hstack((X,-X*K*Dy)) - contr = StateSpace(Ac,Bc,Cc,Dc,sys.dt) - return contr - -def comp_form_i(sys,obs,K,Ts,Cy=[[1]]): - """Compact form Conroller+Observer+Integral part - Only for discrete systems!!! - - Call: - contr=comp_form_i(sys,obs,K,Ts[,Cy]) - - Parameters - ---------- - sys : System in State Space form - obs : Observer in State Space form - K: State feedback gains - Ts: Sampling time - Cy: feedback matric to choose the output for integral part - - Returns - ------- - contr: ss - Controller - - """ - if sys.dt==0.0: - print "contr_form_i works only with discrete systems!" - return - - ny=shape(sys.C)[0] - nu=shape(sys.B)[1] - nx=shape(sys.A)[0] - no=shape(obs.A)[0] - ni=shape(mat(Cy))[0] - - B_obsu = mat(obs.B[:,0:nu]) - B_obsy = mat(obs.B[:,nu:nu+ny]) - D_obsu = mat(obs.D[:,0:nu]) - D_obsy = mat(obs.D[:,nu:nu+ny]) - - k=mat(K) - nk=shape(k)[1] - Ke=k[:,nk-ni:] - K=k[:,0:nk-ni] - X = inv(eye(nu,nu)+K*D_obsu); - - a=mat(obs.A) - c=mat(obs.C) - Cy=mat(Cy) - - tmp1=hstack((a-B_obsu*X*K*c,-B_obsu*X*Ke)) - - tmp2=hstack((zeros((ni,no)),eye(ni,ni))) - A_ctr=vstack((tmp1,tmp2)) - - tmp1=hstack((zeros((no,ni)),-B_obsu*X*K*D_obsy+B_obsy)) - tmp2=hstack((eye(ni,ni)*Ts,-Cy*Ts)) - B_ctr=vstack((tmp1,tmp2)) - - C_ctr=hstack((-X*K*c,-X*Ke)) - D_ctr=hstack((zeros((nu,ni)),-X*K*D_obsy)) - - contr=StateSpace(A_ctr,B_ctr,C_ctr,D_ctr,sys.dt) - return contr - -def sysctr(sys,contr): - """Build the discrete system controller+plant+output feedback - - Call: - syscontr=sysctr(sys,contr) - - Parameters - ---------- - sys : Continous System in State Space form - contr: Controller (with observer if required) - - Returns - ------- - sysc: ss system - The system with reference as input and outputs of plants - as output - - """ - if contr.dt!=sys.dt: - print "Systems with different sampling time!!!" - return - sysf=sys*contr - - nu=shape(sysf.B)[1] - b1=mat(sysf.B[:,0]) - b2=mat(sysf.B[:,1:nu]) - d1=mat(sysf.D[:,0]) - d2=mat(sysf.D[:,1:nu]) - - n2=shape(d2)[0] - - Id=mat(eye(n2,n2)) - X=inv(Id-d2) - - Af=mat(sysf.A)+b2*X*mat(sysf.C) - Bf=b1+b2*X*d1 - Cf=X*mat(sysf.C) - Df=X*d1 - - sysc=StateSpace(Af,Bf,Cf,Df,sys.dt) - return sysc - -def set_aw(sys,poles): - """Divide in controller in input and feedback part - for anti-windup - - Usage - ===== - [sys_in,sys_fbk]=set_aw(sys,poles) - - Inputs - ------ - - sys: controller - poles : poles for the anti-windup filter - - Outputs - ------- - sys_in, sys_fbk: controller in input and feedback part - """ - sys = ss(sys) - den_old=poly(eigvals(sys.A)) - sys=tf(sys) - den = poly(poles) - tmp= tf(den_old,den,sys.dt) - sys_in=tmp*sys - sys_in = sys_in.minreal() - sys_in = ss(sys_in) - sys_fbk=1-tmp - sys_fbk = sys_fbk.minreal() - sys_fbk = ss(sys_fbk) - return sys_in, sys_fbk - -def placep(A,B,P): - """Return the steady state value of the step response os sysmatrix K for - pole placement - - Usage - ===== - K = placep(A,B,P) - - Inputs - ------ - - A : State matrix A - B : INput matrix - P : desired poles - - Outputs - ------- - K : State gains for pole placement - """ - - n = shape(A)[0] - m = shape(B)[1] - tol = 0.0 - mode = 1; - - wrka = zeros((n,m)) - wrk1 = zeros(m) - wrk2 = zeros(m) - iwrk = zeros((m),np.int) - - A,B,ncont,indcont,nblk,z = _wrapper.ssxmc(n,m,A,n,B,wrka,wrk1,wrk2,iwrk,tol,mode) - P = sort(P) - wr = real(P) - wi = imag(P) - - g = zeros((m,n)) - - mx = max(2,m) - rm1 = zeros((m,m)) - rm2 = zeros((m,mx)) - rv1 = zeros(n) - rv2 = zeros(n) - rv3 = zeros(m) - rv4 = zeros(m) - - A,B,g,z,ierr,jpvt = _wrapper.polmc(A,B,g,wr,wi,z,indcont,nblk,rm1, rm2, rv1, rv2, rv3, rv4) - - return g - -""" -These functions are now implemented in python control and should not be used anymore -""" - -def bb_dare(A,B,Q,R): - """Solve Riccati equation for discrete time systems - - Usage - ===== - [K, S, E] = bb_dare(A, B, Q, R) - - Inputs - ------ - A, B: 2-d arrays with dynamics and input matrices - sys: linear I/O system - Q, R: 2-d array with state and input weight matrices - - Outputs - ------- - X: solution of the Riccati eq. - """ - - # Check dimensions for consistency - nstates = B.shape[0]; - ninputs = B.shape[1]; - if (A.shape[0] != nstates or A.shape[1] != nstates): - raise ControlDimension("inconsistent system dimensions") - - elif (Q.shape[0] != nstates or Q.shape[1] != nstates or - R.shape[0] != ninputs or R.shape[1] != ninputs) : - raise ControlDimension("incorrect weighting matrix dimensions") - - X,rcond,w,S,T = \ - sb02od(nstates, ninputs, A, B, Q, R, 'D'); - - return X - - - -def bb_dcgain(sys): - """Return the steady state value of the step response os sys - - Usage - ===== - dcgain=dcgain(sys) - - Inputs - ------ - - sys: system - - Outputs - ------- - dcgain : steady state value - """ - - a=mat(sys.A) - b=mat(sys.B) - c=mat(sys.C) - d=mat(sys.D) - nx=shape(a)[0] - if sys.dt!=0.0: - a=a-eye(nx,nx) - r=rank(a) - if r pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy