def test_iv(self):
t_start = 0
t_step = 1e-2
t_ends = [10, 10 + t_step]
for t_end in t_ends:
t = np.arange(t_start, t_end, t_step)
u = np.ones(len(t)).tolist()
num = [0.1]
den = [1, -0.9]
sys = scipysig.TransferFunction(num, den, dt=t_step)
_, y = scipysig.dlsim(sys, u, t)
y = y.flatten() + 1e-2 * np.random.normal(size=t.size)
data1 = iddata(y, u, t_step, [0])
_, y = scipysig.dlsim(sys, u, t)
y = y.flatten() + 1e-2 * np.random.normal(size=t.size)
data2 = iddata(y, u, t_step, [0])
refModel = ExtendedTF([0.2], [1, -0.8], dt=t_step)
prefilter = refModel * (1 - refModel)
control = [
ExtendedTF([1], [1, 0], dt=t_step),
ExtendedTF([1], [1, 0, 0], dt=t_step),
ExtendedTF([1], [1, 0, 0, 0], dt=t_step),
ExtendedTF([1, 0], [1, 1], dt=t_step)
]
with self.assertRaises(ValueError):
compute_vrft(data1, refModel, control, prefilter, iv=True)
compute_vrft([data1, data2], refModel, control, prefilter, iv=True)
def wiener(a, random_state):
s = random_state.choice([1., -1.], size=(1000, ))
channel = signal.TransferFunction([1, a], [1, 0], dt=1)
t, x = signal.dlsim(channel, s)
R = linalg.toeplitz(np.r_[[1 + a**2, a], np.zeros(7)])
p = np.r_[[1], np.zeros(8)]
w_wiener = np.linalg.inv(R) @ p
with open(f'prova_01/tex/03/{sanitize_path(f"Wiener_{a}")}.tex',
mode='w') as tex_file:
tex_file.write(sympy.latex(sympy.Matrix(w_wiener).n(3)))
equalizer = signal.TransferFunction(w_wiener[:2], [1, 0], dt=1)
t, s_est = signal.dlsim(equalizer, x, t=t)
HW = signal.TransferFunction(np.convolve(channel.num, equalizer.num),
np.convolve(channel.den, equalizer.den),
dt=1)
w, mag, phase = signal.dbode(HW)
fig, axes = plt.subplots(2, 1, sharex=True)
axes[0].set_ylabel('Magnitude')
axes[0].semilogx(w, mag)
axes[1].set_ylabel('Fase')
axes[1].set_xlabel('$\omega$')
axes[1].semilogx(w, phase)
plt.tight_layout()
fig.savefig(f'prova_01/img/03/{sanitize_path(f"Wiener_Bode_{a}")}.png')
def sanityCheck(N=8, fc=2*np.pi*0.25, bw=2*np.pi*0.2):
cc = LinSys.create(N,fc,bw);
(b,a) = cc.getBA()
s = np.zeros( 106, 'int32' )
si = np.zeros( 106 )
mx = 0
pi = 0
to = 0.5
for pol in cc.get():
n = pol.numer(to)[0,:]
ni= pol.numerApprox(np.array(to))
d = pol.denom()
u = np.concatenate( (n, np.zeros(100)) )
h = sig.dlsim((1,d,1), u)[1]
h = np.array( np.round(h*2**17), 'int32' )
thism = np.max(np.abs(h))
if thism > mx:
mx = thism
mi = pi
s = (s + h) % 65536
s[s>32765]-=65536
ui= np.concatenate( (ni, np.zeros(100)) )
hi= sig.dlsim((1,d,1), ui)[1]
si= si + hi
pi= pi + 1
plt.plot( h )
h = sig.impulse((b,a))
print("Max of superposition: {}".format(np.max(np.abs(s))))
print("Max: {} for pole-pair {}".format(mx, mi))
plt.plot( to+np.linspace(0,99,100), s [6:] )
plt.plot( to+np.linspace(0,99,100), si[6:] )
plt.plot( h[0], h[1] )
plt.show()
return cc
def step_1(theta, y, ar_order, ma_order):
ar = np.r_[1, theta[:ar_order]]
ma = np.r_[1, theta[ar_order:ar_order + ma_order]]
while min(len(ar), len(ma)) < max(len(ar), len(ma)):
if len(ar) < len(ma):
np.append(ar, 0)
else:
np.append(ma,0)
sys = (ar, ma, 1)
# print("AR", ar)
# print("MA", ma)
_, et = signal.dlsim(sys, y)
SSE = np.dot(et.T, et)
x = []
delta = 10 ** -6
for i in range(ar_order + ma_order):
the = theta.copy()
the[i] = the[i] + delta
ar = np.r_[1, the[:ar_order]]
ma = np.r_[1, the[ar_order:ar_order + ma_order]]
if len(ar) < len(ma):
ar = np.append(ar, 0)
if len(ma) < len(ar):
ma = np.append(ma, 0)
sys = (ar, ma, 1)
_, et_copy = signal.dlsim(sys, y)
xi = np.subtract(et, et_copy) / delta
if i == 0:
x.append(xi)
x = np.array(x)
else:
if len(x.shape) > 2:
x = np.hstack((x[0], xi))
else:
x = np.hstack((x, xi))
if len(x.shape) > 2:
A = np.dot(x[0].T, x[0])
g = np.dot(x[0].T, et)
else:
A = np.dot(x.T, x)
g = np.dot(x.T, et)
return SSE, x, A, g
def _filtering(cls, signal, system):
"""Filter `signal` by `system`."""
if np.iscomplexobj(signal):
_, filtered_signal_r, _ = sc_sig.dlsim(system, np.real(signal))
_, filtered_signal_i, _ = sc_sig.dlsim(system, np.imag(signal))
filtered_signal = filtered_signal_r + 1j * filtered_signal_i
else:
_, filtered_signal, _ = sc_sig.dlsim(system, signal)
filtered_signal.shape = signal.shape
return filtered_signal
def test_virtualReference(self):
# wrong system
with self.assertRaises(ValueError):
virtual_reference(1, 1, 0)
# cant be constant the system
with self.assertRaises(ValueError):
virtual_reference([1], [1], 0)
# cant be constant the system
with self.assertRaises(ValueError):
virtual_reference(np.array(2), np.array(3), 0)
# wrong data
with self.assertRaises(ValueError):
virtual_reference([1], [1, 1], 0)
t_start = 0
t_end = 10
t_step = 1e-2
t = np.arange(t_start, t_end, t_step)
u = np.ones(len(t)).tolist()
num = [0.1]
den = [1, -0.9]
sys = scipysig.TransferFunction(num, den, dt=t_step)
t, y = scipysig.dlsim(sys, u, t)
y = y[:, 0]
data = iddata(y, u, t_step, [0, 0])
# wrong initial conditions
with self.assertRaises(ValueError):
r, _ = virtual_reference(data, num, den)
#test good data, first order
data = iddata(y, u, t_step, [0])
r, _ = virtual_reference(data, num, den)
for i in range(len(r)):
self.assertTrue(np.isclose(r[i], u[i]))
num = [1 - 1.6 + 0.63]
den = [1, -1.6, 0.63]
sys = scipysig.TransferFunction(num, den, dt=t_step)
t, y = scipysig.dlsim(sys, u, t)
y = y[:, 0]
data = iddata(y, u, t_step, [0, 0])
#test second order
r, _ = virtual_reference(data, num, den)
for i in range(len(r)):
self.assertTrue(np.isclose(r[i], u[i]))
def step1(theta, y, ar_order, ma_order):
ar = np.r_[1, theta[:ar_order]]
ma = np.r_[1, theta[ar_order:ar_order+ma_order]]
while min(len(ar), len(ma)) < max(len(ar), len(ma)):
if len(ar) < len(ma):
ar = np.append(ar, 0)
else:
ma = np.append(ma, 0)
sys = (ar, ma, 1)
_, et = signal.dlsim(sys, y)
sse = np.dot(et.T, et)
x = []
learning_rate = 10 ** -6
for i in range(ar_order + ma_order):
theta_temp = theta.copy()
theta_temp[i] = theta_temp[i] + learning_rate
ar = np.r_[1, theta_temp[:ar_order]]
ma = np.r_[1, theta_temp[ar_order:ar_order+ma_order]]
while min(len(ar), len(ma)) < max(len(ar), len(ma)):
if len(ar) < len(ma):
ar = np.append(ar, 0)
else:
ma = np.append(ma, 0)
sys = (ar, ma, 1)
_, et_copy = signal.dlsim(sys, y)
xi = np.subtract(et, et_copy) / learning_rate
if i == 0:
x.append(xi)
x = np.array(x)
else:
if len(x.shape) > 2:
x = np.hstack((x[0], xi))
else:
x = np.hstack((x, xi))
if len(x.shape) > 2:
a = np.dot(x[0].T, x[0])
g = np.dot(x[0].T, et)
else:
a = np.dot(x.T, x)
g = np.dot(x.T, et)
return sse, x, a, g
def get_responce(self):
linear = sg.dlsim(self.systems["linear"],
self.signal_in["frc"]) #,signal_in["t"]),
#if np.mean(self.signal_in["trq"])>=0:
angular = sg.dlsim(self.systems["angular"],
self.signal_in["trq"]) #,signal_in["t"])
#else:
# print('Torque Negativo')
# angular = sg.dlsim(self.systems["angular"],np.absolute(self.signal_in["trq"]))#,signal_in["t"])
# print(angular)
# angular = angular*-1
# print(angular)
print(linear[1][-1], angular[1][-1])
return 2 * linear[1][-1], 2 * angular[1][-1]
def step1(data, theta, na, nb):
max_order = max(na, nb)
num = [0] * (max_order + 1)
den = [0] * (max_order + 1)
for i in range(na + 1):
if i == 0:
den[i] = 1
else:
den[i] = theta[i - 1]
for i in range(nb + 1):
if i == 0:
num[i] = 1
else:
num[i] = theta[na + i - 1]
system1 = (den, num, 1)
_, e = signal.dlsim(system1, data)
SSE = np.transpose(e).dot(e)
X = []
for i in range(n):
theta_update = copy.copy(theta)
theta_update[i] = theta_update[i] + delta
for i in range(na + 1):
if i == 0:
den[i] = 1
else:
den[i] = theta_update[i - 1]
for i in range(nb + 1):
if i == 0:
num[i] = 1
else:
num[i] = theta_update[na + i - 1]
system = (den, num, 1)
_, e_new = signal.dlsim(system, data)
xi = (e - e_new) / delta
X.append(xi)
X = np.array(X)
X = np.array([X[i].flatten() for i in range(len(X))]).T
A = np.transpose(X).dot(X)
g = np.transpose(X).dot(e)
return SSE, X, A, g
def filter_signal(L: scipysig.dlti,
x: np.ndarray,
x0: np.ndarray = None) -> np.ndarray:
"""Filter data in an iddata object
Parameters
----------
L : scipy.signal.dlti
Discrete-time rational transfer function used to
filter the signal
x : np.ndarray
Signal to filter
x0 : np.ndarray, optional
Initial conditions for L
Returns
-------
signal : iddata
Filtered iddata object
"""
t_start = 0
t_step = L.dt
t_end = x.size * t_step
t = np.arange(t_start, t_end, t_step)
_, y = scipysig.dlsim(L, x, t, x0)
return y.flatten()
def discrete_response(self, time, u, x0=None, dt=1): ''' Calculate the reponse with the specified time and input vector ''' system = self.discrete_ss(dt) tout, y, x = signal.dlsim(system, u, time, x0=x0) return tout, x
def filter(G, u):
# Description to help the user
"""Function used to filter the signals in a MIMO structure.
Inputs: G,u
Outputs: y
Inputs description:
G: Transfer matrix of the MIMO filter. It's a python list of TransferFunctionDiscrete elements. The dimension of the transfer matrix list must be (n,m), where n=number of outputs and m=number of inputs;
u: Input data matrix. The dimension of u must be (N,m), where N is the data length and m is the number of inputs of the system.
Outputs description:
y: Output data matrix. The dimension of y is (N,n), where N is the data length and n is the number of outputs of the system."""
# testing the type of G set by the user and converting it to list
if isinstance(G, signal.ltisys.TransferFunctionDiscrete):
G = [[G]]
# number of outputs
n = len(G)
# number of inputs
m = len(G[0])
# preallocating the output matrix
y = np.zeros((len(u), n))
# loop to calculate each output signal
for i in range(0, n):
for j in range(0, m):
if G[i][j] != 0:
t, v = signal.dlsim(G[i][j], u[:, j])
y[:, i] = y[:, i] + v[:, 0]
# return the output (filtered) signal
return y
def simulation(self, ts_length=90, random_state=None):
"""
Compute a simulated sample path assuming Gaussian shocks.
Parameters
----------
ts_length : scalar(int), optional(default=90)
Number of periods to simulate for
random_state : int or np.random.RandomState, optional
Random seed (integer) or np.random.RandomState instance to set
the initial state of the random number generator for
reproducibility. If None, a randomly initialized RandomState is
used.
Returns
-------
vals : array_like(float)
A simulation of the model that corresponds to this class
"""
random_state = check_random_state(random_state)
sys = self.ma_poly, self.ar_poly, 1
u = random_state.randn(ts_length, 1) * self.sigma
vals = dlsim(sys, u)[1]
return vals.flatten()
def AR(T, order, params):
mean = 0
std = np.sqrt(1)
np.random.seed(10)
e = np.random.normal(mean, std, size=T)
# print('The mean of WN is ', np.mean(e), 'The variance of the WN is ', np.var(e))
num = [1, 0, 0]
den = [1]
den.extend(params)
sys = (num, den, 1)
_, y = signal.dlsim(sys, e)
y = [item for sublist in y for item in sublist]
T_new = len(y) - order - 1
Y = pd.DataFrame(y[order:len(y)])
X = np.zeros((T_new + 1, order))
y_l = list(y)
k = 1
for j in range(order):
for i in range(T_new + 1):
X[i][j] = y_l[order+i-k]
k += 1
X = -1 * pd.DataFrame(X)
x_transpose = X.transpose()
coeff = np.linalg.inv(np.array(x_transpose.dot(X))).dot(x_transpose.dot(Y))
coeff = [round(item, 3) for sublist in coeff for item in sublist]
print('Actual Coefficients for AR({}) with {} samples are {}'.format(order, T, params))
print('Estimated Coefficients for AR({}) with {} samples are {}'.format(order, T, coeff))
return coeff, X, Y
def simulation(self, ts_length=90, random_state=None):
"""
Compute a simulated sample path assuming Gaussian shocks.
Parameters
----------
ts_length : scalar(int), optional(default=90)
Number of periods to simulate for
random_state : int or np.random.RandomState, optional
Random seed (integer) or np.random.RandomState instance to set
the initial state of the random number generator for
reproducibility. If None, a randomly initialized RandomState is
used.
Returns
-------
vals : array_like(float)
A simulation of the model that corresponds to this class
"""
from scipy.signal import dlsim
random_state = check_random_state(random_state)
sys = self.ma_poly, self.ar_poly, 1
u = random_state.randn(ts_length, 1) * self.sigma
vals = dlsim(sys, u)[1]
return vals.flatten()
def simulate(self, x0, t):
t_low = int(self.Timestep / 16)
u = np.hstack((1. / (self.Timestep - 1) * np.arange(1, t_low),
1. / (self.Timestep - 1) * (self.Timestep / 16. - 1.) * np.ones(self.Timestep - t_low + 1)))
sys = self._generate_system()
_, y, _ = signal.dlsim(sys, u, t, x0=x0)
return y
def task3c():
sys1_num = [1, 2, 1]
sys1_denom = [1]
sys2_num = [1, 0]
sys2_denom = [1, 0.9]
sys1_gain = 1
sys1_zeros = [-1, -1]
sys1_poles = []
sys1 = sig.dlti(sys1_num, sys1_denom)
sys2 = sig.dlti(sys2_num, sys2_denom)
w1, mag1, phase1 = sig.dbode(sys1, n=100000)
w2, mag2, phase2 = sig.dbode(sys2, n=100000)
plt.semilogx(w1, mag1)
plt.title("magnitude response of first system")
plt.savefig("task3c mag1.pdf")
plt.show()
plt.semilogx(w1, phase1)
plt.title("phase response of first system")
plt.savefig("task3c phase1.pdf")
plt.show()
plt.semilogx(w2, mag2)
plt.title("magnitude response of second system")
plt.savefig("task3c mag2.pdf")
plt.show()
plt.semilogx(w2, phase2)
plt.title("phase response of second system")
plt.savefig("task3c phase2.pdf")
plt.show()
sys1 = sig.ZerosPolesGain(sys1_zeros, )
n = np.arange(1000)
x = 1 / 2 * np.sin(np.pi * n + np.pi / 4)
out1 = sig.dlsim(sys1, x)
out2 = sig.dlsim(sys2, x)
plt.plot(n, out1)
plt.title("output of system 1")
plt.savefig("task3e output1.pdf")
plt.show()
plt.plot(n, out2)
plt.title("output of system 2")
plt.savefig("task3e output2.pdf")
plt.show()
def calculate_position(self):
#Create a time array,t and input array u
t = np.arange(self.control_buffer[-2][0], self.control_buffer[-1][0],
self.dt)
u = self.elev_angle_f(
np.linspace(self.control_buffer[-2][1][0],
self.control_buffer[-1][1][0], len(t)))
#Run the state space sim
tout, yout, xout = signal.dlsim(
(self.a, self.b, self.c, self.d, self.dt), u, x0=self.x)
self.x = xout[-1] #Stores the current state for the next time.
#integrate the yout
pitch = integrate.cumtrapz(
np.array(yout)[:, 4], dx=self.dt, initial=0) + np.deg2rad(
self.position[3]) #Integrate the pitch rate for pitch change
# Get the vertical and horizontal velocity at every timestep
U_e = np.multiply(np.cos(pitch),
28 - np.array(yout)[:, 0]) - np.multiply(
np.sin(pitch),
np.array(yout)[:, 2])
V_e = np.multiply(np.sin(pitch),
28 - np.array(yout)[:, 0]) + np.multiply(
np.cos(pitch),
np.array(yout)[:, 2])
self.velocity[0] = U_e[-1]
self.velocity[2] = V_e[-1]
dX = integrate.simps(U_e, dx=self.dt)
dZ = integrate.simps(V_e, dx=self.dt)
print(
f'Moving at speed {self.velocity[0]},Moved fwd {dX} m and up {dZ} m'
)
#Update the pitch
self.position[3] = np.rad2deg(pitch[-1])
new_cords = distance.distance(meters=dX).destination(
(self.position[0], self.position[1]),
self.position[5]) # Calculate new lat/lon
#print(f'new_cords {new_cords}')
self.position[0] = new_cords[0]
self.position[1] = new_cords[1]
self.position[2] += dZ
#self.velocity = [U_vel[-1]]
#print(f"New Position: {self.position}")
#input()
#Display timing info
delay = time.time() - self.control_buffer[-1][0]
#print("It has been "+str(delay)+ "seconds!")
#Return new position tuple
return self.position
def changenoise(X,noisepower):
tout, tempX = signal.dlsim(Hz, X)
noise_gaussian = np.random.normal(0, math.sqrt(noisepower), 10000)
d = tempX.T + noise_gaussian
d = d.flatten()
wsnr,e,W = RLS(x,d,order,10000)
return wsnr,e,W
def cal_wn(na, theta, y):
num = [1] + list(theta[na:])
den = [1] + list(theta[:na])
if len(den) < len(num):
den.extend([0 for i in range(len(num) - len(den))])
elif len(num) < len(den):
num.extend([0 for i in range(len(den) - len(num))])
sys = (den, num, 1)
_, e = signal.dlsim(sys, y)
e = [item[0] for item in e]
return np.array(e)
def test_dlsim_trivial(self):
a = np.array([[0.0]])
b = np.array([[0.0]])
c = np.array([[0.0]])
d = np.array([[0.0]])
n = 5
u = np.zeros(n).reshape(-1, 1)
tout, yout, xout = dlsim((a, b, c, d, 1), u)
assert_array_equal(tout, np.arange(float(n)))
assert_array_equal(yout, np.zeros((n, 1)))
assert_array_equal(xout, np.zeros((n, 1)))
def test_dlsim_trivial(self):
a = np.array([[0.0]])
b = np.array([[0.0]])
c = np.array([[0.0]])
d = np.array([[0.0]])
n = 5
u = np.zeros(n).reshape(-1, 1)
tout, yout, xout = dlsim((a, b, c, d, 1), u)
assert_array_equal(tout, np.arange(float(n)))
assert_array_equal(yout, np.zeros((n, 1)))
assert_array_equal(xout, np.zeros((n, 1)))
def calc_ARMA(samples, coeff_MA, coeff_AR):
np.random.seed(42)
mean = 0
std = 1
e = std * (np.random.randn(samples) + mean)
system = (coeff_MA, coeff_AR, 1)
y_dlsim = signal.dlsim(system, e)
y = y_dlsim[1].flatten(
) # # dlsim fn returns a tuple (the [1] element of which is our desired ARMA process but it is in 2d so flatten)
return y
def test_dlsim_simple1d(self):
a = np.array([[0.5]])
b = np.array([[0.0]])
c = np.array([[1.0]])
d = np.array([[0.0]])
n = 5
u = np.zeros(n).reshape(-1, 1)
tout, yout, xout = dlsim((a, b, c, d, 1), u, x0=1)
assert_array_equal(tout, np.arange(float(n)))
expected = (0.5 ** np.arange(float(n))).reshape(-1, 1)
assert_array_equal(yout, expected)
assert_array_equal(xout, expected)
def control_response(data: iddata, error: np.ndarray,
control: list) -> np.ndarray:
t_step = data.ts
t = [i * t_step for i in range(len(error))]
phi = [None] * len(control)
for i, c in enumerate(control):
_, y = scipysig.dlsim(c, error, t)
phi[i] = y.flatten()
phi = np.vstack(phi).T
return phi
def test_dlsim_simple1d(self):
a = np.array([[0.5]])
b = np.array([[0.0]])
c = np.array([[1.0]])
d = np.array([[0.0]])
n = 5
u = np.zeros(n).reshape(-1, 1)
tout, yout, xout = dlsim((a, b, c, d, 1), u, x0=1)
assert_array_equal(tout, np.arange(float(n)))
expected = (0.5 ** np.arange(float(n))).reshape(-1, 1)
assert_array_equal(yout, expected)
assert_array_equal(xout, expected)
def main():
dirac = zeros((100, 1))
dirac[0] = 1
for i in [
[0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9],
]:
reflectance = check_surface_filters(i, False)
plt.figure()
plt.plot(*dlsim((reflectance[0], reflectance[1], 1), dirac))
plt.show()
def main():
dirac = zeros((100, 1))
dirac[0] = 1
for i in [
[0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9],
]:
reflectance = check_surface_filters(i, False)
plt.figure()
plt.plot(*dlsim((reflectance[0], reflectance[1], 1), dirac))
plt.show()
def test_deconvolve(self):
t_start = 0
t_end = 10
t_step = 1e-2
t = np.arange(t_start, t_end, t_step)
sys = ExtendedTF([0.5], [1, -0.9], dt=t_step)
u = np.random.normal(size=t.size)
_, y = scipysig.dlsim(sys, u, t)
y = y[:, 0]
data = iddata(y, u, t_step, [0])
r1, _ = virtual_reference(data, sys.num, sys.den)
r2 = deconvolve_signal(sys, data.y)
self.assertTrue(np.linalg.norm(r2 - r1[:r2.size], np.infty) < 1e-3)
def run():
# Generate 2nd order transfer function
zeta = 0.2 # damping ratio
f_n = 2.0 # natural frequency
w_n = f_n * 2.0*np.pi
num = [w_n**2]
den = [1, 2.0*zeta*w_n, w_n**2]
Gs = signal.TransferFunction(num, den)
# Simulation parameters
n_steps = 1000
t = np.linspace(0, 5, n_steps)
u = np.ones(n_steps) # input signal
u[int(n_steps/2):-1] = -1
# Simulate the output of the continuous-time system
t, y, x = signal.lsim(Gs, U=u, T=t)
dt = t[1]
# Identification
n = 2 # order of the denominator (a_1,...,a_n)
m = 2 # order of the numerator (b_0,...,b_m)
d = 1
rls = ArxRls(n, m, d)
for k in range(n_steps):
rls.update(u[k], y[k])
theta_hat = rls._theta_hat
# Construct discrete-time TF from vector of estimated parameters
num = [theta_hat.item(i) for i in range(n, n+m+1)] # b0 .. bm
den = [theta_hat.item(i) for i in range(0, n)] # a1 .. an
den.insert(0, 1.0) # add 1 to get [1, a1, .., an]
Gz = signal.TransferFunction(num, den, dt=dt)
# TODO: add delay of d
# Simulate the output and compare with the true system
t, y_est = signal.dlsim(Gz, u, t=t)
plt.plot(t, y, t, y_est)
plt.legend(["True", "Estimated"])
plt.xlabel("Time (s)")
plt.ylabel("Amplitude (-)")
plt.show()
# design controller
(kc, ki, kd) = computePidGmvc(num, den, dt, sigma=0.1, delta=0.0, lbda=0.5)
print("kc = {}, ki = {}, kd = {}\n".format(kc, ki, kd))
return
def filter_1od(time, inputSignal, tau, Te):
time = np.array(time)
inputSignal = np.array(inputSignal)
b = 1 - math.exp(-Te / tau)
a = math.exp(-Te / tau)
num = [b]
den = [1, -a]
# filter = y/u = b/(z-a)
tfFilter = signal.TransferFunction(num, den, dt=Te)
resultFilter = signal.dlsim(tfFilter, inputSignal, time, inputSignal[0])
return resultFilter # returns a tuple with the values of tout and yout
def test_dlsim_simple2d(self):
lambda1 = 0.5
lambda2 = 0.25
a = np.array([[lambda1, 0.0], [0.0, lambda2]])
b = np.array([[0.0], [0.0]])
c = np.array([[1.0, 0.0], [0.0, 1.0]])
d = np.array([[0.0], [0.0]])
n = 5
u = np.zeros(n).reshape(-1, 1)
tout, yout, xout = dlsim((a, b, c, d, 1), u, x0=1)
assert_array_equal(tout, np.arange(float(n)))
# The analytical solution:
expected = (np.array([lambda1,
lambda2])**np.arange(float(n)).reshape(-1, 1))
assert_array_equal(yout, expected)
assert_array_equal(xout, expected)
def rank_sensitvity_bis(dsys, X, Y, n_test=100):
"""
Same as before but for the ensemble training. Note that no DMD model is fitted, only OptDMD.
"""
optdmd_train_error, optdmd_test_error = list(), list()
# Fit a DMD model for each possible rank.
for rank in range(1, dsys.A.shape[0] + 1):
# Fit the DMD model (optimal closed-form solution)
optdmd = OptDMD(svd_rank=rank, factorization="svd").fit(X, Y)
# One-step ahead prediction using both DMD models.
y_predict_opt = optdmd.predict(X)
# Compute the one-step ahead prediction error.
optdmd_train_error.append(norm(y_predict_opt - Y) / norm(Y))
# Evaluate the error on test data.
optdmd_error = list()
for _ in range(n_test):
# Test initial condition.
x0_test = normal(loc=0.0, scale=1.0, size=(dsys.A.shape[1]))
# Run simulation to generate dataset.
t, _, x_test = dlsim(dsys,
np.zeros((250, dsys.inputs)),
x0=x0_test)
# Split the training data into input/output snapshots.
y_test, X_test = x_test.T[:, 1:], x_test.T[:, :-1]
# One-step ahead prediction using both DMD models.
y_predict_opt = optdmd.predict(X_test)
# Compute the one-step ahead prediction error.
optdmd_error.append(norm(y_predict_opt - y_test) / norm(y_test))
# Store the error for rank i DMD.
optdmd_test_error.append(np.asarray(optdmd_error))
# Complete rank-sensitivity.
optdmd_test_error = np.asarray(optdmd_test_error)
optdmd_train_error = np.asarray(optdmd_train_error)
return optdmd_train_error, optdmd_test_error
def simulation(self, ts_length=90):
"""
Compute a simulated sample path assuming Gaussian shocks.
Parameters
----------
ts_length : scalar(int), optional(default=90)
Number of periods to simulate for
Returns
-------
vals : array_like(float)
A simulation of the model that corresponds to this class
"""
sys = self.ma_poly, self.ar_poly, 1
u = np.random.randn(ts_length, 1) * self.sigma
vals = dlsim(sys, u)[1]
return vals.flatten()
def _compute_leguerre_regressor_matrix(self, X, y, input_idx, X_cols,
output_idx, y_cols, segment):
"""
Computes the Laguerre Regressor Matrix for a given
signal segment X.
Arguments:
X: a matrix of input signals. Each signal is a column;
input_idx: the sequential number of the execution input;
X_cols: the input data columns in case they are provided;
output_idx: the sequential number of the execution output;
y_cols: the output data columns in case they are provided;
segment: the sequential number of the execution segment (interval).
Output:
Phi: the corresponding regressor matrix for the given segment of signal.
"""
# Take Column Names
input_idx_name, output_idx_name = self._update_index_name(
input_idx, X_cols, output_idx, y_cols)
# Take X and y signal segments
X_seg = X[:, input_idx][self.initial_intervals[segment]]
# Initialize Regressor Matrix
Phi = np.zeros((len(X_seg) - 1, self.Nb))
for order in range(1, self.Nb + 1):
# Include initial values to avoid deflection
X_seg_aux = np.array([X_seg[0]] * 1000 + list(X_seg))
# Compute the Laguerre Filter Transfer Function
L_tf = self._laguerre_filter_tf(order=order)
# Simulate Laguerre Filter for Signal of Column col
_, X_out = signal.dlsim(system=L_tf,
u=X_seg_aux,
t=range(len(X_seg_aux)))
Phi[:, order - 1] = np.squeeze(X_out[1001:])
# Update interval variable
self.Phi_dict["segment" + "_" +
str(segment)][output_idx_name][input_idx_name] = Phi
def test_dlsim_simple2d(self):
lambda1 = 0.5
lambda2 = 0.25
a = np.array([[lambda1, 0.0],
[0.0, lambda2]])
b = np.array([[0.0],
[0.0]])
c = np.array([[1.0, 0.0],
[0.0, 1.0]])
d = np.array([[0.0],
[0.0]])
n = 5
u = np.zeros(n).reshape(-1, 1)
tout, yout, xout = dlsim((a, b, c, d, 1), u, x0=1)
assert_array_equal(tout, np.arange(float(n)))
# The analytical solution:
expected = (np.array([lambda1, lambda2]) **
np.arange(float(n)).reshape(-1, 1))
assert_array_equal(yout, expected)
assert_array_equal(xout, expected)
def test_discrete_approx(self):
"""
Test that the solution to the discrete approximation of a continuous
system actually approximates the solution to the continuous sytem.
This is an indirect test of the correctness of the implementation
of cont2discrete.
"""
def u(t):
return np.sin(2.5 * t)
a = np.array([[-0.01]])
b = np.array([[1.0]])
c = np.array([[1.0]])
d = np.array([[0.2]])
x0 = 1.0
t = np.linspace(0, 10.0, 101)
dt = t[1] - t[0]
u1 = u(t)
# Use lsim2 to compute the solution to the continuous system.
t, yout, xout = lsim2((a, b, c, d), T=t, U=u1, X0=x0,
rtol=1e-9, atol=1e-11)
# Convert the continuous system to a discrete approximation.
dsys = c2d((a, b, c, d), dt, method='bilinear')
# Use dlsim with the pairwise averaged input to compute the output
# of the discrete system.
u2 = 0.5 * (u1[:-1] + u1[1:])
t2 = t[:-1]
td2, yd2, xd2 = dlsim(dsys, u=u2.reshape(-1, 1), t=t2, x0=x0)
# ymid is the average of consecutive terms of the "exact" output
# computed by lsim2. This is what the discrete approximation
# actually approximates.
ymid = 0.5 * (yout[:-1] + yout[1:])
assert_allclose(yd2.ravel(), ymid, rtol=1e-4)
def runLinearAero(E,F,G,C,D,delS,nT,u,x0 = None):
"""@details run time-domain simulation of linear aerodynamics.
@param E Discrete-time state-space matrix.
@param F Discrete-time state-space matrix.
@param G Discrete-time state-space matrix.
@param C Discrete-time state-space matrix.
@param D Discrete-time state-space matrix.
@param delS Non-dimensional time step of model.
@param nT Number of time-steps to run simulation.
@param u Inputs, an nT x m array.
@return x State history, an nT x n array.
@return y Output history, an nT x l array.
"""
invE = np.linalg.inv(E)
# run simulation
tOut, yOut, xOut = dlsim((np.dot(invE,F),np.dot(invE,G),C,D,delS),
u,
None,
x0)
return tOut, yOut, xOut
def Solve_Py(*args, **kwords):
"""@brief time domain solution of linear aeroelastic system.
args:
* 4 (matPath/(A,B,C,D,dt), U, t, x0)
@param matPath string containing absolute path of .mat file with disSys
struct.
@param A state space system matrix.
@param B state space system matrix.
@param C state space system matrix.
@param D state space system matrix.
@param dt Timestep.
@param U u(t), the input sequence (None for free response).
@param t timesteps at which input is defined (None to default).
@param x0 The initial state size(x,1) (None: zero by default).
@param writeDict OrderedDict of 'name':output index to write in loop, Keyword.
@param plotDict OrderedDict of 'name':output index to plot in loop, Keyword.
@returns tout Time values for output.
@returns yout y(t), the system response.
@returns xout x(t), the state history.
"""
if len(args) == 4:
# Initialize discrete time system
if isinstance(args[0], str):
disSys = StateSpace(args[0])
elif isinstance(args[0], tuple):
disSys = StateSpace(args[0][0],
args[0][1],
args[0][2],
args[0][3],
disSys.A)
elif isinstance(args[0],StateSpace):
disSys = args[0]
else:
raise TypeError("First argument (of 4) not recognised.")
# check input sequence
try:
disSys._Ts.shape[1] #check array has 2 dimensions
arg2d = args[1]
except:
arg2d = np.atleast_2d(args[1]) #make 2d for comparison with nU
arg2d = arg2d.T
# assign time to local var
t = args[2]
if (args[1] == None or 'mpcCont' in kwords) and t == None:
if Interactive == True:
userNumber = input('Enter number of time steps (dt = %f):'\
% disSys._Ts)
userNumber = int(userNumber)
assert (userNumber > 0 and userNumber < 1000000),\
IOError("Number of steps must be > 0 and < 10^6.")
else:
userNumber = np.ceil(1.0/disSys.dt)
U = np.zeros((userNumber, disSys.nU))
elif disSys.nU != arg2d.shape[1]:
raise ValueError("Wrong number of inputs in input sequence")
else:
U = arg2d
# Run simulation
if 'writeDict' in kwords or 'plotDict' in kwords \
or 'mpcCont' in kwords or 'gust' in kwords:
# Determine whether to write and/or plot
if 'writeDict' in kwords and Settings.WriteOut == True:
write = True
if 'plotDict' in kwords and Settings.PlotOut == True:
#plot = True
raise NotImplementedError()
# Check function inputs for discrete time system plotting/writing
if t is None:
out_samples = U.shape[0]
stoptime = (out_samples - 1) * disSys._Ts
else:
stoptime = t[-1]
out_samples = int(np.floor(stoptime / disSys._Ts)) + 1
# Pre-build output arrays
xout = np.zeros((out_samples, disSys.A.shape[0]))
yout = np.zeros((out_samples, disSys.C.shape[0]))
tout = np.linspace(0.0, stoptime, num=out_samples)
# Check initial condition
if args[3] is None:
xout[0,:] = np.zeros((disSys.A.shape[1],))
else:
xout[0,:] = np.asarray(args[3])
# Pre-interpolate inputs into the desired time steps
if t is None and U[0,0] is not None:
u_dt = U
elif U[0,0] is None and 'mpcCont' in kwords:
u_dt = np.zeros((t.shape[0],disSys.nU))
else:
if len(U.shape) == 1:
U = U[:, np.newaxis]
u_dt_interp = interp1d(t, U.transpose(),
copy=False,
bounds_error=True)
u_dt = u_dt_interp(tout).transpose()
if write == True:
# Write output file header
outputIndices = list(writeDict.values())
ofile = Settings.OutputDir + \
Settings.OutputFileRoot + \
'_SOL302_out.dat'
fp = open(ofile,'w')
fp.write("{:<14}".format("Time"))
for output in writeDict.keys():
fp.write("{:<14}".format(output))
fp.write("\n")
fp.flush()
# END if write
# Simulate the system
for i in range(0, out_samples - 1):
# get optimal control action
if 'mpcCont' in kwords:
u_dt[i,:kwords['mpcCont'].mpcU] = kwords['mpcCont'].getUopt(xout[i],True)
# get gust velocity at current time step
if 'gust' in kwords:
raise NotImplementedError()
xout[i+1,:] = np.dot(disSys.A, xout[i,:]) + np.dot(disSys.B, u_dt[i,:])
yout[i,:] = np.dot(disSys.C, xout[i,:]) + np.dot(disSys.D, u_dt[i,:])
if write == True:
fp.write("{:<14,e}".format(tout[i]))
for j in yout[i,outputIndices]:
fp.write("{:<14,e}".format(j))
fp.write("\n")
fp.flush()
# Last point
yout[out_samples-1,:] = np.dot(disSys.C, xout[out_samples-1,:]) + \
np.dot(disSys.D, u_dt[out_samples-1,:])
# Write final output and close
fp.write("{:<14,e}".format(tout[out_samples-1]))
for j in yout[out_samples-1,outputIndices]:
fp.write("{:<14,e}".format(j))
fp.close()
return tout, yout, xout
else:
# Run discrete time sim using scipy solver
tout, yout, xout = dlsim((disSys.A,disSys.B,
disSys.C,disSys.D,
disSys._Ts),
U,
t = args[2],
x0 = args[3])
return tout, yout, xout
else:
raise ValueError("Needs 4 positional arguments")
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 simulation(self, ts_length=90) :
" Compute a simulated sample path. "
sys = self.num, self.den, 1
u = np.random.randn(ts_length, 1)
return dlsim(sys, u)[1]
import scipy.io.wavfile as wavfile
D = 500
f, data = wavfile.read('sp04.wav')
t = np.arange(0, len(data) / f, 1 / f)
echoes = np.concatenate((data[:D], data[D:] + 0.5 * data[:-D]))
wavfile.write('echoes.wav', f, echoes)
num = np.zeros(1)
num[0] = 1.0
den = np.zeros(D + 1)
den[0] = 1.0
for a in [0.5, 0.9, 0.25]:
den[-1] = -a
tf = (num, den, 1 / f)
_, y = signal.dlsim(tf, echoes, t=t)
wavfile.write('q2_minus_{0:.2f}.wav'.format(a), f, y)
for a in [0.5, 0.9, 0.25]:
den[-1] = a
tf = (num, den, 1 / f)
_, y = signal.dlsim(tf, echoes, t=t)
wavfile.write('q2_plus_{0:.2f}.wav'.format(a), f, y)
def RunFilter(self,x_vec,t_vec):
den = signal.convolve(np.array([self.Ts1,1]),np.array([self.Ts2,1]));
num = self.dc_gain;
filter_obj = signal.cont2discrete((num,den),self.Ts,method='zoh');
tout, x_filt= signal.dlsim(filter_obj,x_vec,t=t_vec);
return x_filt;
#----------------------------------------------------------------------#
# Take parameters as given
uar_alpha1 = .5
uar_alpha2 = 0.3
uar_alpha3 = -0.5
uar_alpha4 = -0.3
# Define the initial condition, numerator and denominator
uar_x0 = 10.
uar_num = [1.]
uar_den = [1., -uar_alpha1, -uar_alpha2, -uar_alpha3, -uar_alpha4]
uar_simulations = np.zeros((num_sims, len_sims))
uar_sys = (uar_num, uar_den, time_unit)
# Use num and den to do simulation
for i in xrange(num_sims):
# Draw the epsilon shocks
uar_eps = np.random.randn(len_sims-1, 1)
# this stores the discrete impluse response
t_out, y = sig.dlsim(uar_sys, uar_eps, x0=uar_x0)
# store x0 in the first position and from position 1:T-1 store the computed impulse repsonse
uar_simulations[i,0],uar_simulations[i,1:]=uar_x0,y.T
plt.plot(y)
plt.plot(uar_eps)
plt.show()
def SimulateDynamics(self):
t_out_x, y_out_x, x_out_x = signal.dlsim(self.SysX_d, self.SimulationParameters.u_vec, t=self.SimulationParameters.t_vec);
t_out_y, y_out_y, x_out_y = signal.dlsim(self.SysY_d, self.SimulationParameters.u_vec, t=self.SimulationParameters.t_vec);
t_out_z, y_out_z, x_out_z = signal.dlsim(self.SysZ_d, self.SimulationParameters.u_vec, t=self.SimulationParameters.t_vec);
self.SimOL = SimulationResults(t_out_x,t_out_y,t_out_z,y_out_x,y_out_y,y_out_z,x_out_x,x_out_y,x_out_z);
def simulation(self, ts_length=90) :
" Compute a simulated sample path. "
sys = self.ma_poly, self.ar_poly, 1
u = np.random.randn(ts_length, 1)
vals = dlsim(sys, u)[1]
return vals.flatten()
def test_dlsim(self):
a = np.asarray([[0.9, 0.1], [-0.2, 0.9]])
b = np.asarray([[0.4, 0.1, -0.1], [0.0, 0.05, 0.0]])
c = np.asarray([[0.1, 0.3]])
d = np.asarray([[0.0, -0.1, 0.0]])
dt = 0.5
# Create an input matrix with inputs down the columns (3 cols) and its
# respective time input vector
u = np.hstack((np.asmatrix(np.linspace(0, 4.0, num=5)).transpose(),
0.01 * np.ones((5, 1)),
-0.002 * np.ones((5, 1))))
t_in = np.linspace(0, 2.0, num=5)
# Define the known result
yout_truth = np.asmatrix([-0.001,
-0.00073,
0.039446,
0.0915387,
0.13195948]).transpose()
xout_truth = np.asarray([[0, 0],
[0.0012, 0.0005],
[0.40233, 0.00071],
[1.163368, -0.079327],
[2.2402985, -0.3035679]])
tout, yout, xout = dlsim((a, b, c, d, dt), u, t_in)
assert_array_almost_equal(yout_truth, yout)
assert_array_almost_equal(xout_truth, xout)
assert_array_almost_equal(t_in, tout)
# Make sure input with single-dimension doesn't raise error
dlsim((1, 2, 3), 4)
# Interpolated control - inputs should have different time steps
# than the discrete model uses internally
u_sparse = u[[0, 4], :]
t_sparse = np.asarray([0.0, 2.0])
tout, yout, xout = dlsim((a, b, c, d, dt), u_sparse, t_sparse)
assert_array_almost_equal(yout_truth, yout)
assert_array_almost_equal(xout_truth, xout)
assert_equal(len(tout), yout.shape[0])
# Transfer functions (assume dt = 0.5)
num = np.asarray([1.0, -0.1])
den = np.asarray([0.3, 1.0, 0.2])
yout_truth = np.asmatrix([0.0,
0.0,
3.33333333333333,
-4.77777777777778,
23.0370370370370]).transpose()
# Assume use of the first column of the control input built earlier
tout, yout = dlsim((num, den, 0.5), u[:, 0], t_in)
assert_array_almost_equal(yout, yout_truth)
assert_array_almost_equal(t_in, tout)
# Retest the same with a 1-D input vector
uflat = np.asarray(u[:, 0])
uflat = uflat.reshape((5,))
tout, yout = dlsim((num, den, 0.5), uflat, t_in)
assert_array_almost_equal(yout, yout_truth)
assert_array_almost_equal(t_in, tout)
# zeros-poles-gain representation
zd = np.array([0.5, -0.5])
pd = np.array([1.j / np.sqrt(2), -1.j / np.sqrt(2)])
k = 1.0
yout_truth = np.asmatrix([0.0, 1.0, 2.0, 2.25, 2.5]).transpose()
tout, yout = dlsim((zd, pd, k, 0.5), u[:, 0], t_in)
assert_array_almost_equal(yout, yout_truth)
assert_array_almost_equal(t_in, tout)
# Raise an error for continuous-time systems
system = lti([1], [1, 1])
assert_raises(AttributeError, dlsim, system, u)