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