def test_simo_round_trip(self):
# See gh-5753
tf = ([[1, 2], [1, 1]], [1, 2])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2]], rtol=1e-13)
assert_allclose(B, [[1]], rtol=1e-13)
assert_allclose(C, [[0], [-1]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 2], [1, 1]], rtol=1e-13)
assert_allclose(den, [1, 2], rtol=1e-13)
tf = ([[1, 0, 1], [1, 1, 1]], [1, 1, 1])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-1, -1], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[-1, 0], [0, 0]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 0, 1], [1, 1, 1]], rtol=1e-13)
assert_allclose(den, [1, 1, 1], rtol=1e-13)
tf = ([[1, 2, 3], [1, 2, 3]], [1, 2, 3, 4])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2, -3, -4], [1, 0, 0], [0, 1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2, 3], [1, 2, 3]], rtol=1e-13)
assert_allclose(D, [[0], [0]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, 2, 3], [0, 1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 2, 3, 4], rtol=1e-13)
tf = ([1, [2, 3]], [1, 6])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6]], rtol=1e-31)
assert_allclose(B, [[1]], rtol=1e-31)
assert_allclose(C, [[1], [-9]], rtol=1e-31)
assert_allclose(D, [[0], [2]], rtol=1e-31)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1], [2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6], rtol=1e-13)
tf = ([[1, -3], [1, 2, 3]], [1, 6, 5])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6, -5], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, -3], [-4, -2]], rtol=1e-13)
assert_allclose(D, [[0], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, -3], [1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6, 5], rtol=1e-13)
def test_simo_round_trip(self):
# See gh-5753
tf = ([[1, 2], [1, 1]], [1, 2])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2]], rtol=1e-13)
assert_allclose(B, [[1]], rtol=1e-13)
assert_allclose(C, [[0], [-1]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 2], [1, 1]], rtol=1e-13)
assert_allclose(den, [1, 2], rtol=1e-13)
tf = ([[1, 0, 1], [1, 1, 1]], [1, 1, 1])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-1, -1], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[-1, 0], [0, 0]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 0, 1], [1, 1, 1]], rtol=1e-13)
assert_allclose(den, [1, 1, 1], rtol=1e-13)
tf = ([[1, 2, 3], [1, 2, 3]], [1, 2, 3, 4])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2, -3, -4], [1, 0, 0], [0, 1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2, 3], [1, 2, 3]], rtol=1e-13)
assert_allclose(D, [[0], [0]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, 2, 3], [0, 1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 2, 3, 4], rtol=1e-13)
tf = ([1, [2, 3]], [1, 6])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6]], rtol=1e-31)
assert_allclose(B, [[1]], rtol=1e-31)
assert_allclose(C, [[1], [-9]], rtol=1e-31)
assert_allclose(D, [[0], [2]], rtol=1e-31)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1], [2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6], rtol=1e-13)
tf = ([[1, -3], [1, 2, 3]], [1, 6, 5])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6, -5], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, -3], [-4, -2]], rtol=1e-13)
assert_allclose(D, [[0], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, -3], [1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6, 5], rtol=1e-13)
def _transform(b, a, Wn, analog, output):
"""
Shift prototype filter to desired frequency, convert to digital with
pre-warping, and return in various formats.
"""
Wn = np.asarray(Wn)
if not analog:
if np.any(Wn < 0) or np.any(Wn > 1):
raise ValueError("Digital filter critical frequencies "
"must be 0 <= Wn <= 1")
fs = 2.0
warped = 2 * fs * tan(pi * Wn / fs)
else:
warped = Wn
# Shift frequency
b, a = lp2lp(b, a, wo=warped)
# Find discrete equivalent if necessary
if not analog:
b, a = bilinear(b, a, fs=fs)
# Transform to proper out type (pole-zero, state-space, numer-denom)
if output in ('zpk', 'zp'):
return tf2zpk(b, a)
elif output in ('ba', 'tf'):
return b, a
elif output in ('ss', 'abcd'):
return tf2ss(b, a)
elif output in ('sos'):
raise NotImplementedError('second-order sections not yet implemented')
else:
raise ValueError('Unknown output type {0}'.format(output))
def tfSim(inputType, mag, wn, zeta, init_xdot, init_x, step_increment,
endtime):
num = [0, 0, wn**2]
den = [1, 2 * zeta * wn, wn**2]
timesteps = np.arange(0, endtime, step_increment)
if inputType == "sine":
u = mag * np.sin(2 * np.pi * timesteps)
elif inputType == "step":
u = mag * np.ones(timesteps.size)
else:
raise ValueException("unrecognized input type")
[A, B, C, D] = tf2ss(num, den)
sys = StateSpace(A, B, C, D)
print("C: {} ; shape: {}".format(C, C.shape))
init_xdot = init_xdot / C[0][1]
init_x = init_x / C[0][1]
init_cond = np.array([init_xdot, init_x])
print(init_cond)
print("initial conditions shape: {}".format(init_cond.shape))
y = lsim(sys, u, timesteps, init_cond)
return y
def _transform(b, a, Wn, analog, output):
"""
Shift prototype filter to desired frequency, convert to digital with
pre-warping, and return in various formats.
"""
Wn = np.asarray(Wn)
if not analog:
if np.any(Wn < 0) or np.any(Wn > 1):
raise ValueError("Digital filter critical frequencies "
"must be 0 <= Wn <= 1")
fs = 2.0
warped = 2 * fs * tan(pi * Wn / fs)
else:
warped = Wn
# Shift frequency
b, a = lp2lp(b, a, wo=warped)
# Find discrete equivalent if necessary
if not analog:
b, a = bilinear(b, a, fs=fs)
# Transform to proper out type (pole-zero, state-space, numer-denom)
if output in ('zpk', 'zp'):
return tf2zpk(b, a)
elif output in ('ba', 'tf'):
return b, a
elif output in ('ss', 'abcd'):
return tf2ss(b, a)
elif output in ('sos'):
raise NotImplementedError('second-order sections not yet implemented')
else:
raise ValueError('Unknown output type {0}'.format(output))
def _test_system():
"""Test PID algorithm."""
import scipy.signal as sig
# transfer function in s-space describes sys
tf = sig.tf2ss([10], [100, 10])
times = np.arange(1, 200, 5.0)
#step = 1 * np.ones(len(times))
# initialize PID
pid = PidController(2.0, 10.0, 0.0)
pid.anti_windup = 0.2
pid.vmin, pid.vmax = -200.0, 200.0
pid.setpoint = 50.0
pid._prev_time = 0.0
sysout = [0.0]
pidout = [0.0]
real_time = [0.0]
for time in times:
real_time.append(time)
pidout.append(pid.compute_output(sysout[-1], real_time[-1]))
t, sysout, xout = sig.lsim(tf, pidout, real_time)
fig = plt.figure()
ax = fig.add_subplot(111)
ax.plot(real_time, sysout, 'r', real_time, pidout, 'b--')
plt.show()
def to_dSS(self, form="ctrl"):
"""
Transform the transfer function into a state-space
Parameters:
- form: controllable canonical form ('ctrl') or observable canonical form ('obs')
"""
# TODO: code it without scipy
from fixif.LTI import dSS
if form == 'ctrl':
A = mat(diagflat(ones((1, self.order - 1)), 1))
A[self.order - 1, :] = fliplr(-self.den[0, 1:])
B = mat(r_[zeros((self.order - 1, 1)), atleast_2d(1)])
C = mat(
fliplr(self.num[0, 1:]) -
fliplr(self.den[0, 1:]) * self.num[0, 0])
D = mat(atleast_2d(self.num[0, 0]))
elif form == 'obs':
# TODO: to it manually!!
A, B, C, D = tf2ss(array(self.num)[0, :], array(self.den)[0, :])
else:
raise ValueError(
'dTF.to_dSS: the form "%s" is invalid (must be "ctrl" or "obs")'
% form)
return dSS(A, B, C, D)
def __init__(self,order,cutoff,Fs,filtertype):
self.order = order
self.Fs = Fs
self.Wn = self.Fs*0.5 # Time 0.5 to create float type
self.type = filtertype;
self.num, self.den = butter(self.order,np.divide(cutoff,self.Wn),self.type)
self.A, self.B, self.C, self.D = tf2ss(self.num,self.den)
self.Xnn = np.zeros((np.size(self.A,1),1))
def compute_ss(self):
A, B, C, D = signal.tf2ss(self.num, self.den)
self.A = A
self.B = B
self.C = C
self.D = D
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 convert2SS(self):
'''
Returns
-------
SpaceState object
DESCRIPTION. Converts TransferFunction object to StateSpace object
'''
A,B,C,D = signal.tf2ss(self.num_coef.reshape(-1), self.den_coef.reshape(-1))
self.state_space_rep = StateSpace(A,B,C,D)
return self.state_space_rep
def __init__(self, num, den, udelay=None):
self.num, self.den = num, den
self.udelay = udelay
#Creates state space matrices
A, B, C, D = tf2ss(num, den)
if udelay is not None:
A = stack((A, zeros(A.shape, 'float32')))
B = stack((zeros(B.shape, 'float32'), B))
C = stack((C, zeros(C.shape, 'float32')))
D = stack((D, zeros(D.shape, 'float32')))
udelay = [0.0, udelay]
#Calls state space init function
StateSpace.__init__(self, A, B, C, D, delays=udelay)
def test_basic(self):
# Test a round trip through tf2ss and ss2tf.
b = np.array([1.0, 3.0, 5.0])
a = np.array([1.0, 2.0, 3.0])
A, B, C, D = tf2ss(b, a)
assert_allclose(A, [[-2, -3], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2]], rtol=1e-13)
assert_allclose(D, [[1]], rtol=1e-14)
bb, aa = ss2tf(A, B, C, D)
assert_allclose(bb[0], b, rtol=1e-13)
assert_allclose(aa, a, rtol=1e-13)
def test_zero_order_round_trip(self):
# See gh-5760
tf = (2, 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0]], rtol=1e-13)
assert_allclose(D, [[2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
tf = ([[5], [2]], 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0], [0]], rtol=1e-13)
assert_allclose(D, [[5], [2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[5, 0], [2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
def test_zero_order_round_trip(self):
# See gh-5760
tf = (2, 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0]], rtol=1e-13)
assert_allclose(D, [[2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
tf = ([[5], [2]], 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0], [0]], rtol=1e-13)
assert_allclose(D, [[5], [2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[5, 0], [2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
def test_basic(self):
# Test a round trip through tf2ss and ss2tf.
b = np.array([1.0, 3.0, 5.0])
a = np.array([1.0, 2.0, 3.0])
A, B, C, D = tf2ss(b, a)
assert_allclose(A, [[-2, -3], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2]], rtol=1e-13)
assert_allclose(D, [[1]], rtol=1e-14)
bb, aa = ss2tf(A, B, C, D)
assert_allclose(bb[0], b, rtol=1e-13)
assert_allclose(aa, a, rtol=1e-13)
class TestC2dInvariants:
# Some test cases for checking the invariances.
# Array of triplets: (system, sample time, number of samples)
cases = [
(tf2ss([1, 1], [1, 1.5, 1]), 0.25, 10),
(tf2ss([1, 2], [1, 1.5, 3, 1]), 0.5, 10),
(tf2ss(0.1, [1, 1, 2, 1]), 0.5, 10),
]
# Some options for lsim2 and derived routines
tolerances = {'rtol': 1e-9, 'atol': 1e-11}
# Check that systems discretized with the impulse-invariant
# method really hold the invariant
@pytest.mark.parametrize("sys,sample_time,samples_number", cases)
def test_impulse_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = impulse2(sys, T=time, **self.tolerances)
_, yout_disc = dimpulse(c2d(sys, sample_time, method='impulse'),
n=len(time))
assert_allclose(sample_time * yout_cont.ravel(), yout_disc[0].ravel())
# Step invariant should hold for ZOH discretized systems
@pytest.mark.parametrize("sys,sample_time,samples_number", cases)
def test_step_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont = step2(sys, T=time, **self.tolerances)
_, yout_disc = dstep(c2d(sys, sample_time, method='zoh'), n=len(time))
assert_allclose(yout_cont.ravel(), yout_disc[0].ravel())
# Linear invariant should hold for FOH discretized systems
@pytest.mark.parametrize("sys,sample_time,samples_number", cases)
def test_linear_invariant(self, sys, sample_time, samples_number):
time = np.arange(samples_number) * sample_time
_, yout_cont, _ = lsim2(sys, T=time, U=time, **self.tolerances)
_, yout_disc, _ = dlsim(c2d(sys, sample_time, method='foh'), u=time)
assert_allclose(yout_cont.ravel(), yout_disc.ravel())
def __init__(self, num, den, dtmax, **kwargs):
# rescale numerator and coefficients
num = [
num[k] * ((1.0 / dtmax)**(len(num) - 1 - k))
for k in range(len(num))
]
den = [
den[k] * ((1.0 / dtmax)**(len(den) - 1 - k))
for k in range(len(den))
]
# convert transfer function to ABCD
A, B, C, D = tf2ss(num=num, den=den)
# call the super constructor
super().__init__(A=A, B=B, C=C, D=D, **kwargs)
def first_order_estimate(Gp_n,Gp_d,t,u,Contr_type,DT):
(A,B,C,D) =signal.tf2ss(Gp_n,Gp_d) # Transfer Function is converted to State Space
y = Euler(A,B,C,D,t,u,DT)
kp = max(y)
for i in np.arange(0,len(t)):
if y[i]>0:
t1 = t[i]
break
for i in np.arange(0,len(t)):
if y[i] > 0.6321*max(y):
t2 = t[i]
break
tau =((t2 - t1))
Td =(t2 - tau)
return tau, Td, kp
def butter(order, Wn, N=1, btype='lowpass'):
'''
build MIMO butterworth filter of order ord and cut-off freq over Nyquist
freq ratio Wn.
The filter will have N input and N output and N*ord states.
Note: the state-space form of the digital filter does not depend on the
sampling time, but only on the Wn ratio.
As a result, this function only returns the A,B,C,D matrices of the filter
state-space form.
'''
# build DLTI SISO
num, den = scsig.butter(order, Wn, btype=btype, analog=False, output='ba')
Af, Bf, Cf, Df = scsig.tf2ss(num, den)
SSf = scsig.dlti(Af, Bf, Cf, Df, dt=1.0)
SStot = SSf
for ii in range(1, N):
SStot = join(SStot, SSf)
return SStot.A, SStot.B, SStot.C, SStot.D
def apply_f(self, u, x, Ts):
if self.den.size == 1 and self.num.size == 1:
return u * self.num[0] / self.den[0], x
if type(u) is not np.ndarray:
u = np.array([[u]]).T
else:
if u.ndim == 1:
u = u.reshape((u.size, 1))
elif u.shape[1] != 1:
u = u.T
A_t, B_t, C_t, D_t = signal.tf2ss(self.num, self.den)
(A, B, C, D, _) = signal.cont2discrete((A_t, B_t, C_t, D_t),
Ts,
method='bilinear')
A = np.kron(np.eye(u.size), A)
B = np.kron(np.eye(u.size), B)
C = np.kron(np.eye(u.size), C)
D = np.kron(np.eye(u.size), D)
x_vec = x.reshape((x.size, 1))
x1_vec = A.dot(x_vec) + B.dot(u)
y = C.dot(x_vec) + D.dot(u)
# put back in same order
if type(u) is not np.ndarray:
y = y[0, 0]
else:
if u.ndim == 1:
y = y.reshape(y.size)
elif u.shape[1] != 1:
y = y.T
if np.any(abs(y.imag) > 0):
print('y has complex part {}'.format(y))
print((A, B, C, D))
return y.reshape(y.size).real, x1_vec.reshape(x.shape)
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 IIRuncFilter(x, noise, b, a, Uab):
"""Uncertainty propagation for the signal x and the uncertain IIR filter (b,a)
Parameters
----------
x: np.ndarray
filter input signal
noise: float
signal noise standard deviation
b: np.ndarray
filter numerator coefficients
a: np.ndarray
filter denominator coefficients
Uab: np.ndarray
covariance matrix for (a[1:],b)
Returns
-------
y: np.ndarray
filter output signal
Uy: np.ndarray
uncertainty associated with y
References
----------
* Link and Elster [Link2009]_
.. seealso:: :mod:`PyDynamic.uncertainty.propagate_MonteCarlo.SMC`
"""
if not isinstance(noise, np.ndarray):
noise = noise * np.ones(np.shape(x))
p = len(a) - 1
if not len(b) == len(a):
b = np.hstack((b, np.zeros((len(a) - len(b), ))))
# from discrete-time transfer function to state space representation
[A, bs, c, b0] = tf2ss(b, a)
A = np.matrix(A)
bs = np.matrix(bs)
c = np.matrix(c)
phi = zerom((2 * p + 1, 1))
dz = zerom((p, p))
dz1 = zerom((p, p))
z = zerom((p, 1))
P = zerom((p, p))
y = np.zeros((len(x), ))
Uy = np.zeros((len(x), ))
Aabl = np.zeros((p, p, p))
for k in range(p):
Aabl[0, k, k] = -1
for n in range(len(y)):
for k in range(p): # derivative w.r.t. a_1,...,a_p
dz1[:, k] = A * dz[:, k] + np.squeeze(Aabl[:, :, k]) * z
phi[k] = c * dz[:, k] - b0 * z[k]
phi[p + 1] = -np.matrix(a[1:]) * z + x[n] # derivative w.r.t. b_0
for k in range(p + 2, 2 * p + 1): # derivative w.r.t. b_1,...,b_p
phi[k] = z[k - (p + 1)]
P = A * P * A.T + noise[n]**2 * (bs * bs.T)
y[n] = c * z + b0 * x[n]
Uy[n] = phi.T * Uab * phi + c * P * c.T + b[0]**2 * noise[n]**2
# update of the state equations
z = A * z + bs * x[n]
dz = dz1
Uy = np.sqrt(np.abs(Uy))
return y, Uy
def IIR_uncFilter(x,noise,b,a,Uab): """ Uncertainty propagation for the signal x and the uncertain IIR filter (b,a) :param x: filter input signal :param noise: signal noise standard deviation :param b: filter numerator coefficients :param a: filter denominator coefficients :param Uba: 2D matrix of covariance (mutual uncertainties) for (a[1:],b[0:]) :returns y: filter output signal :returns Uy: uncertainty associated with y Implementation of the IIR formula for uncertainty propagation:\n A. Link and C. Elster\n Uncertainty evaluation for IIR filtering using a state-space approach\n Meas. Sci. Technol. vol. 20, 2009 """ if not isinstance(noise,np.ndarray): noise = noise*np.ones(np.shape(x)) p = len(a)-1 if not len(b)==len(a): b = np.hstack((b,np.zeros((len(a)-len(b),)))) # from discrete-time transfer function to state space representation [A,bs,c,b0] = tf2ss(b,a) A = matrix(A); bs = matrix(bs); c = matrix(c); phi = zerom((2*p+1,1)) dz = zerom((p,p)); dz1 = zerom((p,p)) z = zerom((p,1)) P = zerom((p,p)) y = np.zeros((len(x),)) Uy= np.zeros((len(x),)) Aabl = np.zeros((p,p,p)) for k in range(p): Aabl[0,k,k] = -1 for n in range(len(y)): for k in range(p): # derivative w.r.t. a_1,...,a_p dz1[:,k]= A*dz[:,k] + np.squeeze(Aabl[:,:,k])*z phi[k] = c*dz[:,k] - b0*z[k] phi[p+1] = -matrix(a[1:])*z + x[n] # derivative w.r.t. b_0 for k in range(p+2,2*p+1): # derivative w.r.t. b_1,...,b_p phi[k] = z[k-(p+1)] P = A*P*A.T + noise[n]**2*(bs*bs.T) y[n] = c*z + b0*x[n] Uy[n]= phi.T*Uab*phi + c*P*c.T + b[0]**2*noise[n]**2 # update of the state equations z = A*z + bs*x[n] dz = dz1 Uy = np.sqrt(np.abs(Uy)) return y, Uy
""" This script is to simulate a example of the MIT rule """
__author__ = '{Miguel Angel Pimentel Vallejo}'
__email__ = '{[email protected]}'
__date__= '{27/may/2020}'
#Import the modules needed to run the script
import numpy as np
from scipy import signal
import matplotlib.pyplot as plt
from scipy.integrate import odeint
#Transform the transder function to space states
k = 1
k0 = 2
A,B,C,D = signal.tf2ss([k],[1,1])
AG,BG,CG,DG = signal.tf2ss([k0],[1,1])
#Function of the model with adaptative control
def model (x,t,gamma):
#Vector of derivatives
xdot = [0,0,0]
#Comand signal
uc = np.sin(t)
#Reference model
xdot[1] = AG[0,0]*x[1] + BG[0,0]*uc
#Error
if not (len(A) or len(B) or len(C)):
D = np.asarray(D).flatten()
if len(D) != 1:
raise ValueError("D must be scalar for zero-order models")
return (D[0], 1.) # pragma: no cover; solved in scipy>=0.18rc2
nums, den = ss2tf(A, B, C, D)
if len(nums) != 1:
# TODO: support MIMO systems
# https://github.com/scipy/scipy/issues/5753
raise NotImplementedError("System must be SISO")
return nums[0], den
_sys2ss = {
_LSYS: lambda sys: sys.ss,
_LFILT: lambda sys: tf2ss(sys.num, sys.den),
_NUM: lambda sys: tf2ss(sys, 1),
_TF: lambda sys: tf2ss(*sys),
_ZPK: lambda sys: zpk2ss(*sys),
_SS: lambda sys: sys,
}
_sys2zpk = {
_LSYS: lambda sys: sys.zpk,
_LFILT: lambda sys: tf2zpk(sys.num, sys.den),
_NUM: lambda sys: tf2zpk(sys, 1),
_TF: lambda sys: tf2zpk(*sys),
_ZPK: lambda sys: sys,
_SS: lambda sys: ss2zpk(*sys),
}
""" Hammerstein system with zero initial condition. """
np.random.seed(10)
N = int(2e3) # Number of samples
u = np.random.randn(N)
def f_NL(x): return x + 0.2*x**2 + 0.1*x**3
b, a = signal.cheby1(2, 5, 2*0.3, 'low', analog=False)
x = f_NL(u) # Intermediate signal
y = signal.lfilter(b, a, x) # output
scale = np.linalg.lstsq(u[:, None], x)[0].item()
# Initial linear model = scale factor times underlying dynamics
sys = signal.tf2ss(scale*b, a)
T1 = 0 # No periodic transient
T2 = 200 # number of transient samples to discard
sig = Signal(u[:, None, None, None], y[:, None, None, None])
sig.average()
model = PNLSS(sys, sig)
# Hammerstein system only has nonlinear terms in the input.
# Quadratic and cubic terms in state- and output equation
model.nlterms('x', [2, 3], 'inputsonly')
model.nlterms('y', [2, 3], 'inputsonly')
model.transient(T1=T1, T2=T2)
model.optimize(weight=False, nmax=50)
yopt = model.simulate(u)[1]
bvopt = res.x
cost = funRes(bvopt, kv, Yv, dyv, ddyv)
return bvopt, cost
if __name__ == '__main__':
import libss
import scipy.signal as scsig
import matplotlib.pyplot as plt
# build state-space
cfnum = np.array([4, 1.25, 1.5])
cfden = np.array([2, .5, 1])
A, B, C, D = scsig.tf2ss(cfnum, cfden)
# -------------------------------------------------------------- Test cases
# Comment/Uncomment as appropriate
### Test01: 2nd order DLTI system
ds = 2. / 40.
fs = 1. / ds
fn = fs / 2.
kn = 2. * np.pi * fn
kv = np.linspace(0, kn, 301)
SS = scsig.dlti(A, B, C, D, dt=ds)
Cv = libss.freqresp(SS, kv)
Cv = Cv[0, 0, :]
# ### Test02: 2nd order continuous-time LTI system
import numpy as np
from utils import tf, zeros
from scipy import signal
import sympy as sp
s = tf([1, 0], [1])
G_tf = (1 - s) / (1 + s)
G_ss = signal.tf2ss(G_tf.numerator, G_tf.denominator)
A, B, C, D = G_ss
def checkdimensions(x):
if x.ndim == 1:
x = np.array([x])
return x
A = checkdimensions(A)
B = checkdimensions(B)
C = checkdimensions(C)
D = checkdimensions(D)
AB = np.concatenate((A, B), axis=1)
CD = np.concatenate((C, D), axis=1)
M = np.concatenate((AB, CD), axis=0)
rowA, colA = np.shape(A)
rowB, colB = np.shape(B)
rowC, colC = np.shape(C)
def IIRuncFilter(x, noise, b, a, Uab):
"""Uncertainty propagation for the signal x and the uncertain IIR filter (b,a)
Parameters
----------
x: np.ndarray
filter input signal
noise: float
signal noise standard deviation
b: np.ndarray
filter numerator coefficients
a: np.ndarray
filter denominator coefficients
Uab: np.ndarray
covariance matrix for (a[1:],b)
Returns
-------
y: np.ndarray
filter output signal
Uy: np.ndarray
uncertainty associated with y
References
----------
* Link and Elster [Link2009]_
"""
if not isinstance(noise, np.ndarray):
noise = noise * np.ones_like(x) # translate iid noise to vector
p = len(a) - 1
if not len(b) == len(a):
b = np.hstack((b, np.zeros((len(a) - len(b),)))) # adjust dimension for later use
[A, bs, c, b0] = tf2ss(b, a) # from discrete-time transfer function to state space representation
A = np.matrix(A)
bs = np.matrix(bs)
c = np.matrix(c)
phi = np.zeros((2*p+1, 1))
dz = np.zeros((p, p))
dz1 = np.zeros((p, p))
z = np.zeros((p, 1))
P = np.zeros((p, p))
y = np.zeros((len(x),))
Uy = np.zeros((len(x),))
Aabl = np.zeros((p, p, p))
for k in range(p):
Aabl[0, k, k] = -1
for n in range(len(y)): # implementation of the state-space formulas from the paper
for k in range(p): # derivative w.r.t. a_1,...,a_p
dz1[:, k] = A * dz[:, k] + np.squeeze(Aabl[:, :, k]) * z
phi[k] = c * dz[:, k] - b0 * z[k]
phi[p + 1] = -np.matrix(a[1:]) * z + x[n] # derivative w.r.t. b_0
for k in range(p + 2, 2 * p + 1): # derivative w.r.t. b_1,...,b_p
phi[k] = z[k - (p + 1)]
P = A * P * A.T + noise[n] ** 2 * (bs * bs.T)
y[n] = c * z + b0 * x[n]
Uy[n] = phi.T * Uab * phi + c * P * c.T + b[0] ** 2 * noise[n] ** 2
z = A * z + bs * x[n] # update of the state equations
dz = dz1
Uy = np.sqrt(np.abs(Uy)) # calculate point-wise standard uncertainties
return y, Uy
from scipy.signal import tf2ss, lsim, TransferFunction, lti
from numpy.linalg import matrix_rank
from numpy import hstack, vstack
A, B, C, D = tf2ss(1.0, [-22.0, 0, 21.0 * 9.8]) # state space representation
for name, matrix in zip(['A', 'B', 'C', 'D'], [A, B, C, D]):
print("{0}: {1}".format(name, matrix))
print("A*B : {0}".format(A*B))
controllability_matrix = hstack((B, A*B))
print("Controllability Matrix: {0}".format(controllability_matrix))
controllability_rank = matrix_rank(controllability_matrix)
print("Rank {0}".format(controllability_rank))
if controllability_rank < A.shape[0]: # rank < n
print("Not controllable")
else:
print("Controllable")
print()
print("C*A: {0}".format(C*A))
observability_matrix = vstack((C, C*A))
print("Observability Matrix: {0}".format(observability_matrix))
observability_rank = matrix_rank(observability_matrix)
print("Rank {0}".format(observability_rank))
if observability_rank < A.shape[0]: # rank < n
print("Not observable")
else:
def __init__(self):
self.system = sig.tf2ss([10.0], [20.0, 2.0])
self.noise = 1.0 # %
self.inputs = [0.0, 0.0] # U
self.times = [0.0, 0.01] # T
self.start = time.time()
import numpy as np
from scipy import signal
dvec = np.array([1, 2, 3, 4])
A1 = np.array([-dvec, [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
B1 = np.array([[1, 0, 0,
0]]).T # wrong dimension, this requires D has one column
B1 = np.eye(4)
C1 = np.array([[1, 2, 3, 4]])
D1 = np.zeros((1, 4))
print signal.ss2tf(A1, B1, C1, D1)
#same as http://en.wikipedia.org/wiki/State_space_(controls)#Canonical_realizations
signal.ss2tf(*signal.tf2ss(*signal.ss2tf(A1, B1, C1, D1)))
np.testing.assert_almost_equal(
signal.ss2tf(*signal.tf2ss(*signal.ss2tf(A1, B1, C1, D1)))[0],
signal.ss2tf(A1, B1, C1, D1)[0])
'''
dx_t = A x_t + B u_t
y_t = C x_t + D u_t
>>> dvec = np.array([1,2,3,4])
>>> A = np.array([-dvec,[1,0,0,0],[0,1,0,0],[0,0,1,0]])
>>> B = np.array([[1,0,0,0]]).T # wrong dimension, this requires D has one column
>>> B = np.eye(4)
>>> C = np.array([[1,2,3,4]])
>>> D = np.zeros((1,4))
>>> num, den = signal.ss2tf(A,B,C,D)
>>> print num
def tf_step(G, t_end=10, initial_val=0, points=1000, constraint=None, Y=None, method='numeric'):
"""
Validate the step response data of a transfer function by considering dead
time and constraints. A unit step response is generated.
Parameters
----------
G : tf
Transfer function (input[u] or output[y]) to evauate step response.
Y : tf
Transfer function output[y] to evaluate constrain step response
(optional) (required if constraint is specified).
t_end : integer
length of time to evaluate step response (optional).
initial_val : integer
starting value to evalaute step response (optional).
points : integer
number of iteration that will be calculated (optional).
constraint : real
The upper limit the step response cannot exceed. Is only calculated
if a value is specified (optional).
method : ['numeric','analytic']
The method that is used to calculate a constrainted response. A
constraint value is required (optional).
Returns
-------
timedata : array
Array of floating time values.
process : array (1 or 2 dim)
1 or 2 dimensional array of floating process values.
"""
# Surpress the complex casting error
import warnings
warnings.simplefilter("ignore")
timedata = numpy.linspace(0, t_end, points)
if constraint is None:
deadtime = G.deadtime
[timedata, processdata] = numpy.real(G.step(initial_val, timedata))
t_stepsize = max(timedata)/(timedata.size-1)
t_startindex = int(max(0, numpy.round(deadtime/t_stepsize, 0)))
processdata = numpy.roll(processdata, t_startindex)
processdata[0:t_startindex] = initial_val
else:
if method == 'numeric':
A1, B1, C1, D1 = signal.tf2ss(G.numerator, G.denominator)
# adjust the shape for complex state space functions
x1 = numpy.zeros((numpy.shape(A1)[1], numpy.shape(B1)[1]))
if constraint is not None:
A2, B2, C2, D2 = signal.tf2ss(Y.numerator, Y.denominator)
x2 = numpy.zeros((numpy.shape(A2)[1], numpy.shape(B2)[1]))
dt = timedata[1]
processdata1 = []
processdata2 = []
bconst = False
u = 1
for t in timedata:
dxdt1 = A1*x1 + B1*u
y1 = C1*x1 + D1*u
if constraint is not None:
if (y1[0, 0] > constraint) or bconst:
y1[0, 0] = constraint
bconst = True # once constraint the system is oversaturated
u = 0 # TODO : incorrect, find the correct switching condition
dxdt2 = A2*x2 + B2*u
y2 = C2*x2 + D2*u
x2 = x2 + dxdt2 * dt
processdata2.append(y2[0, 0])
x1 = x1 + dxdt1 * dt
processdata1.append(y1[0, 0])
if constraint:
processdata = [processdata1, processdata2]
else: processdata = processdata1
elif method == 'analytic':
# TODO: calculate intercept of step and constraint line
timedata, processdata = [0, 0]
else: raise ValueError('Invalid function parameters')
# TODO: calculate time response
return timedata, processdata
def rfa_mimo(Yfull,
kv,
ds,
tolAbs,
Nnum,
Nden,
Dmatrix=None,
NtrialMax=6,
Ncpu=4,
method='independent'):
"""
Given the frequency response of a MIMO DLTI system, this function returns
the A,B,C,D matrices associated to the rational function approximation of
the original system.
Input:
- Yfull: frequency response (as per libss.freqresp) of full size system over
the frequencies kv.
- kv: array of frequencies over which the RFA approximation is evaluated.
- tolAbs: absolute tolerance for the rfa fitting
- Nnum: number of numerator coefficients for RFA
- Nden: number of denominator coefficients for RFA
- NtrialMax: maximum number of attempts
- method=['intependent']. Method used to produce the system:
- intependent: each input-output combination is treated separately. The
resulting system is a collection of independent SISO DLTIs
"""
Nout, Nin, Nk = Yfull.shape
assert Nk == len(
kv), 'Frequency response Yfull not compatible with frequency range kv'
iivec = range(Nin)
oovec = range(Nout)
plist = list(itertools.product(oovec, iivec))
args_const = (Nnum, Nden, tolAbs, ds, True, NtrialMax, 1e2, False, False)
with mpr.Pool(Ncpu) as pool:
if Dmatrix is None:
P = [
pool.apply_async(rfa_fit_dev,
args=(
kv,
Yfull[oo, ii, :],
) + args_const) for oo, ii in plist
]
else:
P = [
pool.apply_async(rfa_fit_dev,
args=(
kv,
Yfull[oo, ii, :] - Dmatrix[oo, ii],
) + args_const) for oo, ii in plist
]
R = [pp.get() for pp in P]
Asub = []
Bsub = []
Csub = []
Dsub = []
for oo in range(Nout):
Ainner = []
Binner = []
Cinner = []
Dinner = []
for ii in range(Nin):
# get coeffs
pp = Nin * oo + ii
cnopt, cdopt = R[pp]
# assert plist[pp]==(oo,ii), 'Something wrong in loop'
A, B, C, D = tf2ss(cnopt, cdopt)
Ainner.append(A)
Binner.append(B)
Cinner.append(C)
Dinner.append(D)
# build s-s for each output
Asub.append(scalg.block_diag(*Ainner))
Bsub.append(scalg.block_diag(*Binner))
Csub.append(np.block(Cinner))
Dsub.append(np.block(Dinner))
Arfa = scalg.block_diag(*Asub)
Brfa = np.vstack(Bsub)
Crfa = scalg.block_diag(*Csub)
if Dmatrix is not None:
Drfa = np.vstack(Dsub) + Dmatrix
else:
Drfa = np.vstack(Dsub)
return Arfa, Brfa, Crfa, Drfa
def tf_step(G,
t_end=10,
initial_val=0,
points=1000,
constraint=None,
Y=None,
method='numeric'):
"""
Validate the step response data of a transfer function by considering dead
time and constraints. A unit step response is generated.
Parameters
----------
G : tf
Transfer function (input[u] or output[y]) to evauate step response.
Y : tf
Transfer function output[y] to evaluate constrain step response
(optional) (required if constraint is specified).
t_end : integer
length of time to evaluate step response (optional).
initial_val : integer
starting value to evalaute step response (optional).
points : integer
number of iteration that will be calculated (optional).
constraint : real
The upper limit the step response cannot exceed. Is only calculated
if a value is specified (optional).
method : ['numeric','analytic']
The method that is used to calculate a constrainted response. A
constraint value is required (optional).
Returns
-------
timedata : array
Array of floating time values.
process : array (1 or 2 dim)
1 or 2 dimensional array of floating process values.
"""
# Surpress the complex casting error
import warnings
warnings.simplefilter("ignore")
timedata = numpy.linspace(0, t_end, points)
if constraint is None:
deadtime = G.deadtime
[timedata, processdata] = numpy.real(G.step(initial_val, timedata))
t_stepsize = max(timedata) / (timedata.size - 1)
t_startindex = int(max(0, numpy.round(deadtime / t_stepsize, 0)))
processdata = numpy.roll(processdata, t_startindex)
processdata[0:t_startindex] = initial_val
else:
if method == 'numeric':
A1, B1, C1, D1 = signal.tf2ss(G.numerator, G.denominator)
# adjust the shape for complex state space functions
x1 = numpy.zeros((numpy.shape(A1)[1], numpy.shape(B1)[1]))
if constraint is not None:
A2, B2, C2, D2 = signal.tf2ss(Y.numerator, Y.denominator)
x2 = numpy.zeros((numpy.shape(A2)[1], numpy.shape(B2)[1]))
dt = timedata[1]
processdata1 = []
processdata2 = []
bconst = False
u = 1
for t in timedata:
dxdt1 = A1 * x1 + B1 * u
y1 = C1 * x1 + D1 * u
if constraint is not None:
if (y1[0, 0] > constraint) or bconst:
y1[0, 0] = constraint
bconst = True # once constraint the system is oversaturated
u = 0 # TODO : incorrect, find the correct switching condition
dxdt2 = A2 * x2 + B2 * u
y2 = C2 * x2 + D2 * u
x2 = x2 + dxdt2 * dt
processdata2.append(y2[0, 0])
x1 = x1 + dxdt1 * dt
processdata1.append(y1[0, 0])
if constraint:
processdata = [processdata1, processdata2]
else:
processdata = processdata1
elif method == 'analytic':
# TODO: caluate intercept of step and constraint line
timedata, processdata = [0, 0]
else:
raise ValueError('Invalid function parameters')
# TODO: calculate time response
return timedata, processdata
def __init__(self):
self.system = sig.tf2ss([10.0], [20.0, 2.0])
self.noise = 1.0 # %
self.inputs = [0.0, 0.0] # U
self.times = [0.0, 0.01] # T
self.start = time.time()
import libss
import matplotlib.pyplot as plt
### common params
ds=2./40.
fs=1./ds
fn=fs/2.
kn=2.*np.pi*fn
kv=np.linspace(0,kn,301)
# build a state-space
cfnum=np.array([4, 1.25, 1.5])
cfden=np.array([2, .5, 1])
A,B,C,D=tf2ss(cfnum,cfden)
SS=libss.ss(A,B,C,D,dt=ds)
Cvref=libss.freqresp(SS,kv)
Cvref=Cvref[0,0,:]
# Find fitting
Nnum,Nden=3,3
cnopt,cdopt=rfa_fit_dev(kv,Cvref,Nnum,Nden,ds,True,3,Cfbound=1e3,
OutFull=False,Print=True)
print('Error coefficients (DLTI):')
print('Numerator: '+3*'%.2e ' %tuple(np.abs(cnopt-cfnum)))
print('Denominator: '+3*'%.2e ' %tuple(np.abs(cnopt-cfnum)))
# Visualise
Cfit=rfa(cnopt,cdopt,kv,ds)
def IIRuncFilter(x, noise, b, a, Uab):
"""Uncertainty propagation for the signal x and the uncertain IIR filter (b,a)
Parameters
----------
x: np.ndarray
filter input signal
noise: float
signal noise standard deviation
b: np.ndarray
filter numerator coefficients
a: np.ndarray
filter denominator coefficients
Uab: np.ndarray
covariance matrix for (a[1:],b)
Returns
-------
y: np.ndarray
filter output signal
Uy: np.ndarray
uncertainty associated with y
References
----------
* Link and Elster [Link2009]_
"""
if not isinstance(noise, np.ndarray):
noise = noise * np.ones_like(x) # translate iid noise to vector
p = len(a) - 1
if not len(b) == len(a):
b = np.hstack((b, np.zeros((len(a) - len(b),)))) # adjust dimension for later use
[A, bs, c, b0] = tf2ss(b, a) # from discrete-time transfer function to state space representation
A = np.matrix(A)
bs = np.matrix(bs)
c = np.matrix(c)
phi = zerom((2 * p + 1, 1))
dz = zerom((p, p))
dz1 = zerom((p, p))
z = zerom((p, 1))
P = zerom((p, p))
y = np.zeros((len(x),))
Uy = np.zeros((len(x),))
Aabl = np.zeros((p, p, p))
for k in range(p):
Aabl[0, k, k] = -1
for n in range(len(y)): # implementation of the state-space formulas from the paper
for k in range(p): # derivative w.r.t. a_1,...,a_p
dz1[:, k] = A * dz[:, k] + np.squeeze(Aabl[:, :, k]) * z
phi[k] = c * dz[:, k] - b0 * z[k]
phi[p + 1] = -np.matrix(a[1:]) * z + x[n] # derivative w.r.t. b_0
for k in range(p + 2, 2 * p + 1): # derivative w.r.t. b_1,...,b_p
phi[k] = z[k - (p + 1)]
P = A * P * A.T + noise[n] ** 2 * (bs * bs.T)
y[n] = c * z + b0 * x[n]
Uy[n] = phi.T * Uab * phi + c * P * c.T + b[0] ** 2 * noise[n] ** 2
z = A * z + bs * x[n] # update of the state equations
dz = dz1
Uy = np.sqrt(np.abs(Uy)) # calculate point-wise standard uncertainties
return y, Uy
_I = k / Ti * pole
_D = 0.0
if Td is not None:
_D = k * Td * (epsilons[0,i-1] - epsilons[0,i-2])/dt
u = _P + _I + _D
x_kropka = (A @ x_od_t[:, i].reshape(A.shape[1],1) + B * u).T
x_od_t[:, i + 1] = x_od_t[:, i] + dt * x_kropka
y = C @ x_od_t[:, i].reshape(C.shape[1],1) + D * u
epsilons[0,i] = target - y
pole = pole + (epsilons[0,i-1] + epsilons[0,i])*dt/2
return t,epsilons[0,:]
l = [2.]
m = [10.,0.,3.,1.]
_sys = tf2ss(l,m)
A,B,C,D = _sys
dt = 0.1
T = 100
target = 20
#l = [2.]
#m = [2.,2.,1.]
#_sys = tf2ss(l,m)
#A,B,C,D = _sys
#dt = 0.1
#T = 100
#target = 20
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]
for i in range(100):
if(Ampl[i] > H and Ampl[i+1] < H):
H_index = i
print('|H| = ',Ampl[H_index],'dB','\n\u03c9 = ',w[H_index],'rad/s')
############### The sinusodal input ##############################
dt = 0.0001
NN = 500
DT = dt*1000
TT = np.arange(0,NN*dt,dt)
y = np.zeros(NN)
f = np.zeros(NN)
A,B,C,D = sig.tf2ss(num,den)
x = np.zeros(np.shape(B))
omega = w[H_index]
for n in range(NN):
f[n] = sin(omega*n*dt)
plt.subplot(211)
plt.plot(TT,f,'k')
#plt.yticks([-1, 0, 1 ])
#plt.axis([0, NN*dt,-1,1])
plt.grid()
plt.ylabel('f')
def IIR_uncFilter(x, noise, b, a, Uab):
"""
Uncertainty propagation for the signal x and the uncertain IIR filter (b,a)
:param x: filter input signal
:param noise: signal noise standard deviation
:param b: filter numerator coefficients
:param a: filter denominator coefficients
:param Uba: 2D matrix of covariance (mutual uncertainties) for (a[1:],b[0:])
:returns y: filter output signal
:returns Uy: uncertainty associated with y
Implementation of the IIR formula for uncertainty propagation:\n
A. Link and C. Elster\n
Uncertainty evaluation for IIR filtering using a state-space approach\n
Meas. Sci. Technol. vol. 20, 2009
"""
if not isinstance(noise, np.ndarray):
noise = noise * np.ones(np.shape(x))
p = len(a) - 1
if not len(b) == len(a):
b = np.hstack((b, np.zeros((len(a) - len(b), ))))
# from discrete-time transfer function to state space representation
[A, bs, c, b0] = tf2ss(b, a)
A = matrix(A)
bs = matrix(bs)
c = matrix(c)
phi = zerom((2 * p + 1, 1))
dz = zerom((p, p))
dz1 = zerom((p, p))
z = zerom((p, 1))
P = zerom((p, p))
y = np.zeros((len(x), ))
Uy = np.zeros((len(x), ))
Aabl = np.zeros((p, p, p))
for k in range(p):
Aabl[0, k, k] = -1
for n in range(len(y)):
for k in range(p): # derivative w.r.t. a_1,...,a_p
dz1[:, k] = A * dz[:, k] + np.squeeze(Aabl[:, :, k]) * z
phi[k] = c * dz[:, k] - b0 * z[k]
phi[p + 1] = -matrix(a[1:]) * z + x[n] # derivative w.r.t. b_0
for k in range(p + 2, 2 * p + 1): # derivative w.r.t. b_1,...,b_p
phi[k] = z[k - (p + 1)]
P = A * P * A.T + noise[n]**2 * (bs * bs.T)
y[n] = c * z + b0 * x[n]
Uy[n] = phi.T * Uab * phi + c * P * c.T + b[0]**2 * noise[n]**2
# update of the state equations
z = A * z + bs * x[n]
dz = dz1
Uy = np.sqrt(np.abs(Uy))
return y, Uy
if len(D) != 1:
raise ValueError("D must be scalar for zero-order models")
return (D[0], 1.) # pragma: no cover; solved in scipy>=0.18rc2
nums, den = ss2tf(A, B, C, D)
if len(nums) != 1:
# TODO: support MIMO/SIMO/MISO systems
# https://github.com/scipy/scipy/issues/5753
raise NotImplementedError("System (%s, %s, %s, %s) must be SISO to "
"convert to transfer function" %
(A, B, C, D))
return nums[0], den
_sys2ss = {
_LSYS: lambda sys: _ss(sys.ss),
_LFILT: lambda sys: _ss(tf2ss(sys.num, sys.den)),
_NUM: lambda sys: _ss(tf2ss(sys, 1)),
_TF: lambda sys: _ss(tf2ss(*sys)),
_ZPK: lambda sys: _ss(zpk2ss(*sys)),
_SS: lambda sys: _ss(sys),
}
_sys2zpk = {
_LSYS: lambda sys: sys.zpk,
_LFILT: lambda sys: tf2zpk(sys.num, sys.den),
_NUM: lambda sys: tf2zpk(sys, 1),
_TF: lambda sys: tf2zpk(*sys),
_ZPK: lambda sys: sys,
_SS: lambda sys: ss2zpk(*sys),
}
import numpy as np
from utils import tf, zeros
from scipy import signal
import sympy as sp
s = tf([1,0],[1])
G_tf = (1 - s)/(1 + s)
G_ss = signal.tf2ss(G_tf.numerator,G_tf.denominator)
A,B,C,D = G_ss
def checkdimensions(x):
if x.ndim == 1:
x = np.array([x])
return x
A = checkdimensions(A)
B = checkdimensions(B)
C = checkdimensions(C)
D = checkdimensions(D)
AB = np.concatenate((A,B),axis=1)
CD = np.concatenate((C,D), axis=1)
M = np.concatenate((AB,CD),axis=0)
rowA, colA = np.shape(A)
rowB, colB = np.shape(B)
rowC, colC = np.shape(C)
rowD, colD = np.shape(D)
