def main(): P = np.array([[1, 1], [2, 2], [3, 3], [4, -1], [5, 1]]) n = len(P) PX = P.T[0] PY = P.T[1] x = np.zeros(n) y = np.zeros(n) for num in range(0, n): koef = (bezyeKoef(n - 1, num)) x = np.polyadd(x, P[num][0] * koef) y = np.polyadd(y, P[num][1] * koef) print("answer") print(x) print(y) X = [] Y = [] print(np.polyval(y, 0)) for t in np.linspace(0, 1, 100): X += [np.polyval(x, t)] Y += [np.polyval(y, t)] print(X) print(Y) global fig, ax fig, ax = pylab.subplots() #ax = fig.gca(projection='2d') ax.plot(X, Y, label='parametric curve') ax.plot(PX, PY, label="orientir") ax.legend() pylab.show()
def solve_for_nearest( px,py,rx,ry ): dpx = polyder(px) dpy = polyder(py) cp = polymul( dpx, px ) + polymul( dpy, py ) cp = polyadd( cp, -rx*dpx ) cp = polyadd( cp, -ry*dpy ) t = roots(cp) t = real(t[isreal(t)]) t = t[ (t>=0) * (t<=1) ] ##tt = linspace(0,1,100) ##from pylab import plot ##plot( polyval(px,tt), polyval(py,tt), 'k', hold = 0 ) ##plot( [rx],[ry], 'r.' ) ##plot( polyval(px,t[isreal(t)*(real(t)>=0)*(real(t)<=1)]), ## polyval(py,t[isreal(t)*(real(t)>=0)*(real(t)<=1)]), 'o' ) ##pdb.set_trace() if len(t): if len(t) == 1: return t[0] else: ux = polyval( px, t ) uy = polyval( py, t ) d = hypot( ux - rx, uy - ry ) return t[ d==d.min() ][0] else: t = array([0.0,1.0]) ux = polyval( px, t ) uy = polyval( py, t ) d = hypot( ux - rx, uy - ry ) if d[0] < d[1]: return 0.0 else: return 1.0
def PolyKaratsuba(a, b): if len(a) == 1 and len(b) == 1: return a * b deg_a, deg_b = len(a), len(b) if deg_a < deg_b: a, b = b, a deg_a, deg_b = deg_b, deg_a if deg_a & 1: deg_a += 1 a = np.concatenate([np.zeros(1), a]) if deg_a != deg_b: b = np.concatenate([np.zeros(deg_a - deg_b), b]) a1 = a[:deg_a // 2] b1 = b[:deg_a // 2] a2 = a[deg_a // 2:] b2 = b[deg_a // 2:] result_1 = PolyKaratsuba(a1, b1) #f result_2 = PolyKaratsuba(a2, b2) #s result_3 = PolyKaratsuba(np.polyadd(a1,a2), np.polyadd(b1,b2)) #m result_3 = np.polysub(np.polysub(result_3, result_2), result_1) result_1 = np.concatenate([result_1, np.zeros(deg_a)]) result_3 = np.concatenate([result_3, np.zeros(deg_a//2)]) # return np.polyadd(result_1, np.polyadd(result_2, result_3)) res = np.polyadd(result_1, np.polyadd(result_2, result_3)) res = np.polymul(res, [1]) return res
def decode(input: str) -> str: codeword = string_to_list(input) shift = 0 while True: syndrome = compute_syndrome(codeword) syndrome_16 = compute_syndrome_16(syndrome) syndrome_17 = compute_syndrome_17(syndrome) if compute_weight(syndrome) <= 3: errors = syndrome break if compute_weight(syndrome_16) <= 2: errors = np.polyadd(x16, syndrome_16) break if compute_weight(syndrome_17) <= 2: errors = np.polyadd(x17, syndrome_17) break shift += 1 codeword = np.roll(codeword, 1) codeword = normalize(np.polyadd(codeword, errors)) codeword = np.roll(codeword, -shift) decoded_codeword = np.polydiv(codeword, G)[0] decoded_codeword = normalize(decoded_codeword[len(decoded_codeword) - k:]) return list_to_string(decoded_codeword, k)
def PolyKaratsuba(a: np.array, b: np.array): length = max(a.size, b.size) if length < 2: return a * b if length & 1: length += 1 a = np.append(np.zeros((length - a.size, ), dtype=int), a) b = np.append(np.zeros((length - b.size, ), dtype=int), b) len2 = length // 2 #надо поделить пополам al = a[:len2] ar = a[len2:] bl = b[:len2] br = b[len2:] p1 = PolyKaratsuba(al, bl) p2 = PolyKaratsuba(ar, br) p3 = PolyKaratsuba(np.polyadd(al, ar), np.polyadd(bl, br)) p3 = np.polysub(p3, p1) p3 = np.polysub(p3, p2) p1 = np.append(p1, np.zeros((length, ), dtype=np.int)) p3 = np.append(p3, np.zeros((len2, ), dtype=np.int)) res = np.polyadd(p1, p2) res = np.polyadd(res, p3) while (res.size > 1 and res[0] == 0): res = res[1:] return res
def __rsub__(self,other): if type(other) in [int, float]: return ltimul(polyadd(-self.num,self.den*other),self.den) if type(other) in [TransFun, type(self)]: numer = polyadd(polymul(other.num,self.den),-polymul(self.den,other.num)) denom = polymul(self.den,other.den) return ltimul(numer,denom)
def __add__(self, other): if type(other) in [int, float]: return ltimul(numpy.polyadd(self.num, self.den * other), self.den) if type(other) in [TransFun, type(self)]: numer = numpy.polyadd(numpy.polymul(self.num, other.den), polymul(self.den, other.num)) denom = numpy.polymul(self.den, other.den) return ltimul(numer, denom)
def synthesizeFIRLattice(A_N, N, gamma): """ Function to synthesize an FIR optical lattice filter using the algorithm outlined in Section 4.5 of Madsen and Zhao Paramters: A_N: Coefficient array of polynomial in z^-1 of degree N that is part of the 2x2 transfer function N: Filter order gamma: Loss Coefficient per stage Return: kappalcs: List of power coupling coefficients kappa_n's, Lc's and lc + lend for each stage, as well as c_n and s_n list index is same as n Format is: kappalcs[n] = (kappa_n, L_c_n, lc + lend of stage n, c_n, s_n) phi_l: List of phase terms phi_n list index is n-1 B_N: Polynomial (in z^-1) B_N(z) """ B_N = findBPolyMA(A_N, plot=False) B_N_OG = B_N phi_l = [] #List of phi_n's kappalcs = [] #List of kappas, Lc's. This is what we want to return n = N while n >= 0: #print(A_N) #Calculate kappa beta = np.absolute(B_N[0] / A_N[0])**2 kappa = beta / (1.0 + beta) L_c = calcLengths( kappa, 2.0, 2.0 ) #Find lengths of structures we need for layout, convert wavelengths to micrometers c_n = np.sqrt(1.0 - kappa) s_n = np.sqrt(kappa) kappalcs.insert(0, (kappa, L_c)) if n > 0: B_N1 = np.polyadd( -s_n * A_N, c_n * B_N ) #Step-down recursion relation for B polynomial of stage N-1, this is an ndArray B_N1 = B_N1[1:B_N1.size] #B_N1 = np.poly1d(B_N1_arr[1:B_N1_arr.size])#Reduce order by 1 #Shouldn't have complex coefs. for ii in range(B_N1.size): if np.imag(B_N1[ii]) < 2.0E-16: B_N1[ii] = np.real(B_N1[ii]) A_N1_tild = np.polyadd(c_n * A_N, s_n * B_N) phi_n = -(np.angle(A_N1_tild[0]) + np.angle(B_N1[0])) phi_l.insert(0, phi_n) A_N1_tild = (1.0 / gamma) * np.exp(1j * phi_n) * A_N1_tild A_N1 = A_N1_tild[ 0:A_N1_tild.size - 1] #Build polynomial A_N1(z), and reduce order by 1 by eliminating the constant term(multiplying by z) #Shouldn't have complex coefs. for ii in range(A_N1.size): if np.imag(A_N1[ii]) < 2.0E-16: A_N1[ii] = np.real(A_N1[ii]) n = n - 1 A_N = A_N1 B_N = B_N1 return kappalcs, phi_l, B_N_OG
def __init__(self,theta=1.,maxVar=1.): self.theta = theta self.maxVar = maxVar p0 = np.array([theta**2.,3.*theta,3.]) p1 = np.polyadd(np.polyder(p0),-theta*p0) p2 = np.polyadd(np.polyder(p1),-theta*p1) self.p0 = p0 self.p1 = p1 self.p2 = p2
def __rsub__(self, other): """right substraction""" if type(other) in [int, float]: return dTF(polyadd(-self._num.A1, self._den.A1 * other), self._den) if isinstance(other, dTF): numer = polyadd(polymul(other.num.A1, self._den.A1), -polymul(other.den.A1, self._num.A1)) denom = polymul(self._den.A1, other.den.A1) return dTF(numer, denom)
def sumo_sistemas(sys3, sys4): if not isinstance(sys3, signal.lti): sys3 = signal.lti(*sys3) if not isinstance(sys4, signal.lti): sys4 = signal.lti(*sys4) num = np.polyadd(sys3.num, sys4.num) den = np.polyadd(sys3.den, sys4.den) sys = signal.lti(num, den) return sys
def __add__(self, rhs): if isinstance(rhs, numbers.Number): return TransferFunction(numpy.polyadd(self.num, self.den * rhs), self.den) if isinstance(rhs, scipy.signal.TransferFunction): numer = numpy.polyadd(numpy.polymul(self.num, rhs.den), numpy.polymul(self.den, rhs.num)) denom = numpy.polymul(self.den, rhs.den) return TransferFunction(numer, denom) raise TypeError
def __rsub__(self, lhs): if isinstance(lhs, numbers.Number): return TransferFunction(numpy.polyadd(-self.num, self.den * lhs), self.den) if isinstance(lhs, scipy.signal.TransferFunction): numer = numpy.polyadd(numpy.polymul(lhs.num, self.den), -numpy.polymul(self.num, lhs.den)) denom = numpy.polymul(self.den, lhs.den) return TransferFunction(numer, denom) raise TypeError
def compute_join_curvature( px, py ): from scipy.integrate import quad xp = polyder( px, 1 ) xpp = polyder( px, 2 ) yp = polyder( py, 1 ) ypp = polyder( py, 2 ) pn = polyadd( polymul( xp, ypp ), polymul( yp, xpp )) #numerator pd = polyadd( polymul( xp, xp ) , polymul( yp, yp ) ) #denominator integrand = lambda t: fabs(polyval( pn, t )/( polyval( pd, t )**(1.5)) ) return quad(integrand, 0, 1) [0]
def __sub__(self, other): """substraction""" if type(other) in [int, float]: return dTF(polyadd(self._num.A1, -self._den.A1 * other), self._den) if isinstance(other, dTF): numer = polyadd(polymul(self._num.A1, other.den.A1), -polymul(self._den.A1, other.num.A1)) denom = polymul(self._den.A1, other.den.A1) return dTF(numer, denom) raise ValueError("Cannot substract a dTF with a %s", type(other))
def _parallel(self, other): dt = _pickup_dt(self, other) if np.array_equal(self.den, other.den): den = self.den num = np.polyadd(self.num, other.num) else: den = np.convolve(self.den, other.den) num = np.polyadd(np.convolve(self.num, other.den), np.convolve(other.num, self.den)) return TransferFunction(num, den, dt=dt)
def deriv4_6_8_10(self, t, numout=4): """Returns 4th, 6th, 8th and 10th derivatives of the kernel function. """ phi0 = exp(-0.5 * t ** 2) / sqrt(2 * pi) pn = [1, 0, -6, 0, 3] out = [np.polyval(pn, t) * phi0] for _i in range(numout - 1): pnp1 = np.polyadd(-np.r_[pn, 0], np.polyder(pn)) pnp2 = np.polyadd(-np.r_[pnp1, 0], np.polyder(pnp1)) out.append(np.polyval(pnp2, t) * phi0) pn = pnp2 return tuple(out)
def Encrypt(h, msg): global q global t e = reduce(ChiErr(2)) s = reduce(ChiErr(2)) # print("e = ", e) # print("s = ", s) delta = math.floor(q / t) c = [delta * x for x in msg] c = np.polyadd(c, e) H = reduce(np.polymul(h, s)) c = np.polyadd(c, H) return reduce(c)
def ss2tf(sys_): """ Covert state space model to transfer function model. Only for SISO now a(s) = det(sI - A) R_{n-1} = I R_{n-2} = R_{n-1} * A + a_{n-1} * I E_{n-1} = C * R_{n-1} * B G(s) = (E_{n-1}s^{n-1} + E_{n-2}s^{n-2} + ... + E_0) / a(s) + D :param sys_: system :type sys_: StateSpace :return: corresponded transfer function model :rtype: TransferFunction """ n = sys_.A.shape[0] p = sys_.B.shape[1] q = sys_.C.shape[0] cp = np.poly(sys_.A) # characteristic polynomial I = np.eye(n) R = I E = np.empty((n, q * p)) for i in range(n): E[i] = (sys_.C @ R @ sys_.B).ravel('C') R = R @ sys_.A + cp[i + 1] * I # every row in E presents coefficients of each numerator polynomial E = E.T # make sure every element in D correspond to the very E's row D = sys_.D.flatten('C').T if sys_.is_siso: num = np.polyadd(E[0], cp * D[0]) return TransferFunction(num, cp, dt=sys_.dt) else: # return mimo as a list of TransferFunction temporarily ret = [] r = [] for i in range(q * p): num = np.polyadd(E[i], cp * D[i]) r.append(TransferFunction(num, cp, dt=sys_.dt)) if (i + 1) % p == 0: ret.append(r) r = [] return ret
def root_curvature(w,side,dx,n=16): n = min(n, len(w.x)/4) L = cumulative_path_length(w) tt = L/L.max() teval = tt[n] if side==0 else tt[-n] px = np.polyfit(tt[n:-n],w.x[n:-n],2) py = np.polyfit(tt[n:-n],w.y[n:-n],2) xp = np.polyder( px, 1 ) xpp = np.polyder( px, 2 ) yp = np.polyder( py, 1 ) ypp = np.polyder( py, 2 ) pn = np.polyadd( polymul( xp, ypp ), np.polymul( yp, xpp )) #numerator pd = np.polyadd( polymul( xp, xp ) , np.polymul( yp, yp ) ) #denominator kappa = lambda t: np.polyval( pn, t )/( np.polyval( pd, t )**(1.5)) # d Tangent angle/ds return dx*kappa(teval)
def mean_curvature(w, side, dx, n=16): n = min(n, len(w.x) / 4) L = cumulative_path_length(w) tt = L / L.max() teval = tt[n] if side == 0 else tt[-n] px = np.polyfit(tt[n:-n], w.x[n:-n], 2) py = np.polyfit(tt[n:-n], w.y[n:-n], 2) xp = np.polyder(px, 1) xpp = np.polyder(px, 2) yp = np.polyder(py, 1) ypp = np.polyder(py, 2) pn = np.polyadd(np.polymul(xp, ypp), np.polymul(yp, xpp)) # numerator pd = np.polyadd(np.polymul(xp, xp), np.polymul(yp, yp)) # denominator kappa = lambda t: dx * np.polyval(pn, t) / (np.polyval(pd, t) ** (1.5)) # d Tangent angle/ds * ds/dt return quad(kappa, 0, 1, epsrel=1e-3)[0]
def X2O(E, F): ''' Forms the X2O (X of port 2(output), Open) polynomial For the case P(lambda) is even ''' Ee, Eo = Even_Odd_Parts(E) Fe, Fo = Even_Odd_Parts(F) X2On = np.polyadd(Ee, Fe) X2On = np.trim_zeros(X2On, 'f') X2Od = np.polyadd(Eo, Fo) X2Od = np.trim_zeros(X2Od, 'f') return Ee, Eo, Fe, Fo, X2On, X2Od
def Encrypt(h, msg): global q global t e = reduce(ChiErr(9)) #This shouldn't be 2, it should be some s = reduce(ChiKey(9)) #dependent of d, no? print("e = ", e) # print("s = ", s) delta = math.floor(q / t) c = [delta * x for x in msg] c = np.polyadd(c, e) H = reduce(np.polymul(h, s)) c = np.polyadd(c, H) return reduce(c)
def __rsub__(self, other): if type(other) in [int, float]: return ExtendedTF(polyadd(-self.num, self.den * other), self.den, dt=self._dt) if type(other) in [TransFun, type(self)]: if len(self.den) == len( other.den) and np.all(self.den == other.den): numer = polyadd(self.num, -other.num) denom = self.den else: numer = polyadd(polymul(self.num, other.den), -polymul(self.den, other.num)) denom = polymul(self.den, other.den) return ExtendedTF(numer, denom, dt=self._dt)
def plot(low, high, count, func, title): x, y = Calculator._ys(low, high, count, func) coeff_vector = Calculator.getNDDCoeffs(x, y) final_pol = np.polynomial.Polynomial([0.]) # our target polynomial n = coeff_vector.shape[0] # get number of coeffs for i in range(n): p = np.polynomial.Polynomial([1.]) # create a dummy polynomial for j in range(i): # each vector has degree of i # their terms are dependant on 'x' values p_temp = np.polynomial.Polynomial([-x[j], 1.]) # (x - x_j) p = np.polymul(p, p_temp) # multiply dummy with expression p *= coeff_vector[i] # apply coefficient final_pol = np.polyadd(final_pol, p) # add to target polynomial p = np.flip(final_pol[0].coef, axis=0) # Evaluate polynomial at X axis and plot result x_axis = np.linspace(low, high, num=5000) y_axis = np.polyval(p, x_axis) x_red = np.linspace(low, high, num=5000) y_red = [func(x) for x in np.linspace(low, high, num=5000)] plt.plot(x_axis, y_axis) plt.plot(x_red, np.array(y_red)) plt.title(title) plt.show()
def add_using_polynom(self,c1,c2): """Adds using two polynoms from GaloisField """ sum_of_polynoms = np.polyadd(c1,c2) sum_of_polynoms_upgraded = [] for element in sum_of_polynoms: sum_of_polynoms_upgraded.append((int(element))%self.prime) return tuple(sum_of_polynoms_upgraded)
def E(F, P): ''' Forms the Hurwitz polynomial E(lambda) ''' #Generate F(jw)*F(-jw) and P(jw)*p(-jw) for n even #nX = X(-jw). Changes sign of coefficients of odd order only. nF = np.copy(F) nP = np.copy(P) #Odd order indexing from penultimate element while stepping by 2 in reverse. nF[-2::-2] = -1 * nF[-2::-2] nP[-2::-2] = -1 * nP[-2::-2] FnF = np.polymul(F, nF) PnP = np.polymul(P, nP) #Form (E)(nE)=(F)(nF)+(P)(nP). Page 287. EQN(14), EQN(15) EnE = np.polyadd(FnF, PnP) E = np.array([1]) for root in np.roots(EnE): if np.real(root) < 0: E = np.polymul(E, np.array([1, -1 * root])) E = np.real(E) return E, FnF, PnP
def feedback(self, other=1, sign=-1): """Feedback interconnection between two LTI objects.""" other = _convertToTransferFunction(other) if (self.inputs > 1 or self.outputs > 1 or other.inputs > 1 or other.outputs > 1): # TODO: MIMO feedback raise NotImplementedError("TransferFunction.feedback is currently \ only implemented for SISO functions.") # Figure out the sampling time to use if (self.dt is None and other.dt is not None): dt = other.dt # use dt from second argument elif (other.dt is None and self.dt is not None) \ or (self.dt == other.dt): dt = self.dt # use dt from first argument else: raise ValueError("Systems have different sampling times") num1 = self.num[0][0] den1 = self.den[0][0] num2 = other.num[0][0] den2 = other.den[0][0] num = polymul(num1, den2) den = polyadd(polymul(den2, den1), -sign * polymul(num2, num1)) return TransferFunction(num, den, dt)
def error(plant, sensor=None, entrada=None): """Negative feedback connection of plant and sensor. If sensor is None, then it is assumed to be 1. """ if not isinstance(plant, signal.lti): plant = signal.lti(*plant) if sensor is None: sensor = signal.lti([1], [1]) elif not isinstance(sensor, signal.lti): sensor = signal.lti(*sensor) if entrada is None: entrada = signal.lti([1], [1]) elif not isinstance(entrada, signal.lti): entrada = signal.lti(*entrada) # aux = np.polymul(plant.den, sensor.den) num = np.polymul(np.polymul(plant.den, sensor.den),entrada.num) den = np.polyadd(np.polymul(np.polymul(plant.den, sensor.den),entrada.den), np.polymul(np.polymul(plant.num, sensor.num),entrada.den)) sys = signal.lti(num, den) return sys
def Closed_loop(Kz, Kp, Gz, Gp): """ Return zero and pole polynomial for a closed loop function. Parameters ---------- Kz & Gz : list Polynomial constants in the numerator. Kz & Gz : list Polynomial constants in the denominator. Returns ------- Zeros_poly : list List of zero polynomial for closed loop function. Poles_poly : list List of pole polynomial for closed loop function. """ # calculating the product of the two polynomials in the numerator # and denominator of transfer function GK Z_GK = numpy.polymul(Kz, Gz) P_GK = numpy.polymul(Kp, Gp) # calculating the polynomial of closed loop # sensitivity function s = 1/(1+GK) Zeros_poly = Z_GK Poles_poly = numpy.polyadd(Z_GK, P_GK) return Zeros_poly, Poles_poly
def sweep(self, wireFrameProjection, frame, lines, color=[0, 0, 255], sweepThick=5, fullsweepFrame=104): # calculate sweep angle halfcycle = fullsweepFrame / 2 position = (frame % fullsweepFrame) if position > halfcycle: position = fullsweepFrame - position sweep = position / halfcycle allY = [n * 32 for n in range(int(self.projectedY / 32))] # calculate the wireframe positions nlanes = len(lines) - 1 leftPolynomial = np.poly1d(lines[0].currentFit) rightPolynomial = np.poly1d(lines[nlanes].currentFit) # scanning sweep polySweepDiff = np.polysub( lines[nlanes].currentFit, lines[0].currentFit) * sweep sweepPoly = np.polyadd(leftPolynomial, polySweepDiff) allX = sweepPoly(allY) XYPolyline = np.column_stack((allX, allY)).astype(np.int32) cv2.polylines(wireFrameProjection, [XYPolyline], 0, color, sweepThick) sweepLane = 0 for i in range(nlanes): leftLine = np.poly1d(lines[i].currentFit) rightLine = np.poly1d(lines[i+1].currentFit) if (leftLine([self.projectedY])[0] < sweepPoly([self.projectedY])[0] and sweepPoly([self.projectedY])[0] < rightLine([self.projectedY])[0]): sweepLane = i return sweepLane
def createPolyFitRight(self, curImgFtr, leftLane, faint=1.0, resized=False): """ create adjacent lane lines using an existing lane on the left :param curImgFtr: :param leftLane: :param faint: :param resized: :return: """ # create new right line polynomial polyDiff = np.polysub(leftLane.lines[leftLane.right].currentFit, leftLane.lines[leftLane.left].currentFit) self.currentFit = np.polyadd(leftLane.lines[leftLane.right].currentFit, polyDiff) polynomial = np.poly1d(self.currentFit) self.allY = leftLane.lines[leftLane.right].allY self.currentX = polynomial(self.allY) self.allX = self.currentX if len(self.allY) > 75: # We need to increase our pixel count by 2 to get to 100% # confidence and maintain the current pixel count to keep # the line detection self.confidence_based = len(self.allY) * 2 self.confidence = len(self.allY) / self.confidence_based self.detected = True # create linepoly xy1 = np.column_stack((self.currentX + self.maskDelta, self.allY)) xy1 = xy1.astype(np.int32) xy2 = np.column_stack((self.currentX - self.maskDelta, self.allY)) xy2 = xy2.astype(np.int32) self.linePoly = np.concatenate((xy1, xy2[::-1]), axis=0) # create mask self.linemask = np.zeros_like(self.linemask) cv2.fillConvexPoly(self.linemask, self.linePoly, 64) # Add the point at the bottom. allY = np.append(self.allY, self.projectedY - 1) allX = polynomial(allY) self.XYPolyline = np.column_stack((allX, allY)) self.XYPolyline = self.XYPolyline.astype(np.int32) # create the accumulator self.bestFit = self.currentFit # classify the line # print("classifying the right line",self.side) self.getLineStats(curImgFtr.getRoadProjection(), faint=faint, resized=resized) # set bottom of line x = polynomial([self.projectedY - 1]) self.pixelBasePos = x[0]
def fwhm_polyest_e(delta_energy, two_theta, alpha, F, e_f): """ Estimate the fwhm profile in an energy dispersive detector. Calculates the fwhm wrt. the measurement accuracy of the detectors, the Fano factor contribution and angle variation (due to slit size and positioning. Args: delta_energy (ndarray): Energy resolution wrt. energy (keV) two_theta (float): 2theta in radians alpha (float): Half the full angular variation. F (float): Fano factor (approx. 0.13 for Germanium) e_f (float): Energy to make electron-hole pair (approx. 3e-3 keV) Returns: ndarray: 1d array containing the estimated polynomial (k=2). """ # d_E^2 used to calc FWHM -> calc polynomial such that d_E^2 = A*E + B if isinstance(delta_energy, (int, float)): res_sq = [0, delta_energy ** 2] else: e, res = delta_energy[:, 0], delta_energy[:, 1] res_sq = np.polyfit(e, [i ** 2 for i in res], 1) # Polynomial terms that should fit fwhm squared fw_base = [(2 * alpha / np.tan(two_theta)) ** 2, F * e_f * 2.35 ** 2, 0] fw_e_sq = np.polyadd(fw_base, res_sq) # Conversion factor to put fwhm squared in terms of q e_q = 1000 * eV * 4 * np.pi * np.sin(two_theta / 2) / (h * c * 1e10) return [fw_e_sq[0], fw_e_sq[1] * e_q, fw_e_sq[2] * e_q ** 2]
def feedback(self): """ Computes T(z)/(1+T(z)) """ num = self.num den = self.den den = polyadd(num, den) self = ExtendedTF(num, den, dt=self.dt) return self
def closest1(point, bezier): close = None distance = np.infty for curve in bezier: x = curve.x_eq y = curve.y_eq x[-1] -= point.x y[-1] -= point.y dist = np.polyadd(np.polymul(x, x), np.polymul(y, y)) d_f = np.polyder(dist) for r in np.roots(d_f): if np.isreal(r) and r >= 0 and r <= 1: p = Point(np.polyval(curve.x_eq, r), np.polyval(curve.y_eq, r)) d = p.dist(point) if d <= distance: close = curve distance = d if close.color == (0, 0, 0): print("A curva mais próxima é a preta") elif close.color == (255, 0, 0): print("A curva mais próxima é a vermelha") elif close.color == (0, 255, 0): print("A curva mais próxima é a verde") elif close.color == (0, 0, 255): print("A curva mais próxima é a azul") else: print("A curva mais próxima é a roxo") return close
def compute_join_length( px, py, tlow = 0.0, thigh = 1.0 ): from scipy.integrate import quad xp = polyder( px, 1 ) yp = polyder( py, 1 ) xp2 = polymul( xp, xp ) yp2 = polymul( yp, yp ) p = polyadd( xp2, yp2 ) integrand = lambda t: sqrt( polyval( p, t ) ) return quad(integrand, tlow, thigh) [0]
def _addSISO(num1, den1, num2, den2): """Return num/den = num1/den1 + num2/den2. Each numerator and denominator is a list of polynomial coefficients. """ num = polyadd(polymul(num1, den2), polymul(num2, den1)) den = polymul(den1, den2) return num, den
def Closed_loop(Kz, Kp, Gz, Gp): """Kz & Gz is the polynomial constants in the numerator Kp & Gp is the polynomial constants in the denominator""" # calculating the product of the two polynomials in the numerator and denominator of transfer function GK Z_GK = numpy.polymul(Kz, Gz) P_GK = numpy.polymul(Kp, Gp) #calculating the polynomial of closed loop sensitivity function s = 1/(1+GK) Zeros_poly = Z_GK Poles_poly = numpy.polyadd(Z_GK, P_GK) return Zeros_poly, Poles_poly
def invresz(r, p, k, tol=1e-3, rtype='avg'): """Compute b(z) and a(z) from partial fraction expansion: r,p,k If M = len(b) and N = len(a) b(z) b[0] + b[1] z**(-1) + ... + b[M-1] z**(-M+1) H(z) = ------ = ---------------------------------------------- a(z) a[0] + a[1] z**(-1) + ... + a[N-1] z**(-N+1) r[0] r[-1] = --------------- + ... + ---------------- + k[0] + k[1]z**(-1) ... (1-p[0]z**(-1)) (1-p[-1]z**(-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] -------------- + ------------------ + ... + ------------------ (1-p[i]z**(-1)) (1-p[i]z**(-1))**2 (1-p[i]z**(-1))**n See also -------- residuez, poly, polyval, unique_roots """ extra = asarray(k) p, indx = cmplx_sort(p) r = take(r, indx, 0) pout, mult = unique_roots(p, tol=tol, rtype=rtype) p = [] for k in range(len(pout)): p.extend([pout[k]] * mult[k]) a = atleast_1d(poly(p)) if len(extra) > 0: b = polymul(extra, a) else: b = [0] indx = 0 brev = asarray(b)[::-1] for k in range(len(pout)): temp = [] # Construct polynomial which does not include any of this root for l in range(len(pout)): if l != k: temp.extend([pout[l]] * mult[l]) for m in range(mult[k]): t2 = temp[:] t2.extend([pout[k]] * (mult[k] - m - 1)) brev = polyadd(brev, (r[indx] * poly(t2))[::-1]) indx += 1 b = real_if_close(brev[::-1]) return b, a
def invres(r,p,k,tol=1e-3,rtype='avg'): """Compute b(s) and a(s) from partial fraction expansion: r,p,k If M = len(b) and N = len(a) b(s) b[0] x**(M-1) + b[1] x**(M-2) + ... + b[M-1] H(s) = ------ = ---------------------------------------------- a(s) a[0] x**(N-1) + a[1] x**(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 See Also -------- residue, poly, polyval, unique_roots """ extra = k p, indx = cmplx_sort(p) r = take(r,indx,0) pout, mult = unique_roots(p,tol=tol,rtype=rtype) p = [] for k in range(len(pout)): p.extend([pout[k]]*mult[k]) a = atleast_1d(poly(p)) if len(extra) > 0: b = polymul(extra,a) else: b = [0] indx = 0 for k in range(len(pout)): temp = [] for l in range(len(pout)): if l != k: temp.extend([pout[l]]*mult[l]) for m in range(mult[k]): t2 = temp[:] t2.extend([pout[k]]*(mult[k]-m-1)) b = polyadd(b,r[indx]*poly(t2)) indx += 1 b = real_if_close(b) while allclose(b[0], 0, rtol=1e-14) and (b.shape[-1] > 1): b = b[1:] return b, a
def Closed_loop (Kz,Kp,Gz,Gp): """ Kz and Gz are the polynomial constants in the numerator Kp and Gp are the polynomial constants in the denominator""" # calculating the product of the two polynomials in the numerator and denominator of transfer function GK Z_GK =np.polymul(Kz,Gz) P_GK =np.polymul(Kp,Gp) # calculating the polynomial of closed loop function T=(GK/1+GK) Zeros_poly =Z_GK Poles_poly =np.polyadd(Z_GK,P_GK) return Zeros_poly,Poles_poly
def createPolyFitRight(self, curImgFtr, leftLane, faint=1.0, resized=False): # create new right line polynomial polyDiff = np.polysub(leftLane.lines[leftLane.right].currentFit, leftLane.lines[leftLane.left].currentFit) self.currentFit = np.polyadd( leftLane.lines[leftLane.right].currentFit, polyDiff) polynomial = np.poly1d(self.currentFit) self.allY = leftLane.lines[leftLane.right].allY self.currentX = polynomial(self.allY) self.allX = self.currentX if len(self.allY) > 75: # We need to increase our pixel count by 2 to get to 100% # confidence and maintain the current pixel count to keep # the line detection self.confidence_based = len(self.allY) * 2 self.confidence = len(self.allY) / self.confidence_based self.detected = True # create linepoly xy1 = np.column_stack( (self.currentX + self.maskDelta, self.allY)) xy1 = xy1.astype(np.int32) xy2 = np.column_stack( (self.currentX - self.maskDelta, self.allY)) xy2 = xy2.astype(np.int32) self.linePoly = np.concatenate((xy1, xy2[::-1]), axis=0) # create mask self.linemask = np.zeros_like(self.linemask) cv2.fillConvexPoly(self.linemask, self.linePoly, 64) # Add the point at the bottom. allY = np.append(self.allY, self.projectedY - 1) allX = polynomial(allY) self.XYPolyline = np.column_stack((allX, allY)) self.XYPolyline = self.XYPolyline.astype(np.int32) # create the accumulator self.bestFit = self.currentFit # classify the line # print("classifying the right line",self.side) self.getLineStats( curImgFtr.getRoadProjection(), faint=faint, resized=resized) # set bottom of line x = polynomial([self.projectedY - 1]) self.pixelBasePos = x[0]
def __add__(self, other): """An operator overload for adding two terms with ``+``.""" if isinstance(other, Polynomial) and self.transform == other.transform: return Polynomial(coefficients=numpy.polyadd(self.coefficients, other.coefficients), transform=self.transform) elif isinstance(other, Constant): if len(self.coefficients): # pylint: disable=len-as-condition; coefficients might be a NumPy array, where __nonzero__ is not equivalent to len(.) coefficients = list(self.coefficients) coefficients[-1] += other.value return Polynomial(coefficients=coefficients, transform=self.transform) else: return -other else: return super().__add__(other)
def Poly_Zeros_T(Poly_z_K, Poly_p_K, Poly_z_G, Poly_p_G): """Given the polynomial expansion in the denominator and numerator of the controller function K and G then this function return s the poles and zeros of the closed loop transfer function in terms of reference signal the arrays for the input must range from the highest order of the polynomial to the lowest""" Poly_z = numpy.polymul(Poly_z_K, Poly_z_G) Poly_p = numpy.polyadd(numpy.polymul(Poly_p_K, Poly_z_G), numpy.polymul(Poly_p_K, Poly_p_G)) # return the poles and zeros of T Zeros = numpy.roots(Poly_z) Poles = numpy.roots(Poly_p) return Poles, Zeros
def feedback(plant, sensor=None): """Negative feedback connection of plant and sensor. If sensor is None, then it is assumed to be 1. """ if not isinstance(plant, signal.lti): plant = signal.lti(*plant) if sensor is None: sensor = signal.lti([1], [1]) elif not isinstance(sensor, signal.lti): sensor = signal.lti(*sensor) num = np.polymul(plant.num, sensor.den) den = np.polyadd(np.polymul(plant.den, sensor.den), np.polymul(plant.num, sensor.num)) sys = signal.lti(num, den) return sys
def State_Space_mats(Gp_n, Gp_d, kc, ti, td): if ti == 0: Gc_d = 1 Gc_n = [kc * ti * td, (kc * ti), kc] else: Gc_n = [kc * ti * td, (kc * ti), kc] Gc_d = [ti, 0] OL_TF_n = np.polymul(Gp_n, Gc_n) OL_TF_d = np.polymul(Gc_d, Gp_d) CL_TF_n = OL_TF_n CL_TF_d = np.polyadd(OL_TF_d, OL_TF_n) OL_mats = signal.tf2ss(OL_TF_n, OL_TF_d) # Open Loop Transfer Function is converted to State Space CL_mats = signal.tf2ss(CL_TF_n, CL_TF_d) # Closed Loop Transfer Function is converted to State Space return OL_mats, CL_mats
def updatePolyFitRight(self, leftLane): # update new right line polynomial polyDiff = np.polysub(leftLane.lines[leftLane.right].currentFit, leftLane.lines[leftLane.left].currentFit) self.currentFit = np.polyadd( leftLane.lines[leftLane.right].currentFit, polyDiff) polynomial = np.poly1d(self.currentFit) self.allY = leftLane.lines[leftLane.right].allY self.currentX = polynomial(self.allY) self.allX = self.currentX if len(self.allY) > 150: # We need to increase our pixel count by 2 to get to 100% # confidence and maintain the current pixel count to keep # the line detection self.confidence = len(self.allY) / self.confidence_based if self.confidence > 0.5: self.detected = True if self.confidence > 1.0: self.confidence = 1.0 else: self.detected = False # create linepoly xy1 = np.column_stack( (self.currentX + self.maskDelta, self.allY)).astype(np.int32) xy2 = np.column_stack( (self.currentX - self.maskDelta, self.allY)).astype(np.int32) self.linePoly = np.concatenate((xy1, xy2[::-1]), axis=0) # create mask self.linemask = np.zeros_like(self.linemask) cv2.fillConvexPoly(self.linemask, self.linePoly, 64) # Add the point at the bottom. allY = np.append(self.allY, self.projectedY - 1) allX = polynomial(allY) self.XYPolyline = np.column_stack((allX, allY)).astype(np.int32) # create the accumulator self.bestFit = self.currentFit
def wireframe(self, wireFrameProjection, frame, mainLane, color=[255, 255, 255], wireThick=1, sweepThick=5, fullsweepFrame=26): # calculate the wireframe positions nlanes = len(mainLane.lines) - 1 leftPolynomial = np.poly1d(mainLane.lines[0].currentFit) roadleftPolynomial = np.poly1d( mainLane.lines[mainLane.left].currentFit) roadrightPolynomial = np.poly1d( mainLane.lines[mainLane.right].currentFit) rightPolynomial = np.poly1d(mainLane.lines[nlanes].currentFit) delta = (frame * 32) % 128 squares = [] # horizontal lines for i in range(int(self.projectedY / 128)): y1 = 128 * i + delta x1 = leftPolynomial([y1]) x2 = roadleftPolynomial([y1]) x3 = roadrightPolynomial([y1]) x4 = rightPolynomial([y1]) cv2.line(wireFrameProjection, (x1, y1), (x4, y1), color, wireThick * 3) squares.append(((x2, y1), (x3, y1))) # vertical lines allY = [n * 32 for n in range(int(self.projectedY / 32))] polyDiff = np.polysub(mainLane.lines[nlanes].currentFit, mainLane.lines[0].currentFit) / (nlanes * 2) curPoly = leftPolynomial for i in range(nlanes * 2): allX = curPoly(allY) XYPolyline = np.column_stack((allX, allY)).astype(np.int32) cv2.polylines(wireFrameProjection, [ XYPolyline], 0, color, int(wireThick / 4)) curPoly = np.polyadd(curPoly, polyDiff) return squares
def stability_margins(sysdata, returnall=False, epsw=1e-8): """Calculate stability margins and associated crossover frequencies. Parameters ---------- sysdata: LTI system or (mag, phase, omega) sequence sys : LTI system Linear SISO system mag, phase, omega : sequence of array_like Arrays of magnitudes (absolute values, not dB), phases (degrees), and corresponding frequencies. Crossover frequencies returned are in the same units as those in `omega` (e.g., rad/sec or Hz). returnall: bool, optional If true, return all margins found. If false (default), return only the minimum stability margins. For frequency data or FRD systems, only one margin is found and returned. epsw: float, optional Frequencies below this value (default 1e-8) are considered static gain, and not returned as margin. Returns ------- gm: float or array_like Gain margin pm: float or array_loke Phase margin sm: float or array_like Stability margin, the minimum distance from the Nyquist plot to -1 wg: float or array_like Gain margin crossover frequency (where phase crosses -180 degrees) wp: float or array_like Phase margin crossover frequency (where gain crosses 0 dB) ws: float or array_like Stability margin frequency (where Nyquist plot is closest to -1) """ try: if isinstance(sysdata, frdata.FRD): sys = frdata.FRD(sysdata, smooth=True) elif isinstance(sysdata, xferfcn.TransferFunction): sys = sysdata elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3: mag, phase, omega = sysdata sys = frdata.FRD(mag * np.exp(1j * phase * np.pi/180), omega, smooth=True) else: sys = xferfcn._convertToTransferFunction(sysdata) except Exception as e: print (e) raise ValueError("Margin sysdata must be either a linear system or " "a 3-sequence of mag, phase, omega.") # calculate gain of system if isinstance(sys, xferfcn.TransferFunction): # check for siso if not issiso(sys): raise ValueError("Can only do margins for SISO system") # real and imaginary part polynomials in omega: rnum, inum = _polyimsplit(sys.num[0][0]) rden, iden = _polyimsplit(sys.den[0][0]) # test (imaginary part of tf) == 0, for phase crossover/gain margins test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden)) w_180 = np.roots(test_w_180) #print ('1:w_180', w_180) # first remove imaginary and negative frequencies, epsw removes the # "0" frequency for type-2 systems w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)]) #print ('2:w_180', w_180) # evaluate response at remaining frequencies, to test for phase 180 vs 0 resp_w_180 = np.real(np.polyval(sys.num[0][0], 1.j*w_180) / np.polyval(sys.den[0][0], 1.j*w_180)) #print ('resp_w_180', resp_w_180) # only keep frequencies where the negative real axis is crossed w_180 = w_180[np.real(resp_w_180) < 0.0] # and sort w_180.sort() #print ('3:w_180', w_180) # test magnitude is 1 for gain crossover/phase margins test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)), np.polyadd(_polysqr(rden), _polysqr(iden))) wc = np.roots(test_wc) wc = np.real(wc[(np.imag(wc) == 0) * (wc > epsw)]) wc.sort() # stability margin was a bitch to elaborate, relies on magnitude to # point -1, then take the derivative. Second derivative needs to be >0 # to have a minimum test_wstabd = np.polyadd(_polysqr(rden), _polysqr(iden)) test_wstabn = np.polyadd(_polysqr(np.polyadd(rnum,rden)), _polysqr(np.polyadd(inum,iden))) test_wstab = np.polysub( np.polymul(np.polyder(test_wstabn),test_wstabd), np.polymul(np.polyder(test_wstabd),test_wstabn)) # find the solutions, for positive omega, and only real ones wstab = np.roots(test_wstab) #print('wstabr', wstab) wstab = np.real(wstab[(np.imag(wstab) == 0) * (np.real(wstab) >= 0)]) #print('wstab', wstab) # and find the value of the 2nd derivative there, needs to be positive wstabplus = np.polyval(np.polyder(test_wstab), wstab) #print('wstabplus', wstabplus) wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > epsw) * (wstabplus > 0.)]) #print('wstab', wstab) wstab.sort() else: # a bit coarse, have the interpolated frd evaluated again def mod(w): """to give the function to calculate |G(jw)| = 1""" return np.abs(sys.evalfr(w)[0][0]) - 1 def arg(w): """function to calculate the phase angle at -180 deg""" return np.angle(-sys.evalfr(w)[0][0]) def dstab(w): """function to calculate the distance from -1 point""" return np.abs(sys.evalfr(w)[0][0] + 1.) # Find all crossings, note that this depends on omega having # a correct range widx = np.where(np.diff(np.sign(mod(sys.omega))))[0] wc = np.array( [ sp.optimize.brentq(mod, sys.omega[i], sys.omega[i+1]) for i in widx if i+1 < len(sys.omega)]) # find the phase crossings ang(H(jw) == -180 widx = np.where(np.diff(np.sign(arg(sys.omega))))[0] #print('widx (180)', widx, sys.omega[widx]) #print('x', sys.evalfr(sys.omega[widx])[0][0]) widx = widx[np.real(sys.evalfr(sys.omega[widx])[0][0]) <= 0] #print('widx (180,2)', widx) w_180 = np.array( [ sp.optimize.brentq(arg, sys.omega[i], sys.omega[i+1]) for i in widx if i+1 < len(sys.omega) ]) #print('x', sys.evalfr(w_180)[0][0]) #print('w_180', w_180) # find all stab margins? widx = np.where(np.diff(np.sign(np.diff(dstab(sys.omega)))))[0] #print('widx', widx) #print('wstabx', sys.omega[widx]) wstab = np.array([ sp.optimize.minimize_scalar( dstab, bracket=(sys.omega[i], sys.omega[i+1])).x for i in widx if i+1 < len(sys.omega) and np.diff(np.diff(dstab(sys.omega[i-1:i+2])))[0] > 0 ]) #print('wstabf0', wstab) wstab = wstab[(wstab >= sys.omega[0]) * (wstab <= sys.omega[-1])] #print ('wstabf', wstab) # margins, as iterables, converted frdata and xferfcn calculations to # vector for this GM = 1/np.abs(sys.evalfr(w_180)[0][0]) SM = np.abs(sys.evalfr(wstab)[0][0]+1) PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180 if returnall: return GM, PM, SM, w_180, wc, wstab else: return ( (GM.shape[0] or None) and np.amin(GM), (PM.shape[0] or None) and np.amin(PM), (SM.shape[0] or None) and np.amin(SM), (w_180.shape[0] or None) and w_180[GM==np.amin(GM)][0], (wc.shape[0] or None) and wc[PM==np.amin(PM)][0], (wstab.shape[0] or None) and wstab[SM==np.amin(SM)][0])
td = t_d[k] if ti == 0: Gc_d = 1 Gc_n = [kc * ti * td, (kc * ti), kc] else: Gc_n = [kc * ti * td, (kc * ti), kc] Gc_d = [ti, 0] # The process and controller transfer functions are multiplied to make the # open loop TF OL_TF_n = np.polymul(Gp_n, Gc_n) OL_TF_d = np.polymul(Gc_d, Gp_d) CL_TF_n = OL_TF_n CL_TF_d = np.polyadd(OL_TF_d, OL_TF_n) (A, B, C, D) = signal.tf2ss(OL_TF_n, OL_TF_d) # Open Loop Transfer Function is converted to State Space (A_CL, B_CL, C_CL, D_CL) = signal.tf2ss(CL_TF_n, CL_TF_d) # Closed Loop Transfer Function is converted to State Space rootsA = np.array(linalg.eigvals(A_CL)) # step_response = signal.lsim((A,B,C,D),u,t,X0=None,interp=1)[1] # step_responseDDE = DDE(A,B,C,D,t,SP_info,DT) step_responseEuler = Euler(A, B, C, D, t, u, DT) if (rootsA.real < 0).all(): for i in range(0, entries): # Stabilty of the Closed Loop is checked Y = step_responseEuler y[i, k] = Y[i]
w01 = 2*fsig1/fphi B01 = 2*B1/fphi w11 = (np.sqrt(B01**2+4*w01**2)-B01)/2 w21 = (np.sqrt(B01**2+4*w01**2)+B01)/2 hz1 = sp.signal.butter(2, [w11, w21], 'bandpass', output='zpk') w02 = 2*fsig2/fphi B02 = 2*B2/fphi w12 = (np.sqrt(B02**2+4*w02**2)-B02)/2 w22 = (np.sqrt(B02**2+4*w02**2)+B02)/2 hz2 = sp.signal.butter(2, [w12, w22], 'bandpass', output='zpk') hz1_ab = sp.signal.zpk2tf(*hz1) hz2_ab = sp.signal.zpk2tf(*hz2) hz_ab_d = np.polymul(hz1_ab[1], hz2_ab[1]) hz_ab_n1 = np.polymul(hz1_ab[0], hz2_ab[1]) hz_ab_n2 = np.polymul(hz2_ab[0], hz1_ab[1]) hz_ab_n = np.polyadd(hz_ab_n1, hz_ab_n2) hz = sp.signal.tf2zpk(hz_ab_n, hz_ab_d) # Compute impulse response print("...computing impulse response of filter") hz_ir = impulse_response(hz, db=60) # Compute the optimal NTF print("... computing optimal NTF") q0 = q0_from_filter_ir(order, hz_ir) ntf_opti = ntf_fir_from_q0(q0, H_inf=H_inf) # Determine freq values for which plots are created fmin = 10**np.ceil(np.log10(10)) fmax = 10**np.floor(np.log10(fphi/2)) ff = np.logspace(np.log10(fmin), np.log10(fmax), 1000)
def stability_margins(sysdata, deg=True, returnall=False, epsw=1e-10): """Calculate gain, phase and stability margins and associated crossover frequencies. Usage ----- gm, pm, sm, wg, wp, ws = stability_margins(sysdata, deg=True, returnall=False, epsw=1e-10) Parameters ---------- sysdata: linsys or (mag, phase, omega) sequence sys : linsys Linear SISO system mag, phase, omega : sequence of array_like Input magnitude, phase, and frequencies (rad/sec) sequence from bode frequency response data deg=True: boolean If true, all input and output phases in degrees, else in radians returnall=False: boolean If true, return all margins found. Note that for frequency data or FRD systems, only one margin is found and returned. epsw=1e-10: float frequencies below this value are considered static gain, and not returned as margin. Returns ------- gm, pm, sm, wg, wp, ws: float or array_like Gain margin gm, phase margin pm, stability margin sm, and associated crossover frequencies wg, wp, and ws of SISO open-loop. If more than one crossover frequency is detected, returns the lowest corresponding margin. When requesting all margins, the return values are array_like, and all margins are returns for linear systems not equal to FRD """ try: if isinstance(sysdata, frdata.FRD): sys = frdata.FRD(sysdata, smooth=True) elif isinstance(sysdata, xferfcn.TransferFunction): sys = sysdata elif getattr(sysdata, '__iter__', False) and len(sysdata) == 3: mag, phase, omega = sysdata sys = frdata.FRD(mag*np.exp((1j/360.)*phase), omega, smooth=True) else: sys = xferfcn._convertToTransferFunction(sysdata) except Exception as e: print (e) raise ValueError("Margin sysdata must be either a linear system or " "a 3-sequence of mag, phase, omega.") # calculate gain of system if isinstance(sys, xferfcn.TransferFunction): # check for siso if not issiso(sys): raise ValueError("Can only do margins for SISO system") # real and imaginary part polynomials in omega: rnum, inum = _polyimsplit(sys.num[0][0]) rden, iden = _polyimsplit(sys.den[0][0]) # test imaginary part of tf == 0, for phase crossover/gain margins test_w_180 = np.polyadd(np.polymul(inum, rden), np.polymul(rnum, -iden)) w_180 = np.roots(test_w_180) # first remove imaginary and negative frequencies, epsw removes the # "0" frequency for type-2 systems w_180 = np.real(w_180[(np.imag(w_180) == 0) * (w_180 >= epsw)]) # evaluate response at remaining frequencies, to test for phase 180 vs 0 resp_w_180 = np.real(np.polyval(sys.num[0][0], 1.j*w_180) / np.polyval(sys.den[0][0], 1.j*w_180)) # only keep frequencies where the negative real axis is crossed w_180 = w_180[(resp_w_180 < 0.0)] # and sort w_180.sort() # test magnitude is 1 for gain crossover/phase margins test_wc = np.polysub(np.polyadd(_polysqr(rnum), _polysqr(inum)), np.polyadd(_polysqr(rden), _polysqr(iden))) wc = np.roots(test_wc) wc = np.real(wc[(np.imag(wc) == 0) * (wc > epsw)]) wc.sort() # stability margin was a bitch to elaborate, relies on magnitude to # point -1, then take the derivative. Second derivative needs to be >0 # to have a minimum test_wstabn = np.polyadd(_polysqr(rnum), _polysqr(inum)) test_wstabd = np.polyadd(_polysqr(np.polyadd(rnum,rden)), _polysqr(np.polyadd(inum,iden))) test_wstab = np.polysub( np.polymul(np.polyder(test_wstabn),test_wstabd), np.polymul(np.polyder(test_wstabd),test_wstabn)) # find the solutions wstab = np.roots(test_wstab) # and find the value of the 2nd derivative there, needs to be positive wstabplus = np.polyval(np.polyder(test_wstab), wstab) wstab = np.real(wstab[(np.imag(wstab) == 0) * (wstab > epsw) * (np.abs(wstabplus) > 0.)]) wstab.sort() else: # a bit coarse, have the interpolated frd evaluated again def mod(w): """to give the function to calculate |G(jw)| = 1""" return [np.abs(sys.evalfr(w[0])[0][0]) - 1] def arg(w): """function to calculate the phase angle at -180 deg""" return [np.angle(sys.evalfr(w[0])[0][0]) + np.pi] def dstab(w): """function to calculate the distance from -1 point""" return np.abs(sys.evalfr(w[0])[0][0] + 1.) # how to calculate the frequency at which |G(jw)| = 1 wc = np.array([sp.optimize.fsolve(mod, sys.omega[0])])[0] w_180 = np.array([sp.optimize.fsolve(arg, sys.omega[0])])[0] wstab = np.real( np.array([sp.optimize.fmin(dstab, sys.omega[0], disp=0)])[0]) # margins, as iterables, converted frdata and xferfcn calculations to # vector for this PM = np.angle(sys.evalfr(wc)[0][0], deg=True) + 180 GM = 1/(np.abs(sys.evalfr(w_180)[0][0])) SM = np.abs(sys.evalfr(wstab)[0][0]+1) if returnall: return GM, PM, SM, w_180, wc, wstab else: return ( (GM.shape[0] or None) and GM[0], (PM.shape[0] or None) and PM[0], (SM.shape[0] or None) and SM[0], (w_180.shape[0] or None) and w_180[0], (wc.shape[0] or None) and wc[0], (wstab.shape[0] or None) and wstab[0])
def Gen(num_of_gens, Gp_n, Gp_d, SP, tfinal, dt, DT): def plotter(kc,ti,td,x,num,entries,t,tfinal,dt,SP,kcst,tist): global kc_unstable, ti_unstable, td_unstable global kc_offset, ti_offset, td_offset global kc_goodpoints, ti_goodpoints, td_goodpoints global kc_idx, ti_idx, td_idx, xt por,tpr = obj.overshoot(t,x,num,entries,SP) # calculates the risetime tr= obj.risetime(t,x,num,entries,SP) SSoffset = np.isneginf(por) UNSTABLE = np.isnan(por) ISE = obj.ISE(t,x,num,entries,SP) IAE = obj.IAE(t,x,num,entries,SP) ITAE = obj.ITAE(t,x,num,entries,SP) goodpoints = ~(np.isnan(tr)| np.isnan(por)|np.isneginf(por)) idx = np.arange(0,num) tr = tr[goodpoints] por = por[goodpoints] tpr = tpr[goodpoints] ISE = ISE[goodpoints] idx = idx[goodpoints] x = x[goodpoints] xt = x p = pareto.domset([itemgetter(1), itemgetter(2)], zip(idx, por, tr, ISE, IAE, ITAE)) front = p.data idx, ppor, ptr, pise, piae, pitae = map(np.array, zip(*front)) sortidx = np.argsort(ppor) ppor = ppor[sortidx] ptr = ptr[sortidx] pise = pise[sortidx] piae = piae[sortidx] pitae = pitae[sortidx] kc_unstable, ti_unstable, td_unstable = kc[UNSTABLE], ti[UNSTABLE], td[UNSTABLE] kc_offset, ti_offset, td_offset = kc[SSoffset], ti[SSoffset], td[SSoffset] kc_goodpoints, ti_goodpoints, td_goodpoints = kc[goodpoints], ti[goodpoints], td[goodpoints] kc_idx, ti_idx, td_idx = kc[idx], ti[idx], td[idx] t = np.arange(0, tfinal, dt) entries = len(t) num = 20 # number of tuning constant sets y = np.zeros((entries, num)) # Controller choice Contr_type = 'PI' # Choose controller by typing 'P' , 'PI' or 'PID' if Contr_type == 'P': Controller = 1 elif Contr_type == 'PI': Controller = 2 elif Contr_type == 'PID': Controller = 3 [k_c, t_i, t_d] = func.RPG(num, Controller) # Random Parameter Generator # Options: 1= P, 2 = PI, 3 = PID # Different Ysp inputs SP_input = 'step' # Choose Set point input by typing 'step' or 'ramp' step_time = 0 ramp_time = 5. # u = func.Ramp(t,ramp_time,SP) u = func.Step(t, step_time, SP) SP_info = [SP_input, step_time, ramp_time, SP] # coefficients of the transfer function Gp = kp/(s^3 + As^2 + Bs +C) A = 3 B = 3 C = 1 # Old process coefficients used in the initial project kp = 0.125 SP = SP kcst = np.arange(0, 60, dt) # Relatiopnship btwn kc and Ti obtained through the direct substitution method tist = kp * kcst * A ** 2 / (((A * B) - C - (kp * kcst)) * (C + (kp * kcst))) kczn, tizn, tdzn = ZN(Gp_n, Gp_d, t, u, Contr_type, DT) # Ziegler-Nichols settings via function ZN kcch, tich, tdch = Cohen_Coon(Gp_n, Gp_d, t, u, Contr_type, DT) ## System responce for k in range(0, num): if k == num - 1: kc = kczn # Inserting Ziegler-Nicholas parameters ti = tizn td = tdzn elif k == num - 2: # Inserting Cohen-Coon parameters kc = kcch ti = tich td = tdch else: kc = k_c[k] ti = t_i[k] td = t_d[k] if ti == 0: Gc_d = 1 Gc_n = [kc * ti * td, (kc * ti), kc] else: Gc_n = [kc * ti * td, (kc * ti), kc] Gc_d = [ti, 0] # The process and controller transfer functions are multiplied to make the # open loop TF OL_TF_n = np.polymul(Gp_n, Gc_n) OL_TF_d = np.polymul(Gc_d, Gp_d) CL_TF_n = OL_TF_n CL_TF_d = np.polyadd(OL_TF_d, OL_TF_n) (A, B, C, D) = signal.tf2ss(OL_TF_n, OL_TF_d) # Open Loop Transfer Function is converted to State Space (A_CL, B_CL, C_CL, D_CL) = signal.tf2ss(CL_TF_n, CL_TF_d) # Closed Loop Transfer Function is converted to State Space rootsA = np.array(linalg.eigvals(A_CL)) # step_response = signal.lsim((A,B,C,D),u,t,X0=None,interp=1)[1] # step_responseDDE = DDE(A,B,C,D,t,SP_info,DT) step_responseEuler = Euler(A, B, C, D, t, u, DT) if (rootsA.real < 0).all(): for i in range(0, entries): # Stabilty of the Closed Loop is checked Y = step_responseEuler y[i, k] = Y[i] else: y[:, k] = np.NaN k_c[k] = kc t_i[k] = ti kc = k_c ti = t_i td = t_d y = y.T plotter(kc, ti, td, y, num, entries, t, tfinal, dt, SP, kcst, tist) # fig = plt.figure(4) # # plt.plot(t,xt[4][:]) # plt.show() return kc_unstable, ti_unstable, td_unstable, kc_offset, ti_offset, td_offset,kc_goodpoints, ti_goodpoints, td_goodpoints, kc_idx, ti_idx, td_idx
def add(self, u1, u2): return self.adjust(np.polyadd(u1, u2))