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