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 testMinrealBrute(self):
for n, m, p in permutations(range(1,6), 3):
s = matlab.rss(n, p, m)
sr = s.minreal()
if s.states > sr.states:
self.nreductions += 1
else:
np.testing.assert_array_almost_equal(
np.sort(eigvals(s.A)), np.sort(eigvals(sr.A)))
for i in range(m):
for j in range(p):
ht1 = matlab.tf(
matlab.ss(s.A, s.B[:,i], s.C[j,:], s.D[j,i]))
ht2 = matlab.tf(
matlab.ss(sr.A, sr.B[:,i], sr.C[j,:], sr.D[j,i]))
try:
self.assert_numden_almost_equal(
ht1.num[0][0], ht2.num[0][0],
ht1.den[0][0], ht2.den[0][0])
except Exception as e:
# for larger systems, the tf minreal's
# the original rss, but not the balanced one
if n < 6:
raise e
self.assertEqual(self.nreductions, 2)
def test_impulse(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure()
#everything automatically
t, y = impulse(sys)
plot(t, y, label='Simple Case')
#supply time and X0
T = linspace(0, 2, 100)
X0 = [0.2, 0.2]
t, y = impulse(sys, T, X0)
plot(t, y, label='t=0..2, X0=[0.2, 0.2]')
#Test system with direct feed-though, the function should print a warning.
D = [[0.5]]
sys_ft = ss(A, B, C, D)
t, y = impulse(sys_ft)
plot(t, y, label='Direct feedthrough D=[[0.5]]')
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
t, y = impulse(sys)
plot(t, y, label='MIMO System')
legend(loc='best')
def testMinrealBrute(self):
for n, m, p in permutations(range(1, 6), 3):
s = matlab.rss(n, p, m)
sr = s.minreal()
if s.states > sr.states:
self.nreductions += 1
else:
np.testing.assert_array_almost_equal(np.sort(eigvals(s.A)),
np.sort(eigvals(sr.A)))
for i in range(m):
for j in range(p):
ht1 = matlab.tf(
matlab.ss(s.A, s.B[:, i], s.C[j, :], s.D[j, i]))
ht2 = matlab.tf(
matlab.ss(sr.A, sr.B[:, i], sr.C[j, :], sr.D[j,
i]))
try:
self.assert_numden_almost_equal(
ht1.num[0][0], ht2.num[0][0], ht1.den[0][0],
ht2.den[0][0])
except Exception as e:
# for larger systems, the tf minreal's
# the original rss, but not the balanced one
if n < 6:
raise e
self.assertEqual(self.nreductions, 2)
def test_step(self):
"""Test function ``step``."""
figure(); plot_shape = (1, 3)
#Test SISO system
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
#print(sys)
#print("gain:", dcgain(sys))
subplot2grid(plot_shape, (0, 0))
t, y = step(sys)
plot(t, y)
subplot2grid(plot_shape, (0, 1))
T = linspace(0, 2, 100)
X0 = array([1, 1])
t, y = step(sys, T, X0)
plot(t, y)
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
subplot2grid(plot_shape, (0, 2))
t, y = step(sys)
plot(t, y)
def testConnect2(self):
"""Test append and connect() case 2"""
sys = append(ss([[-5, -2.25],
[4, 0]],
[[2],
[0]],
[[0, 1.125]],
[[0]]),
ss([[-1.6667, 0],
[1, 0]],
[[2], [0]],
[[0, 3.3333]], [[0]]),
1)
Q = [[1, 3],
[2, 1],
[3, -2]]
sysc = connect(sys, Q, [3], [3, 1, 2])
np.testing.assert_array_almost_equal(
sysc.A, np.array([[-5, -2.25, 0, -6.6666],
[4, 0, 0, 0],
[0, 2.25, -1.6667, 0],
[0, 0, 1, 0]]))
np.testing.assert_array_almost_equal(
sysc.B, np.array([[2], [0], [0], [0]]))
np.testing.assert_array_almost_equal(
sysc.C, np.array([[0, 0, 0, -3.3333],
[0, 1.125, 0, 0],
[0, 0, 0, 3.3333]]))
np.testing.assert_array_almost_equal(
sysc.D, np.array([[1], [0], [0]]))
def testCombi01(self):
"""Test from a "real" case, combines tf, ss, connect and margin.
This is a type 2 system, with phase starting at -180. The
margin command should remove the solution for w = nearly zero.
"""
# Example is a concocted two-body satellite with flexible link
Jb = 400
Jp = 1000
k = 10
b = 5
# can now define an "s" variable, to make TF's
s = tf([1, 0], [1])
hb1 = 1 / (Jb * s)
hb2 = 1 / s
hp1 = 1 / (Jp * s)
hp2 = 1 / s
# convert to ss and append
sat0 = append(ss(hb1), ss(hb2), k, b, ss(hp1), ss(hp2))
# connection of the elements with connect call
Q = [
[1, -3, -4], # link moment (spring, damper), feedback to body
[2, 1, 0], # link integrator to body velocity
[3, 2, -6], # spring input, th_b - th_p
[4, 1, -5], # damper input
[5, 3, 4], # link moment, acting on payload
[6, 5, 0]
]
inputs = [1]
outputs = [1, 2, 5, 6]
sat1 = connect(sat0, Q, inputs, outputs)
# matched notch filter
wno = 0.19
z1 = 0.05
z2 = 0.7
Hno = (1 + 2 * z1 / wno * s + s**2 / wno**2) / (1 + 2 * z2 / wno * s +
s**2 / wno**2)
# the controller, Kp = 1 for now
Kp = 1.64
tau_PD = 50.
Hc = (1 + tau_PD * s) * Kp
# start with the basic satellite model sat1, and get the
# payload attitude response
Hp = tf(np.array([0, 0, 0, 1]) * sat1)
# total open loop
Hol = Hc * Hno * Hp
gm, pm, wg, wp = margin(Hol)
# print("%f %f %f %f" % (gm, pm, wg, wp))
np.testing.assert_allclose(gm, 3.32065569155)
np.testing.assert_allclose(pm, 46.9740430224)
np.testing.assert_allclose(wg, 0.176469728448)
np.testing.assert_allclose(wp, 0.0616288455466)
def test_step(self):
"""Test function ``step``."""
figure()
plot_shape = (1, 3)
#Test SISO system
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
#print(sys)
#print("gain:", dcgain(sys))
subplot2grid(plot_shape, (0, 0))
t, y = step(sys)
plot(t, y)
subplot2grid(plot_shape, (0, 1))
T = linspace(0, 2, 100)
X0 = array([1, 1])
t, y = step(sys, T, X0)
plot(t, y)
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
subplot2grid(plot_shape, (0, 2))
t, y = step(sys)
plot(t, y)
def test_lsim(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure()
plot_shape = (2, 2)
#Test with arrays
subplot2grid(plot_shape, (0, 0))
t = linspace(0, 1, 100)
u = r_[1:1:50j, 0:0:50j]
y, _t, _x = lsim(sys, u, t)
plot(t, y, label='y')
plot(t, u / 10, label='u/10')
legend(loc='best')
#Test with U=None - uses 2nd algorithm which is much faster.
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
x0 = [-1, -1]
y, _t, _x = lsim(sys, U=None, T=t, X0=x0)
plot(t, y, label='y')
legend(loc='best')
#Test with U=0, X0=0
#Correct reaction to zero dimensional special values
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
y, _t, _x = lsim(sys, U=0, T=t, X0=0)
plot(t, y, label='y')
legend(loc='best')
#Test with MIMO system
subplot2grid(plot_shape, (1, 1))
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
t = array(linspace(0, 1, 100))
u = array([r_[1:1:50j, 0:0:50j], r_[0:1:50j, 0:0:50j]])
x0 = [0, 0, 0, 0]
y, t_out, _x = lsim(sys, u, t, x0)
plot(t_out, y[0], label='y[0]')
plot(t_out, y[1], label='y[1]')
plot(t_out, u[0] / 10, label='u[0]/10')
plot(t_out, u[1] / 10, label='u[1]/10')
legend(loc='best')
#Test with wrong values for t
#T is None; - special handling: Value error
self.assertRaises(ValueError, lsim(sys, U=0, T=None, x0=0))
#T="hello" : Wrong type
#TODO: better wording of error messages of ``lsim`` and
# ``_check_convert_array``, when wrong type is given.
# Current error message is too cryptic.
self.assertRaises(TypeError, lsim(sys, U=0, T="hello", x0=0))
#T=0; - T can not be zero dimensional, it determines the size of the
# input vector ``U``
self.assertRaises(ValueError, lsim(sys, U=0, T=0, x0=0))
#T is not monotonically increasing
self.assertRaises(ValueError,
lsim(sys, U=0, T=[0., 1., 2., 2., 3.], x0=0))
def test_impulse(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure()
#everything automatically
t, y = impulse(sys)
plot(t, y, label='Simple Case')
#supply time and X0
T = linspace(0, 2, 100)
X0 = [0.2, 0.2]
t, y = impulse(sys, T, X0)
plot(t, y, label='t=0..2, X0=[0.2, 0.2]')
#Test system with direct feed-though, the function should print a warning.
D = [[0.5]]
sys_ft = ss(A, B, C, D)
with warnings.catch_warnings():
warnings.simplefilter("ignore")
t, y = impulse(sys_ft)
plot(t, y, label='Direct feedthrough D=[[0.5]]')
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
t, y = impulse(sys)
plot(t, y, label='MIMO System')
legend(loc='best')
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 test_step(self, SISO_mats, MIMO_mats, mplcleanup):
"""Test function ``step``."""
figure()
plot_shape = (1, 3)
#Test SISO system
A, B, C, D = SISO_mats
sys = ss(A, B, C, D)
#print(sys)
#print("gain:", dcgain(sys))
subplot2grid(plot_shape, (0, 0))
y, t = step(sys)
plot(t, y)
subplot2grid(plot_shape, (0, 1))
T = linspace(0, 2, 100)
X0 = array([1, 1])
y, t = step(sys, T, X0)
plot(t, y)
# Test output of state vector
y, t, x = step(sys, return_x=True)
#Test MIMO system
A, B, C, D = MIMO_mats
sys = ss(A, B, C, D)
subplot2grid(plot_shape, (0, 2))
y, t = step(sys)
plot(t, y[:, 0, 0])
def __init__(self, Ts):
# steady state wind defined in the inertial frame
self._steady_state = np.array([[0., 0., 0.]]).T
# self.steady_state = np.array([[3., 1., 0.]]).T
# Dryden gust model parameters (pg 56 UAV book)
# HACK: Setting Va to a constant value is a hack. We set a nominal airspeed for the gust model.
# Could pass current Va into the gust function and recalculate A and B matrices.
Va = 17
# self._A = np.array([[1-Ts*c, -Ts*d],[Ts, 1]])
# self._B = np.array([[Ts],[0]])
# self._C = np.array([[a, b]])
suv = 1.06
sw = 0.7
luv = 200
lw = 50
hu_n = np.array([suv * np.sqrt(2 * Va / luv)])
hu_d = np.array([1, Va / luv])
hv_n = suv * np.sqrt(3 * Va / luv) * np.array(
[1, Va / (np.sqrt(3) * luv)])
hv_d = np.array([1, 2 * Va / luv, (Va / luv)**2])
hw_n = sw * np.sqrt(3 * Va / lw) * np.array(
[1, Va / (np.sqrt(3) * lw)])
hw_d = np.array([1, 2 * Va / lw, (Va / lw)**2])
self.hu = ctl.ss(ctl.tf(hu_n, hu_d, Ts))
self.hv = ctl.ss(ctl.tf(hv_n, hv_d, Ts))
self.hw = ctl.ss(ctl.tf(hw_n, hw_d, Ts))
self.huA = np.asarray(self.hu.A)
self.huB = np.asarray(self.hu.B)
self.huC = np.asarray(self.hu.C)
self.hvA = np.asarray(self.hv.A)
self.hvB = np.asarray(self.hv.B)
self.hvC = np.asarray(self.hv.C)
self.hwA = np.asarray(self.hw.A)
self.hwB = np.asarray(self.hw.B)
self.hwC = np.asarray(self.hw.C)
self._gust_state_u = np.array([0.])
self._gust_state_v = np.array([[0.], [0.]])
self._gust_state_w = np.array([[0.], [0.]])
self._Ts = Ts
def test_dcgain_2():
"""Test function dcgain with different systems"""
#Create different forms of a SISO system
A, B, C, D = make_SISO_mats()
Z, P, k = scipy.signal.ss2zpk(A, B, C, D)
num, den = scipy.signal.ss2tf(A, B, C, D)
sys_ss = ss(A, B, C, D)
#Compute the gain with ``dcgain``
gain_abcd = dcgain(A, B, C, D)
gain_zpk = dcgain(Z, P, k)
gain_numden = dcgain(np.squeeze(num), den)
gain_sys_ss = dcgain(sys_ss)
print 'gain_abcd:', gain_abcd, 'gain_zpk:', gain_zpk
print 'gain_numden:', gain_numden, 'gain_sys_ss:', gain_sys_ss
#Compute the gain with a long simulation
t = linspace(0, 1000, 1000)
_t, y = step(sys_ss, t)
gain_sim = y[-1]
print 'gain_sim:', gain_sim
#All gain values must be approximately equal to the known gain
assert_array_almost_equal([
gain_abcd[0, 0], gain_zpk[0, 0], gain_numden[0, 0], gain_sys_ss[0, 0],
gain_sim
], [0.026948, 0.026948, 0.026948, 0.026948, 0.026948],
decimal=6)
#Test with MIMO system
A, B, C, D = make_MIMO_mats()
gain_mimo = dcgain(A, B, C, D)
print 'gain_mimo: \n', gain_mimo
assert_array_almost_equal(gain_mimo, [[0.026948, 0], [0, 0.026948]],
decimal=6)
class dc:
__slots__ = [
'name', 'sysc', 'Ts', 'sysd', 'x_0', 'y_0', 'p', 'i', 'd', 'ref',
'est', 'slot', 't_arr', 't_attack', 't_detect', 'attacks', 'y_index',
'safeset', 'target_set', 'control_limit', 'max_k',
'worst_case_control', 'k', 'epsilon', 'sep_graph', 'y_label', 'x_left',
'y_up', 'y_lo', 'total', 'thres', 'drift', 'y_real_arr', 's', 'score',
'ymeasure', 'yreal', 'att', 'cin', 'place', 'maxc', 'xreal', 'xmeasure'
]
count = 0
J = 0.01
b = 0.1
K = 0.01
R = 1
L = 0.5
A = [[0, 1, 0], [0, -b / J, K / J], [0, -K / L, -R / L]]
B = [[0], [0], [1 / L]]
C = [[1, 0, 0]]
D = [[0]]
sysc_default = ss(A, B, C, D)
dt_default = 0.2
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7):
self.sysd = c2d(sysc, Ts)
self.Ts = Ts
self.sysc = sysc
self.epsilon = epsilon
self.est = Estimator(self.sysd, 500, self.epsilon)
self.y_index = None
self.worst_case_control = 'current'
self.k = None
self.y_up = None
self.y_lo = None
# diy
self.p = 11
self.i = 0
self.d = 5
self.total = 24
self.slot = int(self.total / Ts)
self.t_arr = linspace(0, self.total, self.slot + 1)
self.ref = [math.pi / 2] * 71 + [math.pi / 2] * 50
self.thres = 0.2
self.drift = 0.01
self.x_0 = [0]
self.y_real_arr = []
self.s = 0
self.att = 0
self.cin = 0
self.ymeasure = 0
self.yreal = 0
self.score = []
self.place = 20
self.maxc = 20
self.xmeasure = [[0], [0], [0]]
self.xreal = [[0], [0], [0]]
self.safeset = {
'lo': [-4, -1000000000, -100000000],
'up': [4, 1000000000, 100000000]
}
def asym_flight(ac: FlightParams):
# Construct the asymmetric flight state-space system for an aircraft
C1a =np.matrix([[(ac.CYbdot-2*ac.mub)*ac.b/ac.V, 0 , 0 , 0],
[0 , -0.5*ac.b/ac.V, 0 , 0],
[0 , 0 , -4*ac.mub*ac.KX2*ac.b/ac.V , 4*ac.mub*ac.KXZ*ac.b/ac.V],
[ac.Cnbdot*ac.b/ac.V, 0 , 4*ac.mub*ac.KXZ*ac.b/ac.V , -4*ac.mub*ac.KZ2*ac.b/ac.V]])
C2a =np.matrix([[-ac.CYb, -ac.CL , -ac.CYp , -ac.CYr+4*ac.mub],
[0 , 0, -1 , 0],
[-ac.Clb , 0 , -ac.Clp , -ac.Clr],
[-ac.Cnb, 0 , -ac.Cnp , -ac.Cnr]])
C3a = np.matrix([[-ac.CYda, -ac.CYdr],
[0 , 0],
[-ac.Clda, -ac.Cldr],
[-ac.Cnda, -ac.Cndr]])
C1ainv = np.linalg.inv(C1a)
Aa = C1ainv*C2a
Ba = C1ainv*C3a
Ca = np.identity(4)
Ca[1,:] = Aa[1,:]
Da = np.zeros([4 , 2])
Da[1,:] = Ba[1,:]
return ml.ss(Aa , Ba , Ca , Da)
def sym_flight(ac: FlightParams):
# Construct the symmetric flight state-space system for an aircraft
C1s = np.matrix([[-2*ac.muc*ac.c/ac.Vtas, 0 , 0 , 0],
[0 , (ac.CZadot -2*ac.muc)*ac.c/ac.Vtas, 0 , 0],
[0 , 0 , -ac.c/ac.Vtas , 0],
[0, ac.Cmadot*ac.c/ac.Vtas , 0 , -2*ac.muc*ac.KY2*ac.c/ac.Vtas]])
C2s = np.matrix([[-ac.CXu, -ac.CXa , -ac.CZ0 , -ac.CXq],
[-ac.CZu , -ac.CZa, ac.CX0 , -ac.CZq - 2*ac.muc],
[0 , 0 , 0 , -1],
[-ac.Cmu, -ac.Cma , 0 , -ac.Cmq]])
C3s = np.matrix([[-ac.CXde],
[-ac.CZde],
[0],
[-ac.Cmde]])
C1sinv = np.linalg.inv(C1s)
As = C1sinv*C2s
Bs = C1sinv*C3s
Cs = np.identity(4)
Ds =np.zeros([4,1])
return ml.ss(As, Bs, Cs, Ds)
def asym_flight(ac: FlightParams):
# Construct the asymmetric flight state-space system for an aircraft
C1a = np.matrix([[(ac.CYbdot - 2 * ac.mub), 0, 0, 0],
[0, -1 / 2 * ac.b / ac.V0, 0, 0],
[
0, 0, -4 * ac.mub * ac.KX2 * ac.b / ac.V0,
4 * ac.mub * ac.KXZ * ac.b / ac.V0
],
[
ac.Cnbdot * ac.b / ac.V0, 0,
4 * ac.mub * ac.KXZ * ac.b / ac.V0,
-4 * ac.mub * ac.KX2 * ac.b / ac.V0
]])
C2a = np.matrix([[ac.CYb, ac.CL, ac.CYp, ac.CYr - 4 * ac.mub],
[0, 0, 1, 0], [ac.Clb, 0, ac.Clp, ac.Clr],
[ac.Cnb, 0, ac.Cnp, ac.Cnr]])
C3a = np.matrix([[ac.CYda, ac.CYdr], [0, 0], [ac.Clda, ac.Cldr],
[ac.Cnda, ac.Cndr]])
C1ainv = np.linalg.inv(C1a)
Aa = -C1ainv * C2a
Ba = -C1ainv * C3a
Ca = np.identity(4)
Da = np.zeros([4, 2])
return ml.ss(Aa, Ba, Ca, Da)
def test_dcgain_2(self):
"""Test function dcgain with different systems"""
#Create different forms of a SISO system
A, B, C, D = self.make_SISO_mats()
Z, P, k = scipy.signal.ss2zpk(A, B, C, D)
num, den = scipy.signal.ss2tf(A, B, C, D)
sys_ss = ss(A, B, C, D)
#Compute the gain with ``dcgain``
gain_abcd = dcgain(A, B, C, D)
gain_zpk = dcgain(Z, P, k)
gain_numden = dcgain(np.squeeze(num), den)
gain_sys_ss = dcgain(sys_ss)
print('gain_abcd:', gain_abcd, 'gain_zpk:', gain_zpk)
print('gain_numden:', gain_numden, 'gain_sys_ss:', gain_sys_ss)
#Compute the gain with a long simulation
t = linspace(0, 1000, 1000)
y, _t = step(sys_ss, t)
gain_sim = y[-1]
print('gain_sim:', gain_sim)
#All gain values must be approximately equal to the known gain
assert_array_almost_equal([gain_abcd[0,0], gain_zpk[0,0],
gain_numden[0,0], gain_sys_ss[0,0], gain_sim],
[0.026948, 0.026948, 0.026948, 0.026948,
0.026948],
decimal=6)
#Test with MIMO system
A, B, C, D = self.make_MIMO_mats()
gain_mimo = dcgain(A, B, C, D)
print('gain_mimo: \n', gain_mimo)
assert_array_almost_equal(gain_mimo, [[0.026948, 0 ],
[0, 0.026948]], decimal=6)
def to_siso(self, iu, iy):
"""Return a SISO system given input and output indices.
**Parameters:**
- *iu*: Index or name of the input
This must be specified unless the system has only one input.
- *iy*: Index or name of the output
This must be specified unless the system has only one output.
**Example:**
>>> lin = LinRes('examples/PID.mat')
>>> lin.to_siso()
A = [[ 0. 0.]
[ 0. -100.]]
<BLANKLINE>
B = [[ 2.]
[ 100.]]
<BLANKLINE>
C = [[ 1. -10.]]
<BLANKLINE>
D = [[ 11.]]
<BLANKLINE>
"""
return ss(self.sys.A, self.sys.B[:, iu],
self.sys.C[iy, :], self.sys.D[iy, iu])
def test_dcgain_2(self):
"""Test function dcgain with different systems"""
#Create different forms of a SISO system
A, B, C, D = self.make_SISO_mats()
num, den = scipy.signal.ss2tf(A, B, C, D)
# numerator is only a constant here; pick it out to avoid numpy warning
Z, P, k = scipy.signal.tf2zpk(num[0][-1], den)
sys_ss = ss(A, B, C, D)
#Compute the gain with ``dcgain``
gain_abcd = dcgain(A, B, C, D)
gain_zpk = dcgain(Z, P, k)
gain_numden = dcgain(np.squeeze(num), den)
gain_sys_ss = dcgain(sys_ss)
# print('gain_abcd:', gain_abcd, 'gain_zpk:', gain_zpk)
# print('gain_numden:', gain_numden, 'gain_sys_ss:', gain_sys_ss)
#Compute the gain with a long simulation
t = linspace(0, 1000, 1000)
y, _t = step(sys_ss, t)
gain_sim = y[-1]
# print('gain_sim:', gain_sim)
#All gain values must be approximately equal to the known gain
assert_array_almost_equal([gain_abcd[0,0], gain_zpk[0,0],
gain_numden[0,0], gain_sys_ss[0,0], gain_sim],
[0.026948, 0.026948, 0.026948, 0.026948,
0.026948],
decimal=6)
def test_dcgain_2(self):
"""Test function dcgain with different systems"""
#Create different forms of a SISO system
A, B, C, D = self.make_SISO_mats()
num, den = scipy.signal.ss2tf(A, B, C, D)
# numerator is only a constant here; pick it out to avoid numpy warning
Z, P, k = scipy.signal.tf2zpk(num[0][-1], den)
sys_ss = ss(A, B, C, D)
#Compute the gain with ``dcgain``
gain_abcd = dcgain(A, B, C, D)
gain_zpk = dcgain(Z, P, k)
gain_numden = dcgain(np.squeeze(num), den)
gain_sys_ss = dcgain(sys_ss)
# print('gain_abcd:', gain_abcd, 'gain_zpk:', gain_zpk)
# print('gain_numden:', gain_numden, 'gain_sys_ss:', gain_sys_ss)
#Compute the gain with a long simulation
t = linspace(0, 1000, 1000)
y, _t = step(sys_ss, t)
gain_sim = y[-1]
# print('gain_sim:', gain_sim)
#All gain values must be approximately equal to the known gain
assert_array_almost_equal(
[gain_abcd, gain_zpk, gain_numden, gain_sys_ss, gain_sim],
[0.026948, 0.026948, 0.026948, 0.026948, 0.026948],
decimal=6)
def testMinreal(self, verbose=False):
"""Test a minreal model reduction"""
# A = [-2, 0.5, 0; 0.5, -0.3, 0; 0, 0, -0.1]
A = [[-2, 0.5, 0], [0.5, -0.3, 0], [0, 0, -0.1]]
# B = [0.3, -1.3; 0.1, 0; 1, 0]
B = [[0.3, -1.3], [0.1, 0.], [1.0, 0.0]]
# C = [0, 0.1, 0; -0.3, -0.2, 0]
C = [[0., 0.1, 0.0], [-0.3, -0.2, 0.0]]
# D = [0 -0.8; -0.3 0]
D = [[0., -0.8], [-0.3, 0.]]
# sys = ss(A, B, C, D)
sys = ss(A, B, C, D)
sysr = minreal(sys, verbose=verbose)
assert sysr.nstates == 2
assert sysr.ninputs == sys.ninputs
assert sysr.noutputs == sys.noutputs
np.testing.assert_array_almost_equal(
eigvals(sysr.A), [-2.136154, -0.1638459])
s = tf([1, 0], [1])
h = (s+1)*(s+2.00000000001)/(s+2)/(s**2+s+1)
hm = minreal(h, verbose=verbose)
hr = (s+1)/(s**2+s+1)
np.testing.assert_array_almost_equal(hm.num[0][0], hr.num[0][0])
np.testing.assert_array_almost_equal(hm.den[0][0], hr.den[0][0])
def system(self):
""" Organizes the obtained state-space matrices into a state-space object utilizing :class:`ss`
:return: State-space system utilizing the control library
:rtype: control.matlab.ss
"""
return ss(self.a_matrix, self.b_matrix, self.c_matrix, self.d_matrix)
def test_impulse_mimo(self, MIMO_mats, mplcleanup):
#Test MIMO system
A, B, C, D = MIMO_mats
sys = ss(A, B, C, D)
y, t = impulse(sys)
plot(t, y[:, :, 0], label='MIMO System')
legend(loc='best')
def test_accept_sys(self):
"""
Tests if passing a StateSpace as ``sys`` raises no errors.
"""
[A, B, C, D] = self.__build_standard_abcd()
sys = ss(A, B, C, D)
dsf(sys=sys)
def test_dcgain_siso(self, SISO_mats):
"""Test function dcgain with SISO systems"""
A, B, C, D = SISO_mats
gain1 = dcgain(ss(A, B, C, D))
assert_array_almost_equal(gain1,
array([[0.0269]]),
decimal=4)
def test_mimo(self):
# MIMO
B = np.matrix('1,0;0,1')
D = np.matrix('0,0')
sysMIMO = ss(self.A, B, self.C, D)
frqMIMO = sysMIMO.freqresp(self.omega)
tfMIMO = tf(sysMIMO)
def test_doubleint(self):
# 30 May 2016, RMM: added to replicate typecast bug in freqresp.py
A = np.matrix('0, 1; 0, 0');
B = np.matrix('0; 1');
C = np.matrix('1, 0');
D = 0;
sys = ss(A, B, C, D);
bode(sys);
def test_doubleint(self):
# 30 May 2016, RMM: added to replicate typecast bug in freqresp.py
A = np.matrix('0, 1; 0, 0')
B = np.matrix('0; 1')
C = np.matrix('1, 0')
D = 0
sys = ss(A, B, C, D)
bode(sys)
def testConnect(self):
"""Test append() and connect()"""
sys1 = ss([[1., -2], [3., -4]], [[5.], [7]], [[6, 8]], [[9.]])
sys2 = ss(-1., 1., 1., 0.)
sys = append(sys1, sys2)
Q = np.array([
[1, 2], # basically feedback, output 2 in 1
[2, -1]
])
sysc = connect(sys, Q, [2], [1, 2])
# print(sysc)
np.testing.assert_array_almost_equal(
sysc.A, np.array([[1, -2, 5], [3, -4, 7], [-6, -8, -10]]))
np.testing.assert_array_almost_equal(sysc.B, np.array([[0], [0], [1]]))
np.testing.assert_array_almost_equal(sysc.C,
np.array([[6, 8, 9], [0, 0, 1]]))
np.testing.assert_array_almost_equal(sysc.D, np.array([[0], [0]]))
def test_mimo(self):
# MIMO
B = np.matrix('1,0;0,1')
D = np.matrix('0,0')
sysMIMO = ss(self.A,B,self.C,D)
frqMIMO = sysMIMO.freqresp(self.omega)
tfMIMO = tf(sysMIMO)
def ss_siso_ct():
A = np.diag([-1 / 75.0])
B = np.array([[1.0]]) / 75.0
C = np.array([[87.8]])
D = np.zeros((1, 1))
T = TSys(ss(A, B, C, D))
T.omegas = [0.0, np.array([0.0, 0.01])]
T.sigmas = [np.array([[87.8]]), np.array([[87.8, 70.24]])]
return T
def main():
# 制御対象を定義(今回はモータを想定)
J = 0.6 # 慣性
D = 0.2 # 粘性
Kt = 0.15 # トルク定数
P = crl.tf([Kt], [J, D, 0]) # モータ角度の電流制御
# 参照モデルの定義
M = crl.tf([50], [1, 50])
# シミュレーション条件を定義
Ts = 0.001 # サンプリング時間
# 制御器構造の定義
beta = crl.ss("0", "1", "0; 1", "1; 0") # PI制御器
print(crl.tf(beta))
beta = crl.c2d(beta, Ts)
# 制御対象と参照モデルのゲイン線図
plt.figure(1)
crl.bode([P, M])
plt.legend(["Plant", "Ref. model"])
# prbsを生成する
n = 12 # m系列信号の次数
Tp = 5 # 何周期印加するか？
Ap = 0.5 # 振幅 [A]
u0 = 2 * prbs(n)
u0 = Ap * (u0 - np.average(u0))
u0 = np.tile(u0, Tp)
t = np.linspace(0, Ts * u0.shape[0], u0.shape[0])
plt.figure(2)
plt.plot(t, u0)
plt.xlabel("Time [s]")
plt.ylabel("Current [A]")
# 実験データの取得
print("=== Start Experiment ===")
Pd = crl.c2d(P, Ts, "tustin") # 双一次変換で離散時間モデルを作成
t = np.arange(start=0, stop=Tp * Ts * (2**n - 1), step=Ts)
(y0, t0, xout) = crl.lsim(Pd, u0[:], t[:]) # 応答を取得
plt.figure(3)
plt.plot(y0)
# VRFTを実行する
print("=== Start VRFT ===")
W = crl.tf([50], [1, 50]) # 周波数重み
L = crl.minreal((1 - M) * M * W) # プレフィルタ
Ld = crl.c2d(L, Ts, "tustin")
(ul, tl, xlout) = crl.lsim(Ld, u0, t)
(phi_l, tl, xlout) = crl.lsim(beta, y0[:])
print(phi_l)
rho = np.linalg.solve(phi_l, ul)
print(rho)
plt.show()
def mimo(self):
"""Create MIMO system, contains ``siso_ss1`` twice"""
m = tsystems()
A = np.array([[1., -2., 0., 0.], [3., -4., 0., 0.], [0., 0., 1., -2.],
[0., 0., 3., -4.]])
B = np.array([[5., 0.], [7., 0.], [0., 5.], [0., 7.]])
C = np.array([[6., 8., 0., 0.], [0., 0., 6., 8.]])
D = np.array([[9., 0.], [0., 9.]])
m.ss1 = ss(A, B, C, D)
return m
def testMinrealBrute(self):
for n, m, p in permutations(range(1, 6), 3):
s = matlab.rss(n, p, m)
sr = s.minreal()
if s.states > sr.states:
self.nreductions += 1
else:
# Check to make sure that poles and zeros match
# For poles, just look at eigenvalues of A
np.testing.assert_array_almost_equal(np.sort(eigvals(s.A)),
np.sort(eigvals(sr.A)))
# For zeros, need to extract SISO systems
for i in range(m):
for j in range(p):
# Extract SISO dynamixs from input i to output j
s1 = matlab.ss(s.A, s.B[:, i], s.C[j, :], s.D[j, i])
s2 = matlab.ss(sr.A, sr.B[:, i], sr.C[j, :], sr.D[j,
i])
# Check that the zeros match
# Note: sorting doesn't work => have to do the hard way
z1 = matlab.zero(s1)
z2 = matlab.zero(s2)
# Start by making sure we have the same # of zeros
self.assertEqual(len(z1), len(z2))
# Make sure all zeros in s1 are in s2
for zero in z1:
# Find the closest zero
self.assertAlmostEqual(min(abs(z2 - zero)), 0.)
# Make sure all zeros in s2 are in s1
for zero in z2:
# Find the closest zero
self.assertAlmostEqual(min(abs(z1 - zero)), 0.)
# Make sure that the number of systems reduced is as expected
# (Need to update this number if you change the seed at top of file)
self.assertEqual(self.nreductions, 2)
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 test_doubleint():
"""Test typcast bug with double int
30 May 2016, RMM: added to replicate typecast bug in frequency_response.py
"""
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
C = np.array([[1, 0]])
D = 0
sys = ss(A, B, C, D)
bode(sys)
def test_mimo():
"""Test MIMO frequency response calls"""
A = np.array([[1, 1], [0, 1]])
B = np.array([[1, 0], [0, 1]])
C = np.array([[1, 0]])
D = np.array([[0, 0]])
omega = np.linspace(10e-2, 10e2, 1000)
sysMIMO = ss(A, B, C, D)
sysMIMO.freqresp(omega)
tf(sysMIMO)
def testMinrealBrute(self):
for n, m, p in permutations(range(1,6), 3):
s = matlab.rss(n, p, m)
sr = s.minreal()
if s.states > sr.states:
self.nreductions += 1
else:
# Check to make sure that poles and zeros match
# For poles, just look at eigenvalues of A
np.testing.assert_array_almost_equal(
np.sort(eigvals(s.A)), np.sort(eigvals(sr.A)))
# For zeros, need to extract SISO systems
for i in range(m):
for j in range(p):
# Extract SISO dynamixs from input i to output j
s1 = matlab.ss(s.A, s.B[:,i], s.C[j,:], s.D[j,i])
s2 = matlab.ss(sr.A, sr.B[:,i], sr.C[j,:], sr.D[j,i])
# Check that the zeros match
# Note: sorting doesn't work => have to do the hard way
z1 = matlab.zero(s1)
z2 = matlab.zero(s2)
# Start by making sure we have the same # of zeros
self.assertEqual(len(z1), len(z2))
# Make sure all zeros in s1 are in s2
for zero in z1:
# Find the closest zero
self.assertAlmostEqual(min(abs(z2 - zero)), 0.)
# Make sure all zeros in s2 are in s1
for zero in z2:
# Find the closest zero
self.assertAlmostEqual(min(abs(z1 - zero)), 0.)
# Make sure that the number of systems reduced is as expected
# (Need to update this number if you change the seed at top of file)
self.assertEqual(self.nreductions, 2)
def test_initial(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure(); plot_shape = (1, 3)
#X0=0 : must produce line at 0
subplot2grid(plot_shape, (0, 0))
t, y = initial(sys)
plot(t, y)
#X0=[1,1] : produces a spike
subplot2grid(plot_shape, (0, 1))
t, y = initial(sys, X0=matrix("1; 1"))
plot(t, y)
#Test MIMO system
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
#X0=[1,1] : produces same spike as above spike
subplot2grid(plot_shape, (0, 2))
t, y = initial(sys, X0=[1, 1, 0, 0])
plot(t, y)
def testFreqResp(self):
"""Compare the bode reponses of the SS systems and TF systems to the original SS
They generally are different realizations but have same freq resp.
Currently this test may only be applied to SISO systems.
"""
for states in range(1,self.maxStates):
for testNum in range(self.numTests):
for inputs in range(1,1):
for outputs in range(1,1):
ssOriginal = matlab.rss(states, inputs, outputs)
tfOriginal_Actrb, tfOriginal_Bctrb, tfOriginal_Cctrb, tfOrigingal_nctrb, tfOriginal_index,\
tfOriginal_dcoeff, tfOriginal_ucoeff = tb04ad(states,inputs,outputs,\
ssOriginal.A,ssOriginal.B,ssOriginal.C,ssOriginal.D,tol1=0.0)
ssTransformed_nr, ssTransformed_A, ssTransformed_B, ssTransformed_C, ssTransformed_D\
= td04ad('R',inputs,outputs,tfOriginal_index,tfOriginal_dcoeff,tfOriginal_ucoeff,tol=0.0)
tfTransformed_Actrb, tfTransformed_Bctrb, tfTransformed_Cctrb, tfTransformed_nctrb,\
tfTransformed_index, tfTransformed_dcoeff, tfTransformed_ucoeff = tb04ad(\
ssTransformed_nr,inputs,outputs,ssTransformed_A, ssTransformed_B, ssTransformed_C,\
ssTransformed_D,tol1=0.0)
numTransformed = np.array(tfTransformed_ucoeff)
denTransformed = np.array(tfTransformed_dcoeff)
numOriginal = np.array(tfOriginal_ucoeff)
denOriginal = np.array(tfOriginal_dcoeff)
ssTransformed = matlab.ss(ssTransformed_A,ssTransformed_B,ssTransformed_C,ssTransformed_D)
for inputNum in range(inputs):
for outputNum in range(outputs):
[ssOriginalMag,ssOriginalPhase,freq] = matlab.bode(ssOriginal,Plot=False)
[tfOriginalMag,tfOriginalPhase,freq] = matlab.bode(matlab.tf(numOriginal[outputNum][inputNum],denOriginal[outputNum]),Plot=False)
[ssTransformedMag,ssTransformedPhase,freq] = matlab.bode(ssTransformed,freq,Plot=False)
[tfTransformedMag,tfTransformedPhase,freq] = matlab.bode(matlab.tf(numTransformed[outputNum][inputNum],denTransformed[outputNum]),freq,Plot=False)
#print 'numOrig=',numOriginal[outputNum][inputNum]
#print 'denOrig=',denOriginal[outputNum]
#print 'numTrans=',numTransformed[outputNum][inputNum]
#print 'denTrans=',denTransformed[outputNum]
np.testing.assert_array_almost_equal(ssOriginalMag,tfOriginalMag,decimal=3)
np.testing.assert_array_almost_equal(ssOriginalPhase,tfOriginalPhase,decimal=3)
np.testing.assert_array_almost_equal(ssOriginalMag,ssTransformedMag,decimal=3)
np.testing.assert_array_almost_equal(ssOriginalPhase,ssTransformedPhase,decimal=3)
np.testing.assert_array_almost_equal(tfOriginalMag,tfTransformedMag,decimal=3)
np.testing.assert_array_almost_equal(tfOriginalPhase,tfTransformedPhase,decimal=2)
def testTF(self, verbose=False):
""" Directly tests the functions tb04ad and td04ad through direct comparison of transfer function coefficients.
Similar to convert_test, but tests at a lower level.
"""
from slycot import tb04ad, td04ad
for states in range(1, self.maxStates):
for inputs in range(1, self.maxI+1):
for outputs in range(1, self.maxO+1):
for testNum in range(self.numTests):
ssOriginal = matlab.rss(states, inputs, outputs)
if (verbose):
print('====== Original SS ==========')
print(ssOriginal)
print('states=', states)
print('inputs=', inputs)
print('outputs=', outputs)
tfOriginal_Actrb, tfOriginal_Bctrb, tfOriginal_Cctrb, tfOrigingal_nctrb, tfOriginal_index,\
tfOriginal_dcoeff, tfOriginal_ucoeff = tb04ad(states,inputs,outputs,\
ssOriginal.A,ssOriginal.B,ssOriginal.C,ssOriginal.D,tol1=0.0)
ssTransformed_nr, ssTransformed_A, ssTransformed_B, ssTransformed_C, ssTransformed_D\
= td04ad('R',inputs,outputs,tfOriginal_index,tfOriginal_dcoeff,tfOriginal_ucoeff,tol=0.0)
tfTransformed_Actrb, tfTransformed_Bctrb, tfTransformed_Cctrb, tfTransformed_nctrb,\
tfTransformed_index, tfTransformed_dcoeff, tfTransformed_ucoeff = tb04ad(ssTransformed_nr,\
inputs,outputs,ssTransformed_A, ssTransformed_B, ssTransformed_C,ssTransformed_D,tol1=0.0)
#print 'size(Trans_A)=',ssTransformed_A.shape
if (verbose):
print('===== Transformed SS ==========')
print(matlab.ss(ssTransformed_A, ssTransformed_B, ssTransformed_C, ssTransformed_D))
# print 'Trans_nr=',ssTransformed_nr
# print 'tfOrig_index=',tfOriginal_index
# print 'tfOrig_ucoeff=',tfOriginal_ucoeff
# print 'tfOrig_dcoeff=',tfOriginal_dcoeff
# print 'tfTrans_index=',tfTransformed_index
# print 'tfTrans_ucoeff=',tfTransformed_ucoeff
# print 'tfTrans_dcoeff=',tfTransformed_dcoeff
#Compare the TF directly, must match
#numerators
np.testing.assert_array_almost_equal(tfOriginal_ucoeff,tfTransformed_ucoeff,decimal=3)
#denominators
np.testing.assert_array_almost_equal(tfOriginal_dcoeff,tfTransformed_dcoeff,decimal=3)
def testTF(self, verbose=False):
""" Directly tests the functions tb04ad and td04ad through direct
comparison of transfer function coefficients.
Similar to convert_test, but tests at a lower level.
"""
from slycot import tb04ad, td04ad
for states in range(1, self.maxStates):
for inputs in range(1, self.maxI+1):
for outputs in range(1, self.maxO+1):
for testNum in range(self.numTests):
ssOriginal = matlab.rss(states, outputs, inputs)
if (verbose):
print('====== Original SS ==========')
print(ssOriginal)
print('states=', states)
print('inputs=', inputs)
print('outputs=', outputs)
tfOriginal_Actrb, tfOriginal_Bctrb, tfOriginal_Cctrb,\
tfOrigingal_nctrb, tfOriginal_index,\
tfOriginal_dcoeff, tfOriginal_ucoeff =\
tb04ad(states, inputs, outputs,
ssOriginal.A, ssOriginal.B,
ssOriginal.C, ssOriginal.D, tol1=0.0)
ssTransformed_nr, ssTransformed_A, ssTransformed_B,\
ssTransformed_C, ssTransformed_D\
= td04ad('R', inputs, outputs, tfOriginal_index,
tfOriginal_dcoeff, tfOriginal_ucoeff,
tol=0.0)
tfTransformed_Actrb, tfTransformed_Bctrb,\
tfTransformed_Cctrb, tfTransformed_nctrb,\
tfTransformed_index, tfTransformed_dcoeff,\
tfTransformed_ucoeff = tb04ad(
ssTransformed_nr, inputs, outputs,
ssTransformed_A, ssTransformed_B,
ssTransformed_C, ssTransformed_D, tol1=0.0)
# print('size(Trans_A)=',ssTransformed_A.shape)
if (verbose):
print('===== Transformed SS ==========')
print(matlab.ss(ssTransformed_A, ssTransformed_B,
ssTransformed_C, ssTransformed_D))
def __build_sys(self, **kwargs):
if 'A' not in kwargs:
raise ValueError('Must pass either a StateSpace instance or A')
A = np.matrix(kwargs['A'])
if A.shape[0] != A.shape[1]:
# TODO Verify that A must be square, or if this can be relaxed
raise ValueError('A must be square.')
size = A.shape[0]
B = np.matrix(kwargs.get('B', np.identity(size)))
C = np.matrix(kwargs.get('C', np.identity(size)))
D = np.matrix(kwargs.get('D', np.zeros((size, size))))
#print 'A = ', A
#print 'B = ', B
#print 'C = ', C
#print 'D = ', D
return ss(A, B, C, D)
def __build_sys(A, B, C, D):
"""
If a :mod:`control.statesp.StateSpace` instance is not given, in dsf,
this takes the A, B, and C (required) and D (optional) parameters
and returns a ``StateSpace`` instance.
"""
if (A is None) or (B is None) or (C is None):
raise ValueError('Must either pass a StateSpace instance or (A, B, C)')
if isinstance(A, list):
A = np.matrix(A)
if isinstance(B, list):
B = np.matrix(B)
if isinstance(C, list):
C = np.matrix(C)
if D is None:
D = np.zeros_like(B)
elif isinstance(D, list):
D = np.matrix(D)
return ss(A, B, C, D)
def readlin(fname):
r"""Load Dymola\ :sup:`®`-formatted linearization results.
**Parameters:**
- *fname*: Name of the results file, including the path and extension
**Returns:**
- An instance of :class:`control.StateSpace`, which contains:
- *A*, *B*, *C*, *D*: Matrices of the linear system
.. code-block:: modelica
der(x) = A*x + B*u;
y = C*x + D*u;
- *state_names*: List of names of the states (*x*)
- *input_names*: List of names of the inputs (*u*)
- *output_names*: List of names of the outputs (*y*)
**Example:**
>>> sys = readlin('examples/PID.mat')
>>> sys.state_names
['I.y', 'D.x']
"""
# This does the task of mfiles/traj/tloadlin.m in the Dymola installation.
# pylint: disable=I0011, W0621
# Load the file.
data, Aclass = read(fname)
# Check the type of results.
if Aclass[0] == 'Atrajectory':
raise AssertionError(fname + " is a simulation result. Use SimRes "
"instead.")
assert Aclass[0] == 'AlinearSystem', (fname + " isn't a simulation or "
"linearization result.")
# Determine the number of states, inputs, and outputs.
ABCD = data['ABCD']
nx = data['nx'][0]
nu = ABCD.shape[1] - nx
ny = ABCD.shape[0] - nx
# Extract the system matrices.
A = ABCD[:nx, :nx] if nx > 0 else [[]]
B = ABCD[:nx, nx:] if nx > 0 and nu > 0 else [[]]
C = ABCD[nx:, :nx] if nx > 0 and ny > 0 else [[]]
D = ABCD[nx:, nx:] if nu > 0 and ny > 0 else [[]]
sys = ss(A, B, C, D)
# Extract the variable names.
xuyName = data['xuyName']
sys.state_names = xuyName[:nx]
sys.input_names = xuyName[nx:nx + nu]
sys.output_names = xuyName[nx + nu:]
return sys
def testConvert(self):
"""Test state space to transfer function conversion."""
verbose = self.debug
from control.statesp import _mimo2siso
#print __doc__
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
for inputs in range(1, self.maxIO):
for outputs in range(1, self.maxIO):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
ssOriginal = matlab.rss(states, outputs, inputs)
if (verbose):
self.printSys(ssOriginal, 1)
# Make sure the system is not degenerate
Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (verbose):
print(" skipping (not reachable)")
continue
Omat = control.obsv(ssOriginal.A, ssOriginal.C)
if (np.linalg.matrix_rank(Omat) != states):
if (verbose):
print(" skipping (not observable)")
continue
tfOriginal = matlab.tf(ssOriginal)
if (verbose):
self.printSys(tfOriginal, 2)
ssTransformed = matlab.ss(tfOriginal)
if (verbose):
self.printSys(ssTransformed, 3)
tfTransformed = matlab.tf(ssTransformed)
if (verbose):
self.printSys(tfTransformed, 4)
# Check to see if the state space systems have same dim
if (ssOriginal.states != ssTransformed.states):
print("WARNING: state space dimension mismatch: " + \
"%d versus %d" % \
(ssOriginal.states, ssTransformed.states))
# Now make sure the frequency responses match
# Since bode() only handles SISO, go through each I/O pair
# For phase, take sine and cosine to avoid +/- 360 offset
for inputNum in range(inputs):
for outputNum in range(outputs):
if (verbose):
print("Checking input %d, output %d" \
% (inputNum, outputNum))
ssorig_mag, ssorig_phase, ssorig_omega = \
control.bode(_mimo2siso(ssOriginal, \
inputNum, outputNum), \
deg=False, Plot=False)
ssorig_real = ssorig_mag * np.cos(ssorig_phase)
ssorig_imag = ssorig_mag * np.sin(ssorig_phase)
#
# Make sure TF has same frequency response
#
num = tfOriginal.num[outputNum][inputNum]
den = tfOriginal.den[outputNum][inputNum]
tforig = control.tf(num, den)
tforig_mag, tforig_phase, tforig_omega = \
control.bode(tforig, ssorig_omega, \
deg=False, Plot=False)
tforig_real = tforig_mag * np.cos(tforig_phase)
tforig_imag = tforig_mag * np.sin(tforig_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tforig_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tforig_imag)
#
# Make sure xform'd SS has same frequency response
#
ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
control.bode(_mimo2siso(ssTransformed, \
inputNum, outputNum), \
ssorig_omega, \
deg=False, Plot=False)
ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, ssxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, ssxfrm_imag)
#
# Make sure xform'd TF has same frequency response
#
num = tfTransformed.num[outputNum][inputNum]
den = tfTransformed.den[outputNum][inputNum]
tfxfrm = control.tf(num, den)
tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
control.bode(tfxfrm, ssorig_omega, \
deg=False, Plot=False)
tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tfxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tfxfrm_imag)
def _load(self, fname='dslin.mat'):
"""Load a linearized Modelica_ model from *fname*.
See :meth:`__init__` for details about the file format.
Returns *None* if the file contains simulation results rather than
linearization results. Otherwise, it raises an error.
"""
# This performs the task of tloadlin.m from Dymola version 7.4:
# on Unix/Linux: /opt/dymola/mfiles/traj/tloadlin.m
# on Windows: C:\Program Files\Dymola 7.4\Mfiles\traj\tloadlin.m
# Load the file.
try:
dslin = loadmat(fname)
except IOError:
print('File "%s" could not be loaded. Check that it exists.' %
fname)
raise
# Check if the file contains the correct variable names.
assert 'Aclass' in dslin, ('There is no linear system in file "%s" '
'(matrix "Aclass" is missing).' % fname)
assert 'nx' in dslin, ('There is no linear system in file "%s" '
'(matrix "nx" is missing).' % fname)
assert 'xuyName' in dslin, ('There is no linear system in file "%s" '
'(matrix "xuyName" is missing).' % fname)
assert 'ABCD' in dslin, ('There is no linear system in file "%s" '
'(matrix "ABCD" is missing).' % fname)
# Check if the file has the correct class name.
if not dslin['Aclass'][0].startswith('AlinearSystem'):
if dslin['Aclass'][0].startswith('Atrajectory'):
return None # The file contains simulation results.
raise AssertionError('File "%s" is not of class AlinearSystem or '
'Atrajectory.' % fname)
# Extract variables from the dictionary (for convenience).
ABCD = dslin['ABCD']
xuyName = dslin['xuyName']
# Check if the matrices have compatible dimensions.
n_x = dslin['nx'][0]
dim1 = ABCD.shape[0]
dim2 = ABCD.shape[1]
assert n_x <= dim1 and n_x <= dim2, (
'nx > number of rows/columns of matrix ABCD in file "%s"' % fname)
n_u = dim2 - n_x
n_y = dim1 - n_x
assert n_x > 0 and n_y > 0, ("As of version 0.4b, the control module "
"cannot accept systems with empty matrixes.")
# Extract the matrices.
if n_x > 0:
A = ABCD[:n_x, :n_x]
if n_u > 0:
B = ABCD[:n_x, n_x:]
else:
B = []
if n_y > 0:
C = ABCD[n_x:, :n_x]
else:
C = []
else:
A = []
B = []
C = []
if n_u > 0 and n_y > 0:
D = ABCD[n_x:, n_x:]
else:
D = []
self.sys = ss(A, B, C, D)
# Extract the names.
if n_x > 0: # States
self.sys.stateName = [name.rstrip() for name in xuyName[:n_x]]
else:
self.sys.stateName = []
if n_u > 0: # Inputs
self.sys.inputName = [name.rstrip() for name in xuyName[n_x:
n_x+n_u]]
else:
self.sys.inputName = []
if n_y > 0: # Outputs
self.sys.outputName = [name.rstrip() for name in xuyName[n_x+n_u:]]
else:
self.sys.outputName = []
def bode(self, axes=None, pairs=None, label='bode',
title=None, colors=['b', 'g', 'r', 'c', 'm', 'y', 'k'],
styles=[(None,None), (3,3), (1,1), (3,2,1,2)], **kwargs):
"""Create a Bode plot of the system's response.
The Bode plots of a MIMO system are overlayed. This is different than
MATLAB\ :sup:`®`, which creates an array of subplots.
**Arguments:**
- *axes*: Tuple (pair) of axes for the magnitude and phase plots
If *axes* is not provided, then axes will be created in a new
figure.
- *pairs*: List of (input index, output index) tuples of each transfer
function to be evaluated
If not provided, all of the transfer functions will be plotted.
- *label*: Label for the figure (ignored if *axes* is provided)
This will be used as the base filename if the figure is saved.
- *title*: Title for the figure
If *title* is *None* (default), then the title will be "Bode Plot
of *fbase*", where *fbase* is the base filename of the data. Use
'' for no title.
- *colors*: Color or list of colors that will be used sequentially
Each may be a character, grayscale, or rgb value.
.. Seealso:: http://matplotlib.sourceforge.net/api/colors_api.html
- *styles*: Line/dash style or list of line/dash styles that will be
used sequentially
Each style is a string representing a linestyle (e.g., "--") or a
tuple of on/off lengths representing dashes. Use "" for no line
and "-" for a solid line.
.. Seealso::
http://matplotlib.sourceforge.net/api/collections_api.html
- *\*\*kwargs*: Additional arguments for :meth:`control.freqplot.bode`
**Returns:**
1. *axes*: Tuple (pair) of axes for the magnitude and phase plots
**Example:**
.. code-block:: python
>>> from modelicares import LinRes, save
>>> from numpy import pi, logspace
>>> lin = LinRes('examples/PID.mat')
>>> lin.bode(label='examples/PID-bode', omega=2*pi*logspace(-2, 3),
... title="Bode Plot of Modelica.Blocks.Continuous.PID") # doctest: +ELLIPSIS
(<matplotlib.axes._subplots.AxesSubplot object at 0x...>, <matplotlib.axes._subplots.AxesSubplot object at 0x...>)
>>> save()
Saved examples/PID-bode.pdf
Saved examples/PID-bode.png
.. only:: html
.. image:: ../examples/PID-bode.png
:scale: 70 %
:alt: example for LinRes.bode()
.. only:: latex
.. figure:: ../examples/PID-bode.pdf
:scale: 80 %
Results of example for :meth:`LinRes.bode`.
"""
# Create axes if necessary.
if axes is None or (None, None):
fig = base.figure(label)
axes = (fig.add_subplot(211), fig.add_subplot(212))
# Create a title if necessary.
if title is None:
title = r"Bode Plot of %s" % self.fbase
# Set up the color(s) and line style(s).
if not iterable(colors):
# Use the single color for all plots.
colors = (colors,)
if not iterable(styles):
# Use the single line style for all plots.
styles = [styles]
elif type(styles[0]) is int:
# One dashes tuple has been provided; use its value for all plots.
styles = [styles]
n_colors = len(colors)
n_styles = len(styles)
# If input/output pair(s) aren't specified, generate a list of all
# pairs.
if not pairs:
pairs = [(i_u, i_y) for i_u in range(self.sys.inputs)
for i_y in range(self.sys.outputs)]
# Create the plots.
for i, (i_u, i_y) in enumerate(pairs):
# Extract the SISO TF. TODO: Is there a better way to do this?
sys = ss(self.sys.A, self.sys.B[:, i_u], self.sys.C[i_y, :],
self.sys.D[i_y, i_u])
bode(sys, Hz=True, label=r'$Y_{%i}/U_{%i}$' % (i_y, i_u),
color=colors[np.mod(i, n_colors)], axes=axes,
style=styles[np.mod(i, n_styles)], **kwargs)
# Note: controls.freqplot.bode() is currently only implemented for
# SISO systems.
# 5/23/11: Since controls.freqplot.bode() already uses subplots for
# the magnitude and phase plots, it would be difficult to modify
# the code to put the Bode plots of a MIMO system into an array of
# subfigures like MATLAB does.
# Decorate and finish.
axes[0].set_title(title)
if len(pairs) > 1:
axes[0].legend()
axes[1].legend()
return axes
def nyquist(self, ax=None, pairs=None, label="nyquist", title=None,
xlabel="Real Axis", ylabel="Imaginary Axis",
colors=['b','g','r','c','m','y','k'], **kwargs):
"""Create a Nyquist plot of the system's response.
The Nyquist plots of a MIMO system are overlayed. This is different
than MATLAB\ :sup:`®`, which creates an array of subplots.
**Arguments:**
- *ax*: Axes onto which the Nyquist diagram should be plotted
If *ax* is not provided, then axes will be created in a new figure.
- *pairs*: List of (input index, output index) tuples of each transfer
function to be evaluated
If not provided, all of the transfer functions will be plotted.
- *label*: Label for the figure (ignored if ax is provided)
This will be used as the base filename if the figure is saved.
- *title*: Title for the figure
If *title* is *None* (default), then the title will be "Nyquist
Plot of *fbase*", where *fbase* is the base filename of the data.
Use '' for no title.
- *xlabel*: x-axis label
- *ylabel*: y-axis label
- *colors*: Color or list of colors that will be used sequentially
Each may be a character, grayscale, or rgb value.
.. Seealso:: http://matplotlib.sourceforge.net/api/colors_api.html
- *\*\*kwargs*: Additional arguments for
:meth:`control.freqplot.nyquist`
**Returns:**
1. *ax*: Axes of the Nyquist plot
**Example:**
.. testsetup::
>>> from modelicares import closeall
>>> closeall()
.. code-block:: python
>>> from modelicares import LinRes, save
>>> from numpy import pi, logspace
>>> lin = LinRes('examples/PID.mat')
>>> lin.nyquist(label='examples/PID-nyquist',
... omega=2*pi*logspace(0, 3, 61), labelFreq=20,
... title="Nyquist Plot of Modelica.Blocks.Continuous.PID") # doctest: +ELLIPSIS
<matplotlib.axes._subplots.AxesSubplot object at 0x...>
>>> save()
Saved examples/PID-nyquist.pdf
Saved examples/PID-nyquist.png
.. only:: html
.. image:: ../examples/PID-nyquist.png
:scale: 70 %
:alt: example for LinRes.nyquist()
.. only:: latex
.. figure:: ../examples/PID-nyquist.pdf
:scale: 70 %
Results of example for :meth:`LinRes.nyquist`.
"""
# Create axes if necessary.
if not ax:
fig = base.figure(label)
ax = fig.add_subplot(111, aspect='equal')
# Create a title if necessary.
if title is None:
title = r"Nyquist Plot of %s" % self.fbase
# Set up the color(s).
if not iterable(colors):
# Use the single color for all plots.
colors = (colors,)
n_colors = len(colors)
# If input/output pair(s) aren't specified, generate a list of all
# pairs.
if not pairs:
pairs = [(i_u, i_y) for i_u in range(self.sys.inputs)
for i_y in range(self.sys.outputs)]
# Create the plots.
for i, (i_u, i_y) in enumerate(pairs):
# Extract the SISO TF. TODO: Is there a better way to do this?
sys = ss(self.sys.A, self.sys.B[:, i_u], self.sys.C[i_y, :],
self.sys.D[i_y, i_u])
nyquist(sys, mark=False, label=r'$Y_{%i}/U_{%i}$' % (i_y, i_u),
color=colors[np.mod(i, n_colors)], ax=ax, **kwargs)
# Note: controls.freqplot.nyquist() is currently only implemented
# for SISO systems.
# Decorate and finish.
if len(pairs) > 1:
ax.legend()
base.add_hlines(ax, color='k', linestyle='--', linewidth=0.5)
base.add_vlines(ax, color='k', linestyle='--', linewidth=0.5)
ax.set_title(title)
if xlabel: # Without this check, xlabel=None will give a label of "None".
ax.set_xlabel(xlabel)
if ylabel: # Same purpose
ax.set_ylabel(ylabel)
return ax
def test_convert_MIMO_to_SISO(self):
'''Convert mimo to siso systems'''
#Test with our usual systems --------------------------------------------
#SISO PT2 system
As, Bs, Cs, Ds = self.make_SISO_mats()
sys_siso = ss(As, Bs, Cs, Ds)
#MIMO system that contains two independent copies of the SISO system above
Am, Bm, Cm, Dm = self.make_MIMO_mats()
sys_mimo = ss(Am, Bm, Cm, Dm)
# t, y = step(sys_siso)
# plot(t, y, label='sys_siso d=0')
sys_siso_00 = _mimo2siso(sys_mimo, input=0, output=0,
warn_conversion=False)
sys_siso_11 = _mimo2siso(sys_mimo, input=1, output=1,
warn_conversion=False)
#print("sys_siso_00 ---------------------------------------------")
#print(sys_siso_00)
#print("sys_siso_11 ---------------------------------------------")
#print(sys_siso_11)
#gain of converted system and equivalent SISO system must be the same
self.assert_systems_behave_equal(sys_siso, sys_siso_00)
self.assert_systems_behave_equal(sys_siso, sys_siso_11)
#Test with additional systems --------------------------------------------
#They have crossed inputs and direct feedthrough
#SISO system
As = matrix([[-81.82, -45.45],
[ 10., -1. ]])
Bs = matrix([[9.09],
[0. ]])
Cs = matrix([[0, 0.159]])
Ds = matrix([[0.02]])
sys_siso = ss(As, Bs, Cs, Ds)
# t, y = step(sys_siso)
# plot(t, y, label='sys_siso d=0.02')
# legend(loc='best')
#MIMO system
#The upper left sub-system uses : input 0, output 1
#The lower right sub-system uses: input 1, output 0
Am = array([[-81.82, -45.45, 0, 0 ],
[ 10, -1, 0, 0 ],
[ 0, 0, -81.82, -45.45],
[ 0, 0, 10, -1, ]])
Bm = array([[9.09, 0 ],
[0 , 0 ],
[0 , 9.09],
[0 , 0 ]])
Cm = array([[0, 0, 0, 0.159],
[0, 0.159, 0, 0 ]])
Dm = matrix([[0, 0.02],
[0.02, 0 ]])
sys_mimo = ss(Am, Bm, Cm, Dm)
sys_siso_01 = _mimo2siso(sys_mimo, input=0, output=1,
warn_conversion=False)
sys_siso_10 = _mimo2siso(sys_mimo, input=1, output=0,
warn_conversion=False)
print("sys_siso_01 ---------------------------------------------")
print(sys_siso_01)
print("sys_siso_10 ---------------------------------------------")
print(sys_siso_10)
#gain of converted system and equivalent SISO system must be the same
self.assert_systems_behave_equal(sys_siso, sys_siso_01)
self.assert_systems_behave_equal(sys_siso, sys_siso_10)
def test_lsim(self):
A, B, C, D = self.make_SISO_mats()
sys = ss(A, B, C, D)
figure(); plot_shape = (2, 2)
#Test with arrays
subplot2grid(plot_shape, (0, 0))
t = linspace(0, 1, 100)
u = r_[1:1:50j, 0:0:50j]
y, _t, _x = lsim(sys, u, t)
plot(t, y, label='y')
plot(t, u/10, label='u/10')
legend(loc='best')
#Test with U=None - uses 2nd algorithm which is much faster.
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
x0 = [-1, -1]
y, _t, _x = lsim(sys, U=None, T=t, X0=x0)
plot(t, y, label='y')
legend(loc='best')
#Test with U=0, X0=0
#Correct reaction to zero dimensional special values
subplot2grid(plot_shape, (0, 1))
t = linspace(0, 1, 100)
y, _t, _x = lsim(sys, U=0, T=t, X0=0)
plot(t, y, label='y')
legend(loc='best')
#Test with matrices
subplot2grid(plot_shape, (1, 0))
t = matrix(linspace(0, 1, 100))
u = matrix(r_[1:1:50j, 0:0:50j])
x0 = matrix("0.; 0")
y, t_out, _x = lsim(sys, u, t, x0)
plot(t_out, y, label='y')
plot(t_out, asarray(u/10)[0], label='u/10')
legend(loc='best')
#Test with MIMO system
subplot2grid(plot_shape, (1, 1))
A, B, C, D = self.make_MIMO_mats()
sys = ss(A, B, C, D)
t = matrix(linspace(0, 1, 100))
u = array([r_[1:1:50j, 0:0:50j],
r_[0:1:50j, 0:0:50j]])
x0 = [0, 0, 0, 0]
y, t_out, _x = lsim(sys, u, t, x0)
plot(t_out, y[0], label='y[0]')
plot(t_out, y[1], label='y[1]')
plot(t_out, u[0]/10, label='u[0]/10')
plot(t_out, u[1]/10, label='u[1]/10')
legend(loc='best')
#Test with wrong values for t
#T is None; - special handling: Value error
self.assertRaises(ValueError, lsim(sys, U=0, T=None, x0=0))
#T="hello" : Wrong type
#TODO: better wording of error messages of ``lsim`` and
# ``_check_convert_array``, when wrong type is given.
# Current error message is too cryptic.
self.assertRaises(TypeError, lsim(sys, U=0, T="hello", x0=0))
#T=0; - T can not be zero dimensional, it determines the size of the
# input vector ``U``
self.assertRaises(ValueError, lsim(sys, U=0, T=0, x0=0))
#T is not monotonically increasing
self.assertRaises(ValueError, lsim(sys, U=0, T=[0., 1., 2., 2., 3.], x0=0))
