#!/usr/bin/env python # # matlab_test.py - test MATLAB compatibility # RMM, 30 Mar 2011 (based on TestMatlab from v0.4a) # # This test suite just goes through and calls all of the MATLAB # functions using different systems and arguments to make sure that # nothing crashes. It doesn't test actual functionality; the module # specific unit tests will do that. from __future__ import print_function import unittest import numpy as np from scipy.linalg import eigvals import scipy as sp from control.matlab import * from control.frdata import FRD from control.exception import slycot_check import warnings # for running these through Matlab or Octave ''' siso_ss1 = ss([1. -2.; 3. -4.], [5.; 7.], [6. 8.], [0]) siso_tf1 = tf([1], [1, 2, 1]) siso_tf2 = tf([1, 1], [1, 2, 3, 1]) siso_tf3 = tf(siso_ss1) siso_ss2 = ss(siso_tf2) siso_ss3 = ss(siso_tf3) siso_tf4 = tf(siso_ss2) A =[ 1. -2. 0. 0.; 3. -4. 0. 0.; 0. 0. 1. -2.; 0. 0. 3. -4. ] B = [ 5. 0.; 7. 0.; 0. 5.; 0. 7. ] C = [ 6. 8. 0. 0.; 0. 0. 6. 8. ] D = [ 9. 0.; 0. 9. ] mimo_ss1 = ss(A, B, C, D) % all boring, since no cross-over margin(siso_tf1) margin(siso_tf2) margin(siso_ss1) margin(siso_ss2) % make a bit better [gm, pm, gmc, pmc] = margin(siso_ss2*siso_ss2*2) ''' class TestMatlab(unittest.TestCase): def setUp(self): """Set up some systems for testing out MATLAB functions""" A = np.matrix("1. -2.; 3. -4.") B = np.matrix("5.; 7.") C = np.matrix("6. 8.") D = np.matrix("9.") self.siso_ss1 = ss(A,B,C,D) # Create some transfer functions self.siso_tf1 = tf([1], [1, 2, 1]); self.siso_tf2 = tf([1, 1], [1, 2, 3, 1]); # Conversions self.siso_tf3 = tf(self.siso_ss1); self.siso_ss2 = ss(self.siso_tf2); self.siso_ss3 = tf2ss(self.siso_tf3); self.siso_tf4 = ss2tf(self.siso_ss2); #Create MIMO system, contains ``siso_ss1`` twice A = np.matrix("1. -2. 0. 0.;" "3. -4. 0. 0.;" "0. 0. 1. -2.;" "0. 0. 3. -4. ") B = np.matrix("5. 0.;" "7. 0.;" "0. 5.;" "0. 7. ") C = np.matrix("6. 8. 0. 0.;" "0. 0. 6. 8. ") D = np.matrix("9. 0.;" "0. 9. ") self.mimo_ss1 = ss(A, B, C, D) # get consistent test results np.random.seed(0) def testParallel(self): sys1 = parallel(self.siso_ss1, self.siso_ss2) sys1 = parallel(self.siso_ss1, self.siso_tf2) sys1 = parallel(self.siso_tf1, self.siso_ss2) sys1 = parallel(1, self.siso_ss2) sys1 = parallel(1, self.siso_tf2) sys1 = parallel(self.siso_ss1, 1) sys1 = parallel(self.siso_tf1, 1) def testSeries(self): sys1 = series(self.siso_ss1, self.siso_ss2) sys1 = series(self.siso_ss1, self.siso_tf2) sys1 = series(self.siso_tf1, self.siso_ss2) sys1 = series(1, self.siso_ss2) sys1 = series(1, self.siso_tf2) sys1 = series(self.siso_ss1, 1) sys1 = series(self.siso_tf1, 1) def testFeedback(self): sys1 = feedback(self.siso_ss1, self.siso_ss2) sys1 = feedback(self.siso_ss1, self.siso_tf2) sys1 = feedback(self.siso_tf1, self.siso_ss2) sys1 = feedback(1, self.siso_ss2) sys1 = feedback(1, self.siso_tf2) sys1 = feedback(self.siso_ss1, 1) sys1 = feedback(self.siso_tf1, 1) def testPoleZero(self): pole(self.siso_ss1); pole(self.siso_tf1); pole(self.siso_tf2); zero(self.siso_ss1); zero(self.siso_tf1); zero(self.siso_tf2); def testPZmap(self): # pzmap(self.siso_ss1); not implemented # pzmap(self.siso_ss2); not implemented pzmap(self.siso_tf1); pzmap(self.siso_tf2); pzmap(self.siso_tf2, Plot=False); def testStep(self): t = np.linspace(0, 1, 10) # Test transfer function yout, tout = step(self.siso_tf1, T=t) youttrue = np.array([0, 0.0057, 0.0213, 0.0446, 0.0739, 0.1075, 0.1443, 0.1832, 0.2235, 0.2642]) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) # Test SISO system with direct feedthrough sys = self.siso_ss1 youttrue = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234, 39.1165, 42.3227, 44.9694, 47.1599, 48.9776]) yout, tout = step(sys, T=t) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) # Play with arguments yout, tout = step(sys, T=t, X0=0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) X0 = np.array([0, 0]); yout, tout = step(sys, T=t, X0=X0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) yout, tout, xout = step(sys, T=t, X0=0, return_x=True) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) if slycot_check(): # Test MIMO system, which contains ``siso_ss1`` twice sys = self.mimo_ss1 y_00, _t = step(sys, T=t, input=0, output=0) y_11, _t = step(sys, T=t, input=1, output=1) np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4) np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4) def testImpulse(self): t = np.linspace(0, 1, 10) # test transfer function yout, tout = impulse(self.siso_tf1, T=t) youttrue = np.array([0., 0.0994, 0.1779, 0.2388, 0.2850, 0.3188, 0.3423, 0.3573, 0.3654, 0.3679]) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) # produce a warning for a system with direct feedthrough with warnings.catch_warnings(): warnings.simplefilter("ignore") #Test SISO system sys = self.siso_ss1 youttrue = np.array([86., 70.1808, 57.3753, 46.9975, 38.5766, 31.7344, 26.1668, 21.6292, 17.9245, 14.8945]) yout, tout = impulse(sys, T=t) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) # Play with arguments yout, tout = impulse(sys, T=t, X0=0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) X0 = np.array([0, 0]); yout, tout = impulse(sys, T=t, X0=X0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) yout, tout, xout = impulse(sys, T=t, X0=0, return_x=True) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) if slycot_check(): #Test MIMO system, which contains ``siso_ss1`` twice sys = self.mimo_ss1 y_00, _t = impulse(sys, T=t, input=0, output=0) y_11, _t = impulse(sys, T=t, input=1, output=1) np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4) np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4) def testInitial(self): #Test SISO system sys = self.siso_ss1 t = np.linspace(0, 1, 10) x0 = np.matrix(".5; 1.") youttrue = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092, 1.1508, 0.5833, 0.1645, -0.1391]) yout, tout = initial(sys, T=t, X0=x0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) # Play with arguments yout, tout, xout = initial(sys, T=t, X0=x0, return_x=True) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) if slycot_check(): #Test MIMO system, which contains ``siso_ss1`` twice sys = self.mimo_ss1 x0 = np.matrix(".5; 1.; .5; 1.") y_00, _t = initial(sys, T=t, X0=x0, input=0, output=0) y_11, _t = initial(sys, T=t, X0=x0, input=1, output=1) np.testing.assert_array_almost_equal(y_00, youttrue, decimal=4) np.testing.assert_array_almost_equal(y_11, youttrue, decimal=4) def testLsim(self): t = np.linspace(0, 1, 10) #compute step response - test with state space, and transfer function #objects u = np.array([1., 1, 1, 1, 1, 1, 1, 1, 1, 1]) youttrue = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234, 39.1165, 42.3227, 44.9694, 47.1599, 48.9776]) yout, tout, _xout = lsim(self.siso_ss1, u, t) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) np.testing.assert_array_almost_equal(tout, t) yout, _t, _xout = lsim(self.siso_tf3, u, t) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) #test with initial value and special algorithm for ``U=0`` u=0 x0 = np.matrix(".5; 1.") youttrue = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092, 1.1508, 0.5833, 0.1645, -0.1391]) yout, _t, _xout = lsim(self.siso_ss1, u, t, x0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) if slycot_check(): #Test MIMO system, which contains ``siso_ss1`` twice #first system: initial value, second system: step response u = np.array([[0., 1.], [0, 1], [0, 1], [0, 1], [0, 1], [0, 1], [0, 1], [0, 1], [0, 1], [0, 1]]) x0 = np.matrix(".5; 1; 0; 0") youttrue = np.array([[11., 9.], [8.1494, 17.6457], [5.9361, 24.7072], [4.2258, 30.4855], [2.9118, 35.2234], [1.9092, 39.1165], [1.1508, 42.3227], [0.5833, 44.9694], [0.1645, 47.1599], [-0.1391, 48.9776]]) yout, _t, _xout = lsim(self.mimo_ss1, u, t, x0) np.testing.assert_array_almost_equal(yout, youttrue, decimal=4) def testMargin(self): #! TODO: check results to make sure they are OK gm, pm, wg, wp = margin(self.siso_tf1); gm, pm, wg, wp = margin(self.siso_tf2); gm, pm, wg, wp = margin(self.siso_ss1); gm, pm, wg, wp = margin(self.siso_ss2); gm, pm, wg, wp = margin(self.siso_ss2*self.siso_ss2*2); np.testing.assert_array_almost_equal( [gm, pm, wg, wp], [1.5451, 75.9933, 1.2720, 0.6559], decimal=3) def testDcgain(self): #Create different forms of a SISO system A, B, C, D = self.siso_ss1.A, self.siso_ss1.B, self.siso_ss1.C, \ self.siso_ss1.D Z, P, k = sp.signal.ss2zpk(A, B, C, D) num, den = sp.signal.ss2tf(A, B, C, D) sys_ss = self.siso_ss1 #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('\ngain_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 np.testing.assert_array_almost_equal( [gain_abcd, gain_zpk, gain_numden, gain_sys_ss, gain_sim], [59, 59, 59, 59, 59]) if slycot_check(): # Test with MIMO system, which contains ``siso_ss1`` twice gain_mimo = dcgain(self.mimo_ss1) # print('gain_mimo: \n', gain_mimo) np.testing.assert_array_almost_equal(gain_mimo, [[59., 0 ], [0, 59.]]) def testBode(self): bode(self.siso_ss1) bode(self.siso_tf1) bode(self.siso_tf2) (mag, phase, freq) = bode(self.siso_tf2, Plot=False) bode(self.siso_tf1, self.siso_tf2) w = logspace(-3, 3); bode(self.siso_ss1, w) bode(self.siso_ss1, self.siso_tf2, w) # Not yet implemented # bode(self.siso_ss1, '-', self.siso_tf1, 'b--', self.siso_tf2, 'k.') def testRlocus(self): rlocus(self.siso_ss1) rlocus(self.siso_tf1) rlocus(self.siso_tf2) klist = [1, 10, 100] rlist, klist_out = rlocus(self.siso_tf2, klist, Plot=False) np.testing.assert_equal(len(rlist), len(klist)) np.testing.assert_array_equal(klist, klist_out) def testNyquist(self): nyquist(self.siso_ss1) nyquist(self.siso_tf1) nyquist(self.siso_tf2) w = logspace(-3, 3); nyquist(self.siso_tf2, w) (real, imag, freq) = nyquist(self.siso_tf2, w, Plot=False) def testNichols(self): nichols(self.siso_ss1) nichols(self.siso_tf1) nichols(self.siso_tf2) w = logspace(-3, 3); nichols(self.siso_tf2, w) nichols(self.siso_tf2, grid=False) def testFreqresp(self): w = logspace(-3, 3) freqresp(self.siso_ss1, w) freqresp(self.siso_ss2, w) freqresp(self.siso_ss3, w) freqresp(self.siso_tf1, w) freqresp(self.siso_tf2, w) freqresp(self.siso_tf3, w) def testEvalfr(self): w = 1j np.testing.assert_almost_equal(evalfr(self.siso_ss1, w), 44.8-21.4j) evalfr(self.siso_ss2, w) evalfr(self.siso_ss3, w) evalfr(self.siso_tf1, w) evalfr(self.siso_tf2, w) evalfr(self.siso_tf3, w) if slycot_check(): np.testing.assert_array_almost_equal( evalfr(self.mimo_ss1, w), np.array( [[44.8-21.4j, 0.], [0., 44.8-21.4j]])) @unittest.skipIf(not slycot_check(), "slycot not installed") def testHsvd(self): hsvd(self.siso_ss1) hsvd(self.siso_ss2) hsvd(self.siso_ss3) @unittest.skipIf(not slycot_check(), "slycot not installed") def testBalred(self): balred(self.siso_ss1, 1) balred(self.siso_ss2, 2) balred(self.siso_ss3, [2, 2]) @unittest.skipIf(not slycot_check(), "slycot not installed") def testModred(self): modred(self.siso_ss1, [1]) modred(self.siso_ss2 * self.siso_ss1, [0, 1]) modred(self.siso_ss1, [1], 'matchdc') modred(self.siso_ss1, [1], 'truncate') @unittest.skipIf(not slycot_check(), "slycot not installed") def testPlace_varga(self): place_varga(self.siso_ss1.A, self.siso_ss1.B, [-2, -2]) def testPlace(self): place(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5]) def testAcker(self): acker(self.siso_ss1.A, self.siso_ss1.B, [-2, -2.5]) @unittest.skipIf(not slycot_check(), "slycot not installed") def testLQR(self): (K, S, E) = lqr(self.siso_ss1.A, self.siso_ss1.B, np.eye(2), np.eye(1)) # Should work if [Q N;N' R] is positive semi-definite (K, S, E) = lqr(self.siso_ss2.A, self.siso_ss2.B, 10*np.eye(3), \ np.eye(1), [[1], [1], [2]]) @unittest.skip("check not yet implemented") def testLQR_checks(self): # Make sure we get a warning if [Q N;N' R] is not positive semi-definite (K, S, E) = lqr(self.siso_ss2.A, self.siso_ss2.B, np.eye(3), \ np.eye(1), [[1], [1], [2]]) def testRss(self): rss(1) rss(2) rss(2, 1, 3) def testDrss(self): drss(1) drss(2) drss(2, 1, 3) def testCtrb(self): ctrb(self.siso_ss1.A, self.siso_ss1.B) ctrb(self.siso_ss2.A, self.siso_ss2.B) def testObsv(self): obsv(self.siso_ss1.A, self.siso_ss1.C) obsv(self.siso_ss2.A, self.siso_ss2.C) @unittest.skipIf(not slycot_check(), "slycot not installed") def testGram(self): gram(self.siso_ss1, 'c') gram(self.siso_ss2, 'c') gram(self.siso_ss1, 'o') gram(self.siso_ss2, 'o') def testPade(self): pade(1, 1) pade(1, 2) pade(5, 4) def testOpers(self): self.siso_ss1 + self.siso_ss2 self.siso_tf1 + self.siso_tf2 self.siso_ss1 + self.siso_tf2 self.siso_tf1 + self.siso_ss2 self.siso_ss1 * self.siso_ss2 self.siso_tf1 * self.siso_tf2 self.siso_ss1 * self.siso_tf2 self.siso_tf1 * self.siso_ss2 # self.siso_ss1 / self.siso_ss2 not implemented yet # self.siso_tf1 / self.siso_tf2 # self.siso_ss1 / self.siso_tf2 # self.siso_tf1 / self.siso_ss2 def testUnwrap(self): phase = np.array(range(1, 100)) / 10.; wrapped = phase % (2 * np.pi) unwrapped = unwrap(wrapped) def testSISOssdata(self): ssdata_1 = ssdata(self.siso_ss2); ssdata_2 = ssdata(self.siso_tf2); for i in range(len(ssdata_1)): np.testing.assert_array_almost_equal(ssdata_1[i], ssdata_2[i]) @unittest.skipIf(not slycot_check(), "slycot not installed") def testMIMOssdata(self): m = (self.mimo_ss1.A, self.mimo_ss1.B, self.mimo_ss1.C, self.mimo_ss1.D) ssdata_1 = ssdata(self.mimo_ss1); for i in range(len(ssdata_1)): np.testing.assert_array_almost_equal(ssdata_1[i], m[i]) def testSISOtfdata(self): tfdata_1 = tfdata(self.siso_tf2); tfdata_2 = tfdata(self.siso_tf2); for i in range(len(tfdata_1)): np.testing.assert_array_almost_equal(tfdata_1[i], tfdata_2[i]) def testDamp(self): A = np.mat('''-0.2 0.06 0 -1; 0 0 1 0; -17 0 -3.8 1; 9.4 0 -0.4 -0.6''') B = np.mat('''-0.01 0.06; 0 0; -32 5.4; 2.6 -7''') C = np.eye(4) D = np.zeros((4,2)) sys = ss(A, B, C, D) wn, Z, p = damp(sys, False) # print (wn) np.testing.assert_array_almost_equal( wn, np.array([4.07381994, 3.28874827, 3.28874827, 1.08937685e-03])) np.testing.assert_array_almost_equal( Z, np.array([1.0, 0.07983139, 0.07983139, 1.0])) def testConnect(self): sys1 = ss("1. -2; 3. -4", "5.; 7", "6, 8", "9.") sys2 = ss("-1.", "1.", "1.", "0.") sys = append(sys1, sys2) Q= np.mat([ [ 1, 2], [2, -1] ]) # basically feedback, output 2 in 1 sysc = connect(sys, Q, [2], [1, 2]) # print(sysc) np.testing.assert_array_almost_equal( sysc.A, np.mat('1 -2 5; 3 -4 7; -6 -8 -10')) np.testing.assert_array_almost_equal( sysc.B, np.mat('0; 0; 1')) np.testing.assert_array_almost_equal( sysc.C, np.mat('6 8 9; 0 0 1')) np.testing.assert_array_almost_equal( sysc.D, np.mat('0; 0')) def testConnect2(self): 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.mat([[-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.mat([[2], [0], [0], [0]])) np.testing.assert_array_almost_equal( sysc.C, np.mat([[0, 0, 0, -3.3333], [0, 1.125, 0, 0], [0, 0, 0, 3.3333]])) np.testing.assert_array_almost_equal( sysc.D, np.mat([[1], [0], [0]])) def testFRD(self): h = tf([1], [1, 2, 2]) omega = np.logspace(-1, 2, 10) frd1 = frd(h, omega) assert isinstance(frd1, FRD) frd2 = frd(frd1.fresp[0,0,:], omega) assert isinstance(frd2, FRD) @unittest.skipIf(not slycot_check(), "slycot not installed") 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) self.assertEqual(sysr.states, 2) self.assertEqual(sysr.inputs, sys.inputs) self.assertEqual(sysr.outputs, sys.outputs) 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 testSS2cont(self): sys = ss( np.mat("-3 4 2; -1 -3 0; 2 5 3"), np.mat("1 4 ; -3 -3; -2 1"), np.mat("4 2 -3; 1 4 3"), np.mat("-2 4; 0 1")) sysd = c2d(sys, 0.1) np.testing.assert_array_almost_equal( np.mat( """0.742840837331905 0.342242024293711 0.203124211149560; -0.074130792143890 0.724553295044645 -0.009143771143630; 0.180264783290485 0.544385612448419 1.370501013067845"""), sysd.A) np.testing.assert_array_almost_equal( np.mat(""" 0.012362066084719 0.301932197918268; -0.260952977031384 -0.274201791021713; -0.304617775734327 0.075182622718853"""), sysd.B) 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(sp.matrix([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)) self.assertAlmostEqual(gm, 3.32065569155) self.assertAlmostEqual(pm, 46.9740430224) self.assertAlmostEqual(wg, 0.176469728448) self.assertAlmostEqual(wp, 0.0616288455466) def test_tf_string_args(self): # Make sure that the 's' variable is defined properly s = tf('s') G = (s + 1)/(s**2 + 2*s + 1) np.testing.assert_array_almost_equal(G.num, [[[1, 1]]]) np.testing.assert_array_almost_equal(G.den, [[[1, 2, 1]]]) self.assertTrue(isctime(G, strict=True)) # Make sure that the 'z' variable is defined properly z = tf('z') G = (z + 1)/(z**2 + 2*z + 1) np.testing.assert_array_almost_equal(G.num, [[[1, 1]]]) np.testing.assert_array_almost_equal(G.den, [[[1, 2, 1]]]) self.assertTrue(isdtime(G, strict=True)) #! TODO: not yet implemented # def testMIMOtfdata(self): # sisotf = ss2tf(self.siso_ss1) # tfdata_1 = tfdata(sisotf) # tfdata_2 = tfdata(self.mimo_ss1, input=0, output=0) # for i in range(len(tfdata)): # np.testing.assert_array_almost_equal(tfdata_1[i], tfdata_2[i]) def test_suite(): return unittest.TestLoader().loadTestsFromTestCase(TestMatlab) if __name__ == '__main__': unittest.main()