def c2d(sys, Ts, method='ZOH'):
"""
@summary: From continuous time convert discrete time
@param sys: an instance of the LTI class or a tuple describing the system.
The following gives the number of elements in the tuple and
the interpretation:
2: (num, den)
4: (A, B, C, D)
@param Ts: Sampling time
@param method: The name of the discretization method
@return: The discrete system
"""
if (len(sys) == 2):
(A, B, C, D) = tf2ss(sys)
else:
(A, B, C, D) = sys
n = A.shape[0]
nb = B.shape[1]
if (method == 'ZOH'):
ztmp = zeros((nb, n + nb))
tmp = hstack((A, B))
tmp = vstack((tmp, ztmp))
tmp = expm(tmp * Ts)
A = tmp[0:n, 0:n]
B = tmp[0:n, n:n + nb]
sysd = (A, B, C, D)
if (len(sys) == 2):
return ss2tf(sysd)
return sysd
def test_issiso_mimo(self):
# MIMO transfer function
sys = tf([[[-1, 41], [1]], [[1, 2], [3, 4]]],
[[[1, 10], [1, 20]], [[1, 30], [1, 40]]])
self.assertEqual(issiso(sys), False)
self.assertEqual(issiso(sys, strict=True), False)
# MIMO state space system
sys = tf2ss(sys)
self.assertEqual(issiso(sys), False)
self.assertEqual(issiso(sys, strict=True), False)
def test_issiso_mimo(self):
# MIMO transfer function
sys = tf([[[-1, 41], [1]], [[1, 2], [3, 4]]],
[[[1, 10], [1, 20]], [[1, 30], [1, 40]]]);
self.assertEqual(issiso(sys), False)
self.assertEqual(issiso(sys, strict=True), False)
# MIMO state space system
sys = tf2ss(sys)
self.assertEqual(issiso(sys), False)
self.assertEqual(issiso(sys, strict=True), False)
def test_issiso(self):
self.assertEqual(issiso(1), True)
self.assertRaises(ValueError, issiso, 1, strict=True)
# SISO transfer function
sys = tf([-1, 42], [1, 10])
self.assertEqual(issiso(sys), True)
self.assertEqual(issiso(sys, strict=True), True)
# SISO state space system
sys = tf2ss(sys)
self.assertEqual(issiso(sys), True)
self.assertEqual(issiso(sys, strict=True), True)
def test_issiso(self):
self.assertEqual(issiso(1), True)
self.assertRaises(ValueError, issiso, 1, strict=True)
# SISO transfer function
sys = tf([-1, 42], [1, 10])
self.assertEqual(issiso(sys), True)
self.assertEqual(issiso(sys, strict=True), True)
# SISO state space system
sys = tf2ss(sys)
self.assertEqual(issiso(sys), True)
self.assertEqual(issiso(sys, strict=True), True)
def Pn_ss(dt=0.0005, Km=10, Tm=0.1):
num = [0., Km]
den = [Tm, 1.]
sysc = tf(num, den)
sysd = sysc.sample(Ts=dt, method='tustin', alpha=None)
ss_d = tf2ss(sysd)
print('Pn: ', sysd)
"""
plt.figure()
bode(sysc)
plt.show()
"""
return ss_d
def FofPn_ss_2nd(dt=0.0005, omega_d=100, Km=8, Tm=0.20, tau=0.05):
num = [Tm * tau * (omega_d**2), (Tm + tau) * (omega_d**2), omega_d**2]
den = [Km, 2 * Km * omega_d, Km * (omega_d**2)]
sysc = tf(num, den)
sysd = sysc.sample(Ts=dt, method='tustin', alpha=None)
ss_d = tf2ss(sysd)
print('P: ', sysd)
"""
plt.figure()
bode(sysc)
plt.show()
"""
return ss_d
def P_ss_2nd(dt=0.0005, Km=8, Tm=0.20, tau=0.05):
num = [0., 0., Km]
den = [Tm * tau, Tm + tau, 1.]
sysc = tf(num, den)
sysd = sysc.sample(Ts=dt, method='tustin', alpha=None)
ss_d = tf2ss(sysd)
print('P: ', sysd)
"""
plt.figure()
bode(sysc)
plt.show()
"""
return ss_d
def siso(self):
"""Set up some systems for testing out MATLAB functions"""
s = tsystems()
A = np.array([[1., -2.], [3., -4.]])
B = np.array([[5.], [7.]])
C = np.array([[6., 8.]])
D = np.array([[9.]])
s.ss1 = ss(A, B, C, D)
# Create some transfer functions
s.tf1 = tf([1], [1, 2, 1])
s.tf2 = tf([1, 1], [1, 2, 3, 1])
# Conversions
s.tf3 = tf(s.ss1)
s.ss2 = ss(s.tf2)
s.ss3 = tf2ss(s.tf3)
s.tf4 = ss2tf(s.ss2)
return s
def FofPn_ss(dt=0.0005, omega_d=100, Km=10, Tm=0.1):
#omega_d = 100.
#Km = 10.
#Tm = 0.10
num = [Tm * omega_d / Km, omega_d / Km]
den = [1, omega_d]
sysc = tf(num, den)
sysd = sysc.sample(Ts=dt, method='tustin', alpha=None)
ss_d = tf2ss(sysd)
print('FofPn: ', sysd)
"""
plt.figure()
bode(sysc)
plt.show()
"""
return ss_d
def c2d(sys, Ts, method="ZOH"):
"""
@summary: From continuous time convert discrete time
@param sys: an instance of the LTI class or a tuple describing the system.
The following gives the number of elements in the tuple and
the interpretation:
2: (num, den)
4: (A, B, C, D)
@param Ts: Sampling time
@param method: The name of the discretization method
@return: The discrete system
"""
if method == "ZOH":
if len(sys) == 2:
(A, B, C, D) = tf2ss(sys)
else:
(A, B, C, D) = sys
n = A.shape[0]
nb = B.shape[1]
if method == "ZOH":
ztmp = zeros((nb, n + nb))
tmp = hstack((A, B))
tmp = vstack((tmp, ztmp))
tmp = expm(tmp * Ts)
A = tmp[0:n, 0:n]
B = tmp[0:n, n : n + nb]
sysd = (A, B, C, D)
if len(sys) == 2:
return ss2tf(sysd)
return sysd
return None
def servo_loop(gain, tau):
# +
# r----->O------C------G------->y
# - ^ |
# | |
# |-------------H---|
g = ctrl.series(
ctrl.tf(np.array([1]), np.array([1, 0])), # Integrator
ctrl.tf(np.array([1]), np.array([0.1, 1]))) # Actuator lag
h = ctrl.tf(np.array([1]), np.array([0.01, 1])) # Sensor lag
c = ctrl.tf(np.array([tau, 1]), np.array([0.008, 1])) * gain # Control law
fbsum = mat.ss([], [], [], [1, -1])
sys_ol = mat.append(mat.tf2ss(c), mat.tf2ss(g), mat.tf2ss(h))
sys_cl = mat.append(mat.tf2ss(c), mat.tf2ss(g), mat.tf2ss(h), fbsum)
q_ol = np.array([[2, 1], [3, 2]])
q_cl = np.array([[1, 4], [2, 1], [3, 2], [5, 3]])
sys_ol = mat.connect(sys_ol, q_ol, [1], [3])
sys_cl = mat.connect(sys_cl, q_cl, [4], [2])
return mat.margin(sys_ol), sys_ol, sys_cl
def do_sys_id(self):
num_poles = 2
num_zeros = 1
if not self._use_subspace:
method = 'ARMAX'
#sys_id = system_identification(self._y.T, self._u[0], method, IC='BIC', na_ord=[0, 5], nb_ord=[1, 5], nc_ord=[0, 5], delays=[0, 5], ARMAX_max_iterations=300, tsample=self._Ts, centering='MeanVal')
sys_id = system_identification(self._y.T,
self._u[0],
method,
ARMAX_orders=[num_poles, 1, 1, 0],
ARMAX_max_iterations=300,
tsample=self._Ts,
centering='MeanVal')
print(sys_id.G)
if self._verbose:
print(sys_id.G)
print("System poles of discrete G: ", cnt.pole(sys_id.G))
# Convert to continuous tf
G = harold.Transfer(sys_id.NUMERATOR,
sys_id.DENOMINATOR,
dt=self._Ts)
G_cont = harold.undiscretize(G, method='zoh')
self._sys_tf = G_cont
self._A, self._B, self._C, self._D = harold.transfer_to_state(
G_cont, output='matrices')
if self._verbose:
print("Continuous tf:", G_cont)
# Convert to state space, because ARMAX gives transfer function
ss_roll = cnt.tf2ss(sys_id.G)
A = np.asarray(ss_roll.A)
B = np.asarray(ss_roll.B)
C = np.asarray(ss_roll.C)
D = np.asarray(ss_roll.D)
if self._verbose:
print(ss_roll)
# simulate identified system using input from data
xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u)
y_error = self._y - yid
self._fitness = 1 - (y_error.var() / self._y.var())**2
if self._verbose:
print("Fittness %", self._fitness * 100)
if self._plot:
plt.figure(1)
plt.plot(self._t[0], self._y[0])
plt.plot(self._t[0], yid[0])
plt.xlabel("Time")
plt.title("Time response Y(t)=U*G(t)")
plt.legend([
self._y_name, self._y_name + '_identified: ' +
'{:.3f} fitness'.format(self._fitness)
])
plt.grid()
plt.show()
else:
sys_id = system_identification(self._y,
self._u,
self._subspace_method,
SS_fixed_order=num_poles,
SS_p=self._subspace_p,
SS_f=50,
tsample=self._Ts,
SS_A_stability=True,
centering='MeanVal')
#sys_id = system_identification(self._y, self._u, self._subspace_method, SS_orders=[1,10], SS_p=self._subspace_p, SS_f=50, tsample=self._Ts, SS_A_stability=True, centering='MeanVal')
if self._verbose:
print("x0", sys_id.x0)
print("A", sys_id.A)
print("B", sys_id.B)
print("C", sys_id.C)
print("D", sys_id.D)
A = sys_id.A
B = sys_id.B
C = sys_id.C
D = sys_id.D
# Get discrete transfer function from state space
sys_tf = cnt.ss2tf(A, B, C, D)
if self._verbose:
print("TF ***in z domain***", sys_tf)
# Get numerator and denominator
(num, den) = cnt.tfdata(sys_tf)
# Convert to continuous tf
G = harold.Transfer(num, den, dt=self._Ts)
if self._verbose:
print(G)
G_cont = harold.undiscretize(G, method='zoh')
self._sys_tf = G_cont
self._A, self._B, self._C, self._D = harold.transfer_to_state(
G_cont, output='matrices')
if self._verbose:
print("Continuous tf:", G_cont)
# get zeros
tmp_tf = cnt.ss2tf(self._A, self._B, self._C, self._D)
self._zeros = cnt.zero(tmp_tf)
# simulate identified system using discrete system
xid, yid = fsetSIM.SS_lsim_process_form(A, B, C, D, self._u,
sys_id.x0)
y_error = self._y - yid
self._fitness = 1 - (y_error.var() / self._y.var())**2
if self._verbose:
print("Fittness %", self._fitness * 100)
if self._plot:
plt.figure(1)
plt.plot(self._t[0], self._y[0])
plt.plot(self._t[0], yid[0])
plt.xlabel("Time")
plt.title("Time response Y(t)=U*G(t)")
plt.legend([
self._y_name, self._y_name + '_identified: ' +
'{:.3f} fitness'.format(self._fitness)
])
plt.grid()
plt.show()
