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 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 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]]))
