def test_dcgain(self):
"""Test function dcgain with different systems"""
if slycot_check():
#Test MIMO systems
A, B, C, D = self.make_MIMO_mats()
gain1 = dcgain(ss(A, B, C, D))
gain2 = dcgain(A, B, C, D)
sys_tf = ss2tf(A, B, C, D)
gain3 = dcgain(sys_tf)
gain4 = dcgain(sys_tf.num, sys_tf.den)
#print("gain1:", gain1)
assert_array_almost_equal(gain1,
array([[0.0269, 0.], [0., 0.0269]]),
decimal=4)
assert_array_almost_equal(gain1, gain2)
assert_array_almost_equal(gain3, gain4)
assert_array_almost_equal(gain1, gain4)
#Test SISO systems
A, B, C, D = self.make_SISO_mats()
gain1 = dcgain(ss(A, B, C, D))
assert_array_almost_equal(gain1, array([[0.0269]]), decimal=4)
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_dcgain(self):
"""Test function dcgain with different systems"""
#Test MIMO systems
A, B, C, D = self.make_MIMO_mats()
gain1 = dcgain(ss(A, B, C, D))
gain2 = dcgain(A, B, C, D)
sys_tf = ss2tf(A, B, C, D)
gain3 = dcgain(sys_tf)
gain4 = dcgain(sys_tf.num, sys_tf.den)
#print("gain1:", gain1)
assert_array_almost_equal(gain1,
array([[0.0269, 0. ],
[0. , 0.0269]]),
decimal=4)
assert_array_almost_equal(gain1, gain2)
assert_array_almost_equal(gain3, gain4)
assert_array_almost_equal(gain1, gain4)
#Test SISO systems
A, B, C, D = self.make_SISO_mats()
gain1 = dcgain(ss(A, B, C, D))
assert_array_almost_equal(gain1,
array([[0.0269]]),
decimal=4)
def get_tf(self, desired_windspeed, desired_azimuth, desired_input,
desired_output):
windspeed_ii = np.where(self.windspeeds == desired_windspeed)[0]
desiredinput_ii = self.inputnames.index(desired_input)
desiredoutput_ii = self.outputnames.index(desired_output)
SS = self.SS[windspeed_ii[0]]
desired_wtg = ctrl.ss2tf(SS.A, SS.B[:, desiredinput_ii],
SS.C[desiredoutput_ii, :],
SS.D[desiredoutput_ii, desiredinput_ii])
return desired_wtg
def get_transfer_functions(statespace_model):
transfer_functions = ss2tf(statespace_model)
# Loop through the coefficients and set ones close to zero equal to zero.
# This will modify the actual transfer_functions arrays
all_coeffs = [
transfer_functions.num[0][0], transfer_functions.den[0][0],
transfer_functions.num[1][0], transfer_functions.den[1][0]
]
for i in range(len(all_coeffs)):
for j in range(len(all_coeffs[i])):
if abs(all_coeffs[i][j]) < (10**(-10)):
all_coeffs[i][j] = 0
theta_tf = tf(all_coeffs[0], all_coeffs[1])
cart_tf = tf(all_coeffs[2], all_coeffs[3])
return theta_tf, cart_tf
def test_dcgain_mimo(self, MIMO_mats):
"""Test function dcgain with MIMO systems"""
#Test MIMO systems
A, B, C, D = MIMO_mats
gain1 = dcgain(ss(A, B, C, D))
gain2 = dcgain(A, B, C, D)
sys_tf = ss2tf(A, B, C, D)
gain3 = dcgain(sys_tf)
gain4 = dcgain(sys_tf.num, sys_tf.den)
#print("gain1:", gain1)
assert_array_almost_equal(gain1,
array([[0.0269, 0.], [0., 0.0269]]),
decimal=4)
assert_array_almost_equal(gain1, gain2)
assert_array_almost_equal(gain3, gain4)
assert_array_almost_equal(gain1, gain4)
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 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 pid_design(self):
n_x = self._A.shape[0] # number of sates
n_u = self._B.shape[1] # number of inputs
n_y = self._C.shape[0] # number of outputs
# Augmented state system (for LQR)
A_lqr = np.block([[self._A, np.zeros((n_x, n_y))],
[self._C, np.zeros((n_y, n_y))]])
B_lqr = np.block([[self._B], [np.zeros((n_y, 1))]])
# Define Q,R
Q = np.diag(self._q)
R = np.array([self._r])
# Solve for P in continuous-time algebraic Riccati equation (CARE)
#print("A_lqr shape", A_lqr.shape)
#print("Q shape",Q.shape)
(P, L, F) = cnt.care(A_lqr, B_lqr, Q, R)
if self._verbose:
print("P matrix", P)
print("Feedback gain", F)
# Calculate Ki_bar, Kp_bar
Kp_bar = np.array([F[0][0:n_x]])
Ki_bar = np.array([F[0][n_x:]])
if self._verbose:
print("Kp_bar", Kp_bar)
print("Ki_bar", Ki_bar)
# Calculate the PID kp kd gains
C_bar = np.block(
[[self._C],
[self._C.dot(self._A) - (self._C.dot(self._B)).dot(Kp_bar)]])
if self._verbose:
print("C_bar", C_bar)
print("C_bar shape", C_bar.shape)
# Calculate PID kp ki gains
kpd = Kp_bar.dot(np.linalg.inv(C_bar))
if self._verbose:
print("kpd: ", kpd, "with shape: ", kpd.shape)
kp = kpd[0][0]
kd = kpd[0][1]
# ki gain
ki = (1. + kd * self._C.dot(self._B)).dot(Ki_bar)
self._K = [kp, ki[0][0], kd]
G_plant = cnt.ss2tf(self._A, self._B, self._C, self._D)
# create PID transfer function
d_tc = 1. / 125. # Derivative time constant at nyquist freq
p_tf = self._K[0]
i_tf = self._K[1] * cnt.tf([1], [1, 0])
d_tf = self._K[2] * cnt.tf([1, 0], [d_tc, 1])
pid_tf = cnt.parallel(p_tf, i_tf, d_tf)
open_loop_tf = cnt.series(pid_tf, G_plant)
closedLoop_tf = cnt.feedback(open_loop_tf, 1)
if self._verbose:
print(" *********** PID gains ***********")
print("kp: ", self._K[0])
print("ki: ", self._K[1])
print("kd: ", self._K[2])
print(" *********************************")
return pid_tf, closedLoop_tf
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()
########################################################
Input (Espaço de estados)
########################################################
'''
A = np.array([[0, 1, 0],
[0, 0, 1],
[-30, -31, -10]])
B = np.array([[0],
[0],
[1]])
C = np.array([0, 0, 1])
D = np.array([[0]])
# Espaço de estados -> Funcao de transferencia
S = ctl.ss(A, B, C, D)
G = ctl.ss2tf(S)
# Polos a partir do espaço de estados
print('Polos = ', ctl.pole(S))
'''
########################################################
Input (Funcao de transferencia)
########################################################
'''
# num = [0, 0, 1, 0]
# den = [1, 6, 5, 1]
# G = ctl.tf(num, den)
# # Funcao de transferencia -> Espaço de estados
'''
# System matrices as 2D arrays :
A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [-490, -137, -12, -2],
[-170, -383, -2.3, -9]])
B = np.array([[0], [0], [1000 / 310], [-693 / 250]])
C = np.array([[1, 0, 0, 0]])
# C = np.array([[0, 1, 0, 0]])
D = np.array([[0]])
Sp = ctl.ss(A, B, C, D)
print('S =', Sp)
################################################################################
# Funcao de transferencia
################################################################################
G = ctl.ss2tf(Sp)
print('G =', G)
'''
################################################################################
Input (Funcao de transferencia Gpid)
################################################################################
'''
num = np.array([1, 15000.1, 1500])
den = np.array([0, 1, 0])
Gpid = ctl.tf(num, den)
print('Gpid:', Gpid)
################################################################################
# Em serie (G*Gpid)
################################################################################
S = ctl.series(G, Gpid)
import numpy as np
end = 8
s = cm.tf([1, 0], [1])
A = np.array([[0, 1, 0, 0],
[-1, -1, 1, 0],
[0, 0, 0, 1],
[1, 0, -1, 1]])
b = np.array([[0], [0], [0], [1]])
c = np.array([1, 0, 0, 0])
d = [0]
SS = cm.feedback(cm.ss(A, b, c, d), 1)
TF_conv = cm.ss2tf(SS)
TF = cm.feedback((1) / (s**4 + s**2))
out_SS, t = cm.step(1 * SS, np.linspace(0, end, 200))
out_TF, t = cm.step(1 * TF, np.linspace(0, end, 200))
out_TF_conv, t = cm.step(1 * TF_conv, np.linspace(0, end, 200))
plt.plot(t, out_SS, lw=2, label="ss")
plt.plot(t, out_TF, lw=1, label="tf")
plt.plot(t, out_TF_conv, lw=0.5, label="tf_conv")
plt.legend()
plt.show()
# https://www.wolframalpha.com/input/?i=(%7B%7Bs,+0,+0,+0%7D,+%7B0,+s,+0,+0%7D,+%7B0,+0,+s,+0%7D,+%7B0,+0,+0,+s%7D%7D+-+%7B%7B0,+1,+0,+0%7D,+%7B-1,+-1,+1,+0%7D,+%7B0,+0,+0,+1%7D,+%7B1,+0,+-1,+1%7D%7D)
from control import matlab import numpy as np import matplotlib.pyplot as plt A = np.array([[0, 1], [-1, -1.8]]) b = np.array([[0], [1]]) c = np.array([1, 0]) d = 0 T = 0.3 S = matlab.ss(A, b, c, d) #print(S) Gs = matlab.ss2tf(S) #print(Gs) P = matlab.c2d(S, T, method='zoh') #print(P) Gz = matlab.ss2tf(P) #PはSを離散時間系に変換したやつ #print(Gz) #p.13の問題6.の答え #matlab.bode(S ,matlab.logspace(-2, 2)) #ボード線図を描こう！ yt, tt = matlab.step(S, np.arange(0, 10, 0.01)) #まずは連続時間系 #plt.plot(tt, yt) #連続時間系での出力をグラフにする yk, tk = matlab.step(P, np.arange(0, 10, T)) #次は離散時間系 plt.plot(tk, yk, marker='o') #離散時間系のプロット、プロット点も描画する
def convert_transfer_function_model(ss_model):
tf_model = ss2tf(ss_model)
return tf_model
