def PolyGCD(a: numpy.array, b: numpy.array):
a = numpy.trim_zeros(a)
b = numpy.trim_zeros(b)
if (len(a) < len(b) or (len(a) == 0) or (len(b) == 0)):
return None, None, None
p0 = a[0]
p1 = b[0]
a0, a1 = a / p0, b / p1
s0, s1 = 1 / p0, 0
t0, t1 = 0, 1 / p1
while (len(a1)):
q, r = numpy.polydiv(a0, a1)
if (len(numpy.trim_zeros(a0 - numpy.polymul(a1, q))) == 0):
return a1, s1, t1
a0, a1 = a1, a0 - numpy.polymul(a1, q)
a1 = numpy.trim_zeros(a1)
lu = a1[0]
a1 = a1 / lu
tmp = numpy.trim_zeros(numpy.polymul(s1, q))
if (len(tmp) == 0):
tmp = 0
s0, s1 = s1, (s0 - tmp) / lu
tmpt = numpy.trim_zeros(numpy.polymul(t1, q))
if (len(tmpt) == 0):
tmpt = 0
t0, t1 = t1, (t0 - tmpt) / lu
return a1, s1, t1
def grad_BP(clause, x, k, ctype, FC_table):
grad = []
nv = len(clause)
coef = [FC_table[ci] for ci in range(1, len(clause) + 1)]
if nv < 40:
for i in clause:
x_rest = [
xi / abs(xi) * x[abs(xi) - 1] for xi in clause if xi != i
]
esps = cardcons(x_rest)
grad.append(np.dot(coef, esps) * (abs(i) / i))
return grad
esps = [] # Elementary symetric Polymomials
x_prime = [
-x[abs(clause[i]) - 1] * (clause[i] / abs(clause[i]))
for i in range(len(clause))
]
forward_message = []
backward_message = []
forward_message.append([1])
backward_message.append([1])
for i in range(1, nv):
forward_message.append(
np.polymul(forward_message[i - 1], poly1d([x_prime[i - 1]], True)))
backward_message.append(
np.polymul(backward_message[i - 1], poly1d([x_prime[nv - i]],
True)))
for i in range(1, nv + 1):
esps.append(
np.polymul(forward_message[i - 1], backward_message[nv - i]))
for i in range(nv):
grad.append(np.dot(coef, esps[i].c) * (abs(clause[i]) / clause[i]))
return grad
def mkSystem(fo_r, Q_r, fo_f, bw_f, ord_f=5, nsys = 4, tol_fo_f=0.0, tol_fo_r=0.0):
if ( abs(tol_fo_f) > 0.05 ):
raise RuntimeError("-.05 < tol_fo_f < +.05")
if ( abs(tol_fo_r) > 0.05 ):
raise RuntimeError("-.05 < tol_fo_r < +.05")
if isinstance(fo_r,(tuple,list)):
nsys = len(fo_r)
else:
fo_r = [fo_r for i in range(nsys)]
Q_r = [Q_r for i in range(nsys)]
lsys_list = []
for i in range(nsys):
fo_f_i = fo_f*(1.0 + tol_fo_f*np.random.randn())
fo_r_i = fo_r[i]*(1.0 + tol_fo_r*np.random.randn())
[bf,af] = sig.iirfilter(ord_f, 2*np.pi*np.array([fo_f_i-bw_f/2,fo_f_i+bw_f/2]), rp=0.5, ftype='cheby1', analog=True)
if fo_r[i] > 0.0:
[br,ar] = mkResonator(fo_r_i, Q_r[i])
bsys = np.polymul(br,bf)
asys = np.polymul(ar,af)
else:
bsys = bf
asys = af
try:
lsys=LinSys( bsys,asys )
except RuntimeError as e:
norm = e.args[1]*1.001
print("Normalized numerator dividing by {}!".format(norm))
bsys = bsys/norm
lsys = LinSys(bsys,asys)
lsys_list.append(lsys)
return lsys_list
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 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 _combine_reception_params(in_reception_parameters,
out_reception_parameters):
"""Combine reception parameters to join two signals into one, e.g., for
optimising out a passthrough Node.
"""
# Construct the new reception parameters
# Combine the filters
filter_in = in_reception_parameters.filter
filter_out = out_reception_parameters.filter
if (filter_in is None or filter_out is None):
# If either filter is None then just use the filter from the other
# connection
new_filter = filter_in or filter_out
elif (isinstance(filter_in, nengo.LinearFilter) and
isinstance(filter_out, nengo.LinearFilter)):
# Both filters are linear filters, so multiply the numerators and
# denominators together to get a new linear filter.
new_num = np.polymul(filter_in.num, filter_out.num)
new_den = np.polymul(filter_in.den, filter_out.den)
new_filter = nengo.LinearFilter(new_num, new_den)
else:
raise NotImplementedError
# Take the size in from the second reception parameter, construct the new
# reception parameters.
return ReceptionParameters(new_filter, out_reception_parameters.width)
def get_transfer_function(self):
den = 1
num = 1
for i in self.filters:
den = polymul(den, i.a)
num = polymul(num, i.b)
return Filter(num, den)
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 A_weighting(fs):
"""Design of an A-weighting filter.
b, a = A_weighting(fs) designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = scipy.signal.lfilter(b, a, x).
Warning: `fs` should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2*pi * f4)**2 * (10**(A1000/20)), 0, 0, 0, 0]
DENs = polymul([1, 4*pi * f4, (2*pi * f4)**2],
[1, 4*pi * f1, (2*pi * f1)**2])
DENs = polymul(polymul(DENs, [1, 2*pi * f3]),
[1, 2*pi * f2])
# Use the bilinear transformation to get the digital filter.
# (Octave, MATLAB, and PyLab disagree about Fs vs 1/Fs)
return bilinear(NUMs, DENs, fs)
def concat(self, other):
"""Create new reception parameters by combining this set of reception
parameters with another.
"""
# Combine the filters
if self.filter is None:
new_filter = other.filter
elif other.filter is None:
new_filter = self.filter
elif (isinstance(self.filter, LinearFilter)
and isinstance(other.filter, LinearFilter)):
# Combine linear filters by multiplying their numerators and
# denominators.
new_filter = LinearFilter(
np.polymul(self.filter.num, other.filter.num),
np.polymul(self.filter.den, other.filter.den))
else:
raise NotImplementedError(
"Cannot combine filters of type {} and {}".format(
type(self.filter), type(other.filter)))
# Combine the learning rules
if self.learning_rule is not None and other.learning_rule is not None:
raise NotImplementedError(
"Cannot combine learning rules {} and {}".format(
self.learning_rule, other.learning_rule))
new_learning_rule = self.learning_rule or other.learning_rule
# Create the new reception parameters
return ReceptionParameters(new_filter, other.width, new_learning_rule)
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 ClosedLoop_TF(AOL_TF, Beta_TF, C_s):
n1 = AOL_TF.num[0][0]
d1 = AOL_TF.den[0][0]
n2 = Beta_TF.num[0][0]
d2 = Beta_TF.den[0][0]
n1 = conv_arr2list(n1)
d1 = conv_arr2list(d1)
n2 = conv_arr2list(n2)
d2 = conv_arr2list(d2)
num = np.polymul(conv_list2arr(n1), conv_list2arr(d2))
den1 = np.polymul(conv_list2arr(d1), conv_list2arr(d2))
den2 = np.polymul(conv_list2arr(n1), conv_list2arr(n2))
den1 = conv_arr2list(den1)
den2 = conv_arr2list(den2)
if len(den1) > len(den2):
i = len(den2)
while i < len(den1):
den2 = append2first(den2, 0)
i = i + 1
else:
i = len(den1)
while i < len(den2):
den1 = append2first(den1, 0)
i = i + 1
den = conv_list2arr(add_elm_by_elm(den2, den1))
CL_TF = C_s * mt.tf(num, den)
num_cl = CL_TF.num
den_cl = CL_TF.den
return CL_TF, num_cl[0][0], den_cl[0][0]
def _combine_reception_params(in_reception_parameters,
out_reception_parameters):
"""Combine reception parameters to join two signals into one, e.g., for
optimising out a passthrough Node.
"""
# Construct the new reception parameters
# Combine the filters
filter_in = in_reception_parameters.filter
filter_out = out_reception_parameters.filter
if (filter_in is None or filter_out is None):
# If either filter is None then just use the filter from the other
# connection
new_filter = filter_in or filter_out
elif (isinstance(filter_in, nengo.LinearFilter)
and isinstance(filter_out, nengo.LinearFilter)):
# Both filters are linear filters, so multiply the numerators and
# denominators together to get a new linear filter.
new_num = np.polymul(filter_in.num, filter_out.num)
new_den = np.polymul(filter_in.den, filter_out.den)
new_filter = nengo.LinearFilter(new_num, new_den)
else:
raise NotImplementedError
# Take the size in from the second reception parameter, construct the new
# reception parameters.
return ReceptionParameters(new_filter, out_reception_parameters.width)
def __mul__(self, other):
if type(other) in [int, float]:
return ltimul(self.num * other, self.den)
elif type(other) in [TransFun, ltimul]:
numer = numpy.polymul(self.num, other.num)
denom = numpy.polymul(self.den, other.den)
return ltimul(numer, denom)
def __rtruediv__(self, other):
if type(other) in [int, float]:
return ltimul(other * self.den, self.num)
if type(other) in [TransFun, ltimul]:
numer = numpy.polymul(self.den, other.num)
denom = numpy.polymul(self.num, other.den)
return ltimul(numer, denom)
def Kc(n, p, theta_deg):
#n is even EQN(37) p294
m = n // 2
c, delta_c, wc_p = c_v(n, theta_deg)
#const_c calculation from page 304
const_c = 1 / (delta_c * np.sqrt(1 / p**2 - 1))
#EQN(46) expand for n even
F = np.array([1, 0, 0])
P = np.array([1 / const_c])
#These are the inverse of the transmission zero frequencies
co_tz = np.array([])
for u in range(2, m + 1):
#form numerator of K(lambda) or F(lambda)
F = np.polymul(F, np.array([1, 0, c[2 * u - 1]**2]))
#form denominator of K(lambda) or P(lambda)
P = np.polymul(P, np.array([c[2 * u - 1]**2, 0, 1]))
#collect the coefficient a[v] used in P(lambda)
co_tz = np.append(co_tz, c[2 * u - 1])
return F, P, co_tz, wc_p
def poly_pulverizer(a: np.ndarray, b: np.ndarray):
"""return (g, x, y) such that a*x + b*y = g = gcd(a, b)"""
a = np.array(a)
b = np.array(b)
if a.size < b.size:
a, b = b, a
a = np.trim_zeros(a, trim="f") % 2
b = np.trim_zeros(b, trim="f") % 2
r = np.ndarray((1,))
x1, x2, y1, y2 = 1, 0, 0, 1
while np.trim_zeros(r).size != 0:
q, r = np.polydiv(a, b)
a = b
b = r
x2 = np.polysub(x1, np.polymul(q, x2))
y2 = np.polysub(y1, np.polymul(q, y2))
x1, y1 = x2, y2
a = np.trim_zeros(a % 2, trim="f")
b = np.trim_zeros(b % 2, trim="f")
return b, x2, y2
def __truediv__(self, other):
"""Divide two LTI objects."""
if isinstance(other, (int, float, complex, np.number)):
other = _convertToTransferFunction(other,
inputs=self.inputs,
outputs=self.inputs)
else:
other = _convertToTransferFunction(other)
if (self.inputs > 1 or self.outputs > 1 or other.inputs > 1
or other.outputs > 1):
raise NotImplementedError(
"TransferFunction.__truediv__ is currently \
implemented only for SISO systems.")
# 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")
num = polymul(self.num[0][0], other.den[0][0])
den = polymul(self.den[0][0], other.num[0][0])
return TransferFunction(num, den, dt)
def __rtruediv__(self, other):
if type(other) in [int, float]:
return ExtendedTF(other * self.den, self.num, dt=self._dt)
if type(other) in [TransFun, ExtendedTF]:
numer = polymul(self.den, other.num)
denom = polymul(self.num, other.den)
return ExtendedTF(numer, denom, dt=self._dt)
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 __mul__(self, other):
if type(other) in [int, float]:
return ExtendedTF(self.num * other, self.den, dt=self._dt)
elif type(other) in [TransFun, ExtendedTF]:
numer = polymul(self.num, other.num)
denom = polymul(self.den, other.den)
return ExtendedTF(numer, denom, dt=self._dt)
def __truediv__(self, other):
if type(other) in [int, float]:
return ltimul(self.num, self.den * other)
if type(other) in [TransFun, ltimul]:
numer = polymul(self.num, other.den)
denom = polymul(self.den, other.num)
return ltimul(numer, denom)
def draw_cascade(self):
if self.ui.chk_all_selected.isChecked(): #Graficame PLS
if any(self.stages_list) == True:
A = np.poly1d(1)
B = np.poly1d(1)
for i in range(0, len(self.stages_list)):
if self.stages_list[i]:
temp_list = []
temp_list.append(self.fix(self.sos[i]))
b, a = ss.sos2tf(temp_list)
b = np.poly1d(b)
a = np.poly1d(a)
B = np.polymul(B, b)
A = np.polymul(A, a)
# self.sos=np.insert(self.sos[0],0,ss.tf2sos(B.c, A.c))
# for t in range(0,len(self.stages_list)):
# self.stages_list[t] = False
# self.stages_list[len(self.stages_list)-1] = True
w = np.logspace(np.log10(1),
np.log10(self.furthestPZ() * 100 /
(2 * np.pi)),
num=10000) * 2 * np.pi
bode = ss.bode(ss.TransferFunction(B.c, A.c), w=w)
self.axes_mag.plot(bode[0] / (2 * np.pi),
bode[1],
label='acumulada')
self.axes_mag.set_xscale('log')
self.axes_mag.set_xlabel('Frequency [Hz]')
self.axes_mag.set_ylabel('Magnitude [dB]')
self.axes_mag.minorticks_on()
self.axes_mag.legend(loc='best')
self.canvas_mag.draw()
else:
self.manage_plot()
def concat(self, other):
"""Create new reception parameters by combining this set of reception
parameters with another.
"""
# Combine the filters
if self.filter is None:
new_filter = other.filter
elif other.filter is None:
new_filter = self.filter
elif (isinstance(self.filter, LinearFilter) and
isinstance(other.filter, LinearFilter)):
# Combine linear filters by multiplying their numerators and
# denominators.
new_filter = LinearFilter(
np.polymul(self.filter.num, other.filter.num),
np.polymul(self.filter.den, other.filter.den)
)
else:
raise NotImplementedError(
"Cannot combine filters of type {} and {}".format(
type(self.filter), type(other.filter)))
# Combine the learning rules
if self.learning_rule is not None and other.learning_rule is not None:
raise NotImplementedError(
"Cannot combine learning rules {} and {}".format(
self.learning_rule, other.learning_rule))
new_learning_rule = self.learning_rule or other.learning_rule
# Create the new reception parameters
return ReceptionParameters(new_filter, other.width, new_learning_rule)
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 A_weighting(fs):
"""Design of an A-weighting filter.
b, a = A_weighting(fs) designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = scipy.signal.lfilter(b, a, x).
Warning: `fs` should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2*pi * f4)**2 * (10**(A1000/20)), 0, 0, 0, 0]
DENs = polymul([1, 4*pi * f4, (2*pi * f4)**2],
[1, 4*pi * f1, (2*pi * f1)**2])
DENs = polymul(polymul(DENs, [1, 2*pi * f3]),
[1, 2*pi * f2])
# Use the bilinear transformation to get the digital filter.
# (Octave, MATLAB, and PyLab disagree about Fs vs 1/Fs)
return bilinear(NUMs, DENs, fs)
def __truediv__(self, other):
"""Divide two LTI objects."""
if isinstance(other, (int, float, complex)):
other = _convertToTransferFunction(
other, inputs=self.inputs,
outputs=self.inputs)
else:
other = _convertToTransferFunction(other)
if (self.inputs > 1 or self.outputs > 1 or
other.inputs > 1 or other.outputs > 1):
raise NotImplementedError(
"TransferFunction.__truediv__ is currently \
implemented only for SISO systems.")
# 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")
num = polymul(self.num[0][0], other.den[0][0])
den = polymul(self.den[0][0], other.num[0][0])
return TransferFunction(num, den, dt)
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 L(self):
# Returns the loop transfer function, L = PK given a plant and controller
# transfer function (scipy.signal.TransferFunction objects)
num = np.polymul(self.P.num, self.C.num)
den = np.polymul(self.P.den, self.C.den)
return signal.TransferFunction(num, den)
def Ka(n, p, theta_deg):
#n is odd, EQN(37) p294
m = (n - 1) // 2
a, delta_a, wc_p = a_v(n, theta_deg)
#const_c calculation from page 304
const_c = 1 / (delta_a * np.sqrt(1 / p**2 - 1))
#EQN(37) expand for n odd
F = np.array([1, 0])
P = np.array([1 / const_c])
#These are the inverse of the transmission zero frequencies
co_tz = np.array([])
for u in range(1, m + 1):
#form numerator of K(lambda) or F(lambda)
F = np.polymul(F, [1, 0, a[2 * u]**2])
#form denominator of K(lambda) or P(lambda)
P = np.polymul(P, [a[2 * u]**2, 0, 1])
#collect the coefficient a[v] used in P(lambda)
co_tz = np.append(co_tz, a[2 * u])
return F, P, co_tz, wc_p
def __init__(self, pos_init, dir_init, alpha_init, pos_final, dir_final, alpha_final):
delta_pos = pos_final - pos_init - alpha_init * dir_init
delta_vel = alpha_final * dir_final - alpha_init * dir_init
# path coefficients
self.coeffs_pos = np.zeros(shape=(2, 4), dtype=float)
for i in range(0, 2):
c_temp = np.dot(np.array([[-2.0, 1.0], [3.0, -1.0]]), np.array([delta_pos[i], delta_vel[i]]))
self.coeffs_pos[i, 0] = c_temp[0]
self.coeffs_pos[i, 1] = c_temp[1]
self.coeffs_pos[i, 2] = alpha_init * dir_init[i]
self.coeffs_pos[i, 3] = pos_init[i]
self.coeffs_vel = np.array([np.polyder(self.coeffs_pos[0,:]),
np.polyder(self.coeffs_pos[1,:])])
self.coeffs_acc = np.array([np.polyder(self.coeffs_vel[0,:]),
np.polyder(self.coeffs_vel[1,:])])
self.coeffs_jerk = np.array([np.polyder(self.coeffs_acc[0,:]),
np.polyder(self.coeffs_acc[1,:])])
# curvature coefficients
self.coeffs_num = np.polysub(np.polymul(self.coeffs_vel[0, :], self.coeffs_acc[1, :]),
np.polymul(self.coeffs_vel[1, :], self.coeffs_acc[0, :]))
self.coeffs_denom = np.polyadd(np.polymul(self.coeffs_vel[0, :], self.coeffs_vel[0, :]),
np.polymul(self.coeffs_vel[1, :], self.coeffs_vel[1, :]))
# roots for minimizing curvature
self.roots_init = False
def Kb(n, p, theta_deg):
#EQN(39) for n even
m = n // 2
bP, bS, delta_b, wc_p = b_v(n, theta_deg)
#c calculation from page 304
const_c = 1 / (delta_b * np.sqrt(1 / p**2 - 1))
#EQN(43) expand for n even
F = np.array([1, 0, (bP[1]**2)])
P = np.array([1 / const_c])
#These are the inverse of the transmission zero frequencies
co_tz = np.array([])
for u in range(2, m + 1):
#form numerator of K(lambda) or F(lambda)
F = np.polymul(F, np.array([1, 0, bP[2 * u - 1]**2]))
#form denominator of K(lambda) or P(lambda)
P = np.polymul(P, np.array([bS[2 * u - 1]**2, 0, 1]))
#collect the constants bS[2*u-1] used in P(lambda)
co_tz = np.append(co_tz, bS[2 * u - 1])
return F, P, co_tz, wc_p
def decascade(self, ncascade=1):
'''
Reduces cascades of low order filters into smaller cascades of high order filters.
``ncascade`` is the number of cascaded filters to use, which should be
a divisor of the original number.
Note that higher order filters are often numerically unstable.
'''
n, m, p = self.filt_b.shape
if p%ncascade!=0:
raise ValueError('Number of cascades must be a divisor of original number of cascaded filters.')
b = np.zeros((n, (m-1)*(p/ncascade)+1, ncascade))
a = np.zeros((n, (m-1)*(p/ncascade)+1, ncascade))
for i in range(n):
for k in range(ncascade):
bp = np.ones(1)
ap = np.ones(1)
for j in range(k*(p/ncascade), (k+1)*(p/ncascade)):
bp = np.polymul(bp, self.filt_b[i, ::-1, j])
ap = np.polymul(ap, self.filt_a[i, ::-1, j])
bp = bp[::-1]
ap = ap[::-1]
a0 = ap[0]
ap /= a0
bp /= a0
b[i, :len(bp), k] = bp
a[i, :len(ap), k] = ap
self.filt_b = np.array(b, order='F')
self.filt_a = np.array(a, order='F')
self.filt_state = np.zeros((b.shape[0], b.shape[1], b.shape[2]), order='F')
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 get_TF(zeros, poles, constant):
num_fact = []
den_fact = []
i = 0
while i < len(zeros):
num_fact.insert(i, [1, -1 * zeros[i]])
i = i + 1
i = 0
while i < len(poles):
den_fact.insert(i, [1, -1 * poles[i]])
i = i + 1
i = 0
num = [1]
den = [1]
while i < (len(zeros)):
num = np.polymul(num, num_fact[i])
i = i + 1
i = 0
while i < len(poles):
den = np.polymul(den, den_fact[i])
i = i + 1
H_s = constant * mt.tf(num, den)
# print(H_s)
d, n = [], []
for i in range(0, len(num)):
n.append(num[i].real * constant)
for i in range(0, len(den)):
d.append(den[i].real)
return H_s, n, d
def traiter_situation_1():
sx = recuperer_valeur()
sx = np.array(sx)
gx = np.array([1.0, 1.0])
xd = np.array([1.0, 0.0])
rx = np.polydiv(np.polymul(sx, xd), gx)[1][0]
print("CRC: R(x)= ", int(rx))
print("T(x)= " + str(np.concatenate((sx, np.array([int(i) for i in np.polydiv(np.polymul(sx, xd), gx)[0]])))))
def _poly_iw_real_crossing(num_iw, den_iw, epsw):
# Return w where imag(H(iw)) == 0
test_w = np.polysub(np.polymul(num_iw.imag, den_iw.real),
np.polymul(num_iw.real, den_iw.imag))
w = np.roots(test_w)
w = np.real(w[np.isreal(w)])
w = w[w >= epsw]
return w
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 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 make_C_weighting(fs): den = p1 den = np.polymul(den, p1) den = np.polymul(den, p4) den = np.polymul(den, p4) num = np.poly1d([(2.*np.pi*f4)**2, 0, 0]) B, A = signal.bilinear(num, den, fs) return B, A
def mls_polynomial_coefficients(rho, degree):
"""Determine the coefficients for a MLS polynomial smoother
Parameters
----------
rho : {float}
Spectral radius of the matrix in question
degree : {int}
Degree of polynomial coefficients to generate
Returns
-------
Tuple of arrays (coeffs,roots) containing the
coefficients for the (symmetric) polynomial smoother and
the roots of polynomial prolongation smoother.
The coefficients of the polynomial are in descending order
References
----------
.. [1] Parallel multigrid smoothing: polynomial versus Gauss--Seidel
M. F. Adams, M. Brezina, J. J. Hu, and R. S. Tuminaro
J. Comp. Phys., 188 (2003), pp. 593--610
Examples
--------
>>> from pyamg.relaxation.chebyshev import mls_polynomial_coefficients
>>> mls = mls_polynomial_coefficients(2.0, 2)
>>> print mls[0] # coefficients
[ 6.4 -48. 144. -220. 180. -75.8 14.5]
>>> print mls[1] # roots
[ 1.4472136 0.5527864]
"""
# std_roots = np.cos(np.pi * (np.arange(degree) + 0.5)/ degree)
# print std_roots
roots = rho/2.0 * (1.0 - np.cos(2*np.pi*(np.arange(degree,
dtype='float64') + 1)/(2.0*degree+1.0)))
# print roots
roots = 1.0/roots
# S_coeffs = list(-np.poly(roots)[1:][::-1])
S = np.poly(roots)[::-1] # monomial coefficients of S error propagator
SSA_max = rho/((2.0*degree+1.0)**2) # upper bound spectral radius of S^2A
S_hat = np.polymul(S, S) # monomial coefficients of \hat{S} propagator
S_hat = np.hstack(((-1.0/SSA_max)*S_hat, [1]))
# coeff for combined error propagator \hat{S}S
coeffs = np.polymul(S_hat, S)
coeffs = -coeffs[:-1] # coeff for smoother
return (coeffs, roots)
def residue(b,a,tol=1e-3,rtype='avg'):
"""Compute partial-fraction expansion of b(s) / a(s).
If M = len(b) and N = len(a)
b(s) b[0] s**(M-1) + b[1] s**(M-2) + ... + b[M-1]
H(s) = ------ = ----------------------------------------------
a(s) a[0] s**(N-1) + a[1] s**(N-2) + ... + a[N-1]
r[0] r[1] r[-1]
= -------- + -------- + ... + --------- + k(s)
(s-p[0]) (s-p[1]) (s-p[-1])
If there are any repeated roots (closer than tol), then the partial
fraction expansion has terms like
r[i] r[i+1] r[i+n-1]
-------- + ----------- + ... + -----------
(s-p[i]) (s-p[i])**2 (s-p[i])**n
See also: invres, poly, polyval, unique_roots
"""
b,a = map(asarray,(b,a))
k,b = polydiv(b,a)
p = roots(a)
r = p*0.0
pout, mult = unique_roots(p,tol=tol,rtype=rtype)
p = []
for n in range(len(pout)):
p.extend([pout[n]]*mult[n])
p = asarray(p)
# Compute the residue from the general formula
indx = 0
for n in range(len(pout)):
bn = b.copy()
pn = []
for l in range(len(pout)):
if l != n:
pn.extend([pout[l]]*mult[l])
an = atleast_1d(poly(pn))
# bn(s) / an(s) is (s-po[n])**Nn * b(s) / a(s) where Nn is
# multiplicity of pole at po[n]
sig = mult[n]
for m in range(sig,0,-1):
if sig > m:
# compute next derivative of bn(s) / an(s)
term1 = polymul(polyder(bn,1),an)
term2 = polymul(bn,polyder(an,1))
bn = polysub(term1,term2)
an = polymul(an,an)
r[indx+m-1] = polyval(bn,pout[n]) / polyval(an,pout[n]) \
/ factorial(sig-m)
indx += sig
return r, p, k
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 Rule_7(w_start, w_end):
# Rule 7 determining the phase of GGm at -180deg
# this is solved visually from a plot
w = np.logspace(w_start, w_end, 1000)
Pz = np.polymul(G()[0], Gm()[0])
Pp = np.polymul(G()[1], Gm()[1])
[w, h] = scs.freqs(Pz, Pp, w)
plt.semilogx(w, (180 / np.pi) * (phase(h) + w * Time_Delay()))
plt.show()
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 series(sys1, sys2):
"""Series connection of two systems.
"""
if not isinstance(sys1, signal.lti):
sys1 = signal.lti(*sys1)
if not isinstance(sys2, signal.lti):
sys2 = signal.lti(*sys2)
num = np.polymul(sys1.num, sys2.num)
den = np.polymul(sys1.den, sys2.den)
sys = signal.lti(num, den)
return sys
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 __init__(self, c=4):
c_ops = self._c_ops
if not c in c_ops:
raise ValueError("c should be one of " + str(c_ops))
index = c / 2 - 1
p_1 = (1,)
for _i in xrange(self._coefs_1_orders[index]):
p_1 = np.polymul(p_1, self._coefs_1)
p_2 = self._coefs_2s[index]
p = np.polymul(p_1, p_2)
self.poly = np.poly1d(p)
self._diff_order = -1
self.set_diff_order(0)
def A_weighting(fs):
"""
Design of an A-weighting filter.
Designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = lfilter(b, a, x).
Warning: fs should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
fs : float
Sampling frequency
Since this uses the bilinear transform, frequency response around fs/2 will
be inaccurate at lower sampling rates. A-weighting is undefined above
20 kHz, though.
Example:
from scipy.signal import freqz
import matplotlib.pyplot as plt
fs = 200000 # change to 48000 to see truncation
b, a = A_weighting(fs)
f = np.logspace(np.log10(10), np.log10(fs/2), 1000)
w = 2*pi * f / fs
w, h = freqz(b, a, w)
plt.semilogx(w*fs/(2*pi), 20*np.log10(abs(h)))
plt.grid(True, color='0.7', linestyle='-', which='both', axis='both')
plt.axis([10, 100e3, -50, 20])
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2*pi * f4)**2 * (10**(A1000/20)), 0, 0, 0, 0]
DENs = polymul([1, 4*pi * f4, (2*pi * f4)**2],
[1, 4*pi * f1, (2*pi * f1)**2])
DENs = polymul(DENs, [1, 2*pi * f3])
DENs = polymul(DENs, [1, 2*pi * f2])
# Analog confirmed to match https://en.wikipedia.org/wiki/A-weighting#A_2
# Use the bilinear transformation to get the digital filter.
return bilinear(NUMs, DENs, fs)
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 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 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 combine(self, obj):
"""Combine in series with another LinearFilter."""
if not isinstance(obj, LinearFilter):
raise ValidationError(
"Can only combine with other LinearFilters", attr='obj')
if self.analog != obj.analog:
raise ValidationError(
"Cannot combine analog and digital filters", attr='obj')
num = np.polymul(self.num, obj.num)
den = np.polymul(self.den, obj.den)
return LinearFilter(num, den,
analog=self.analog,
default_size_in=self.default_size_in,
default_size_out=self.default_size_out,
default_dt=self.default_dt,
seed=self.seed)
def solve(self, A):
f = np.array([2.0,0.0,-1.0,1.0])
f = np.poly1d(f)
g = np.array(A)
g = np.poly1d(g)
f = np.polymul(f, g)
return f
def change_exponent(self, _pow):
""" Change exponent
Multiply top and bottom by an integer multiple of the
self.denom_poly.
Examples
--------
>>> import numpy
>>> b = ECquasi([3,4,20], m=30, exponent=4)
>>> x = numpy.linspace(0,1,101)
>>> c = b.change_exponent(3)
>>> c
ECquasi(array([ 1.11111111e-04, 1.48148148e-04, 1.07407407e-02,
1.33333333e-02, 3.66666667e-01, 4.00000000e-01,
5.00000000e+00, 4.00000000e+00, 2.00000000e+01]), m=30.000000, exponent=7.000000)
>>> numpy.allclose(c(x), b(x))
True
"""
if np.isfinite(self.m):
_denom_poly = self.denom_poly()
if int(_pow) != _pow or _pow < 0:
raise ValueError('expecting a non-negative integer')
p = _denom_poly**int(_pow)
exponent = self.exponent + _pow
coeffs = np.polymul(self, p).coeffs
return ECquasi(coeffs, exponent=exponent, m=self.m)
else:
return ECquasi(self.coeffs, exponent=self.exponent, m=self.m)
def __rmul__(self, other):
"""Right multiply two LTI objects (serial connection)."""
# Convert the second argument to a transfer function.
if isinstance(other, (int, float, complex)):
other = _convertToTransferFunction(other, inputs=self.inputs,
outputs=self.inputs)
else:
other = _convertToTransferFunction(other)
# Check that the input-output sizes are consistent.
if other.inputs != self.outputs:
raise ValueError("C = A * B: A has %i column(s) (input(s)), but B \
has %i row(s)\n(output(s))." % (other.inputs, self.outputs))
inputs = self.inputs
outputs = other.outputs
# 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")
# Preallocate the numerator and denominator of the sum.
num = [[[0] for j in range(inputs)] for i in range(outputs)]
den = [[[1] for j in range(inputs)] for i in range(outputs)]
# Temporary storage for the summands needed to find the
# (i, j)th element
# of the product.
num_summand = [[] for k in range(other.inputs)]
den_summand = [[] for k in range(other.inputs)]
for i in range(outputs): # Iterate through rows of product.
for j in range(inputs): # Iterate through columns of product.
for k in range(other.inputs): # Multiply & add.
num_summand[k] = polymul(other.num[i][k], self.num[k][j])
den_summand[k] = polymul(other.den[i][k], self.den[k][j])
num[i][j], den[i][j] = _addSISO(
num[i][j], den[i][j],
num_summand[k], den_summand[k])
return TransferFunction(num, den, dt)
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 __div__(self, other):
"""Divide two LTI objects."""
if isinstance(other, (int, float, complex)):
other = _convertToFRD(other, inputs=self.inputs,
outputs=self.inputs, omega=self.omega)
else:
other = _convertToFRD(other, omega=self.omega)
if (self.inputs > 1 or self.outputs > 1 or
other.inputs > 1 or other.outputs > 1):
raise NotImplementedError("FRD.__div__ is currently \
implemented only for SISO systems.")
num = polymul(self.num[0][0], other.den[0][0])
den = polymul(self.den[0][0], other.num[0][0])
return FRD(num, den)
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
