def test_reduced_order_compensator(self): with warnings.catch_warnings(): warnings.simplefilter("ignore") A = np.array([[0., 1., 0., 0.], [0., -25., -0.98, 0.], [0., 0., 0., 1.], [0., 25., 10.78, 0.]]).astype('float') B = np.array([[0.], [0.5], [0.], [-0.5]]).reshape( (4, 1)).astype('float') C = np.array([[1., 0., 0., 0.]]).reshape((1, 4)).astype('float') D = np.zeros((1, 1)).astype('float') ### Controller ### desiredPolesCtrl = np.array([ -25., -4., np.complex(-2., 2. * np.sqrt(3)), np.complex(-2., -2. * np.sqrt(3)) ]) G = ch6_utils.bassGura(A, B, desiredPolesCtrl) G1 = G[0, 0].reshape((1, 1)) G2 = G[0, 1:].reshape((1, 3)) ### Observer ### desiredPolesObsv = np.roots( np.array([1.0, 2. * 5., 2. * 5.**2, 5.**3])) Aaa = np.array(A[0, 0]).reshape((1, 1)) Aau = np.array(A[0, 1:]).reshape((1, 3)) Aua = np.array(A[1:, 0]).reshape((3, 1)) Auu = np.array(A[1:, 1:]).reshape((3, 3)) Ba = np.array(B[0]).reshape((1, 1)) Bu = np.array(B[1:]).reshape((3, 1)) Ca = np.array([[1]]).reshape((1, 1)).astype('float') L, Gbb, H = ch7_utils.reducedOrderObserver(Aaa, Aau, Aua, Auu, Ba, Bu, Ca, desiredPolesObsv) ### Compensator ### F = Auu - L @ Aau Ar = F - H @ G2 Br = Ar @ L + Gbb - H @ G1 Cr = G2 Dr = G1 + G2 @ L sysD = control.StateSpace(Ar, Br, Cr, Dr) compensatorTF = control.ss2tf(sysD) sysplant = control.StateSpace(A, B, C, D) openLoopTF = control.ss2tf(sysplant) # poles are zeros of return difference poles = control.zero(1. + compensatorTF * openLoopTF) for p in poles: polePresent = any([ np.isclose(np.complex(p), np.complex(pt)) for pt in np.hstack([desiredPolesCtrl, desiredPolesObsv]) ]) self.assertTrue(polePresent)
def test_full_order_compensator(self): with warnings.catch_warnings(): warnings.simplefilter("ignore") A = np.array([[0., 1., 0., 0.], [0., -25., -0.98, 0.], [0., 0., 0., 1.], [0., 25., 10.78, 0.]]).astype('float') B = np.array([[0.], [0.5], [0.], [-0.5]]).astype('float') C = np.array([[1., 0., 0., 0.]]).astype('float') D = np.zeros((1, 1)).astype('float') desiredPolesCtrl = np.array([ -25., -4., np.complex(-2., 2. * np.sqrt(3)), np.complex(-2., -2. * np.sqrt(3)) ]) w0 = 5. desiredPolesObsv = np.roots( np.array([ 1.0, 2.613 * w0, (2. + np.sqrt(2)) * w0**2, 2.613 * w0**3, w0**4 ])) compensatorTF = fullOrderCompensator(A, B, C, D, desiredPolesCtrl, desiredPolesObsv) openLoopTF = control.ss2tf(control.StateSpace(A, B, C, D)) # poles are zeros of return difference poles = control.zero(1. + compensatorTF * openLoopTF) for p in poles: polePresent = any([ np.isclose(np.complex(p), np.complex(pt)) for pt in np.hstack([desiredPolesCtrl, desiredPolesObsv]) ]) self.assertTrue(polePresent)
def fullOrderCompensator(A, B, C, D, controlPoles, observerPoles): """ combine controller and observer constructions into a compensator design currently, supports single-input-single-output systems only Inputs: A (numpy matrix/array, type=real) - system state matrix B (numpy matrix/array, type=real) - system control matrix C (numpy matrix/array, type=real) - system measurement matrix desiredPoles (numpy matrix/array, type=complex) - desired pole locations D (numpy matrix/array, type=real) - direct path from control to measurement matrix E (numpy matrix/array, type=real) - (default=None) exogeneous input matrix controlPoles (numpy matrix/array, type=complex) - desired controller system poles observerPoles (numpy matrix/array, type=complex) - desired observer system poles Returns: (control.TransferFunction) - compensator transfer function from input to output Raises: TypeError - if input system is not SISO """ if B.shape[1] > 1 or C.shape[0] > 1: raise TypeError( 'Only single-input-single-output (SISO) systems are currently supported' ) A = A.astype('float') B = B.astype('float') C = C.astype('float') D = D.astype('float') G = ch6_utils.bassGura(A, B, controlPoles) K = ch7_utils.obsBassGura(A, C, observerPoles) return control.ss2tf(control.StateSpace(A - B @ G - K @ C, K, G, D))
def myfunc(variant: int): """Generate test systems. """ A = np.array((0, 0, 0, 1, 0, -0.9215, 0, 1, -0.738)).reshape((3, 3)) B = np.array((1 + 0.1 * variant, 0, 0)).reshape((3, 1)) C = np.array((0, 0.151, -0.6732)).reshape((1, 3)) D = np.zeros((1, 1)) sys1 = StateSpace(A, B, C, D) sys2 = tf2ss(ss2tf(sys1)) As, Z = schur(A) Bs = Z.T @ B Cs = C @ Z Ds = D # print(Bs) sys3 = StateSpace(As, Bs, Cs, Ds) Ds[0, 0] = 0.3 sys4 = StateSpace(As, Bs, Cs, Ds) Ds[0, 0] = 0 Ab = np.zeros((4, 4)) Ab[:3, :3] = A Bb = np.zeros((4, 1)) Bb[:3, :] = B Cb = np.zeros((1, 4)) Cb[:, :3] = C sys5 = StateSpace(Ab, Bb, Cb, D) # sys5.A = Ab # sys5.B = Bb # sys5.C = Cb return locals()
def mimo_tf2(self, siso_ss2, mimo_ss2): T = copy(mimo_ss2) # construct from siso to avoid slycot during fixture setup tf_ = ss2tf(siso_ss2.sys) T.sys = TransferFunction([[tf_.num[0][0], [0]], [[0], tf_.num[0][0]]], [[tf_.den[0][0], [1]], [[1], tf_.den[0][0]]], 0) return T
def test_ss_input_with_int_element(self): ident = np.matrix(np.identity(2), dtype=int) a = np.matrix([[0, 1], [-1, -2]], dtype=int) * ident b = np.matrix([[0], [1]], dtype=int) c = np.matrix([[0, 1]], dtype=int) d = 0 sys = ctl.ss(a, b, c, d) sys2 = ctl.ss2tf(sys) self.assertAlmostEqual(ctl.dcgain(sys), ctl.dcgain(sys2))
def test_printing(self): """Print SISO""" sys = ss2tf(rss(4, 1, 1)) assert isinstance(str(sys), str) assert isinstance(sys._repr_latex_(), str) # SISO, discrete time sys = sample_system(sys, 1) assert isinstance(str(sys), str) assert isinstance(sys._repr_latex_(), str)
def test_ss2tf(self): """Test SISO ss2tf""" A = np.array([[-4, -1], [-1, -4]]) B = np.array([[1], [3]]) C = np.array([[3, 1]]) D = 0 sys = ss2tf(A, B, C, D) true_sys = TransferFunction([6., 14.], [1., 8., 15.]) np.testing.assert_almost_equal(sys.num, true_sys.num) np.testing.assert_almost_equal(sys.den, true_sys.den)
def test_ss_input_with_int_element(self): ident = np.matrix(np.identity(2), dtype=int) a = np.matrix([[0, 1], [-1, -2]], dtype=int) * ident b = np.matrix([[0], [1]], dtype=int) c = np.matrix([[0, 1]], dtype=int) d = 0 sys = ctl.ss(a, b, c, d) sys2 = ctl.ss2tf(sys) self.assertAlmostEqual(ctl.dcgain(sys), ctl.dcgain(sys2))
def test_size_mismatch(self): """Test size mismacht""" sys1 = ss2tf(rss(2, 2, 2)) # Different number of inputs sys2 = ss2tf(rss(3, 1, 2)) with pytest.raises(ValueError): TransferFunction.__add__(sys1, sys2) # Different number of outputs sys2 = ss2tf(rss(3, 2, 1)) with pytest.raises(ValueError): TransferFunction.__add__(sys1, sys2) # Inputs and outputs don't match with pytest.raises(ValueError): TransferFunction.__mul__(sys2, sys1) # Feedback mismatch (MIMO not implemented) with pytest.raises(NotImplementedError): TransferFunction.feedback(sys2, sys1)
def plot_poles(trim_args={'h': 0, 'va': 12, 'gamma': 0}, ac_name='cularis'): dm = p1_fw_dyn.DynamicModel( os.path.join(p3_u.pat_dir(), 'data/vehicles/{}.xml'.format(ac_name))) Xe, Ue = dm.trim(trim_args, debug=True) A, B = dm.get_jacobian(Xe, Ue) _eval, _evel = np.linalg.eig(A) B1 = B[:, 2][np.newaxis].T C = np.array([0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0]) D = np.zeros((1, 1)) ss = control.ss(A, B1, C, D) tf = control.ss2tf(ss) pdb.set_trace()
def test_tf2io(self): # Create a transfer function from the state space system linsys = self.siso_linsys tfsys = ct.ss2tf(linsys) iosys = ct.tf2io(tfsys) # Verify correctness via simulation T, U, X0 = self.T, self.U, self.X0 lti_t, lti_y, lti_x = ct.forced_response(linsys, T, U, X0) ios_t, ios_y = ios.input_output_response(iosys, T, U, X0) np.testing.assert_array_almost_equal(lti_t, ios_t) np.testing.assert_array_almost_equal(lti_y, ios_y, decimal=3)
def testSs2tfStaticMimo(self): """Regression: ss2tf for MIMO static gain""" # 2x3 TFM a = [] b = [] c = [] d = np.array([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]]) gtf = ss2tf(ss(a, b, c, d)) # we need a 3x2x1 array to compare with gtf.num numref = d[..., np.newaxis] np.testing.assert_allclose(numref, np.array(gtf.num) / np.array(gtf.den))
def testSs2tfStaticMimo(self): """Regression: ss2tf for MIMO static gain""" import control # 2x3 TFM a = [] b = [] c = [] d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]]) gtf = control.ss2tf(control.ss(a,b,c,d)) # we need a 3x2x1 array to compare with gtf.num # np.testing.assert_array_equal doesn't seem to like a matrices # with an extra dimension, so convert to ndarray numref = np.asarray(d)[...,np.newaxis] np.testing.assert_array_equal(numref, np.array(gtf.num) / np.array(gtf.den))
def testSs2tfStaticMimo(self): """Regression: ss2tf for MIMO static gain""" import control # 2x3 TFM a = [] b = [] c = [] d = np.matrix([[0.5, 30, 0.0625], [-0.5, -1.25, 101.3]]) gtf = control.ss2tf(control.ss(a, b, c, d)) # we need a 3x2x1 array to compare with gtf.num # np.testing.assert_array_equal doesn't seem to like a matrices # with an extra dimension, so convert to ndarray numref = np.asarray(d)[..., np.newaxis] np.testing.assert_array_equal(numref, np.array(gtf.num) / np.array(gtf.den))
def test_indexing(self): """Test TF scalar indexing and slice""" tm = ss2tf(rss(5, 3, 3)) # scalar indexing sys01 = tm[0, 1] np.testing.assert_array_almost_equal(sys01.num[0][0], tm.num[0][1]) np.testing.assert_array_almost_equal(sys01.den[0][0], tm.den[0][1]) # slice indexing sys = tm[:2, 1:3] np.testing.assert_array_almost_equal(sys.num[0][0], tm.num[0][1]) np.testing.assert_array_almost_equal(sys.den[0][0], tm.den[0][1]) np.testing.assert_array_almost_equal(sys.num[0][1], tm.num[0][2]) np.testing.assert_array_almost_equal(sys.den[0][1], tm.den[0][2]) np.testing.assert_array_almost_equal(sys.num[1][0], tm.num[1][1]) np.testing.assert_array_almost_equal(sys.den[1][0], tm.den[1][1]) np.testing.assert_array_almost_equal(sys.num[1][1], tm.num[1][2]) np.testing.assert_array_almost_equal(sys.den[1][1], tm.den[1][2])
def generate_random_sys_and_save_2(m, n): while True: roots = -np.random.randint(1, 100, n) A = np.diag(roots) B = np.random.rand(n, m) C = np.random.rand(m, n) D = 10 * np.random.rand(m, m) ss = control.ss(A, B, C, D) tf = control.ss2tf(ss) ss = control.tf2ss(tf) sys = WeightedSystem(ss.A, ss.B, ss.C, ss.D, np.zeros(n, n), ss.C.H, ss.D + ss.D.H) alg = get_algorithm_object(sys, 10**(-3)) if sys._check_positivity(sys.H0): continue if sys._check_positivity(alg._get_H_matrix(alg._get_initial_X())): break sys.save()
def testSs2tfStaticSiso(self): """Regression: ss2tf for SISO static gain""" import control gsiso = control.ss2tf(control.ss([], [], [], 0.5)) np.testing.assert_array_equal([[[0.5]]], gsiso.num) np.testing.assert_array_equal([[[1.]]], gsiso.den)
C = [0.0, 1.0] D = 0.0 G = con.StateSpace(A, B, C, D) # Weighting functions w0 = 8.0 # Desired closed-loop bandwidth A = 1 / 100 # Desired disturbance attenuation inside bandwidth M = 1.5 # Desired bound on hinfnorm(S) & hinfnorm(T) w1 = con.TransferFunction([1 / M, w0], [1.0, w0 * A]) #sensitivity weight w2 = con.TransferFunction([1.0, 0.1], [1, 100]) # K*S weight # w3 = [] #empty control weight on T k, cl, info = con.mixsyn(G, w1, w2, w3=None) print(info[0]) #gamma L = G * k Ltf = con.ss2tf(L) Ltf = con.minreal(Ltf) S = 1.0 / (1.0 + Ltf) T = 1 - S plt.figure(1) sw1 = triv_sigma(1.0 / w1, w) sigS = triv_sigma(S, w) plt.semilogx(w, 20 * np.log10(sw1[:, 0]), label=r'$\sigma_1(1/W1)$') plt.semilogx(w, 20 * np.log10(sigS[:, 0]), label=r'$\sigma_1(S)$') plt.ylim([-60, 10]) plt.ylabel('magnitude [dB]') plt.xlim([1e-3, 1e3]) plt.xlabel('freq [rad/s]') plt.legend() plt.title('Singular values of S')
def testSs2tfStaticSiso(self): """Regression: ss2tf for SISO static gain""" import control gsiso = control.ss2tf(control.ss([], [], [], 0.5)) np.testing.assert_array_equal([[[0.5]]], gsiso.num) np.testing.assert_array_equal([[[1.]]], gsiso.den)
import numpy from scipy import signal Izz = 0.08594 # moment of inertia about the z-axis Kd = .002 # damping coefficient lf = .085 # distance from canard to z-axis # state space representation x'(t)=Ax+Bu, y(t)=Cx+D A = numpy.matrix([[0, 1], [0, -Kd / Izz]]) B = numpy.matrix([[0], [lf / Izz]]) C = numpy.matrix([1, 0]) D = 0 # convert it to a transfer function # using control library transfer_func = control.ss2tf(A, B, C, D) # note: we can do the same thing with # scipy but its not as pretty for output # ex: transfer_func = signal.ss2tf(A, B, C, D) is same thing print transfer_func # give me a bode plot of it # using the control library # this yields results that differ from scipy # why? #mag, phase, omega = control.bode(transfer_func) # now lets make a linear time invariant representation # with scipy
def tsystem(self, request): """Define some test systems""" """continuous""" A = np.array([[1., -2.], [3., -4.]]) B = np.array([[5.], [7.]]) C = np.array([[6., 8.]]) D = np.array([[9.]]) siso_ss1 = TSys(StateSpace(A, B, C, D, 0)) siso_ss1.t = np.linspace(0, 1, 10) siso_ss1.ystep = np.array([9., 17.6457, 24.7072, 30.4855, 35.2234, 39.1165, 42.3227, 44.9694, 47.1599, 48.9776]) siso_ss1.X0 = np.array([[.5], [1.]]) siso_ss1.yinitial = np.array([11., 8.1494, 5.9361, 4.2258, 2.9118, 1.9092, 1.1508, 0.5833, 0.1645, -0.1391]) ss1 = siso_ss1.sys """D=0, continuous""" siso_ss2 = TSys(StateSpace(ss1.A, ss1.B, ss1.C, 0, 0)) siso_ss2.t = siso_ss1.t siso_ss2.ystep = siso_ss1.ystep - 9 siso_ss2.initial = siso_ss1.yinitial - 9 siso_ss2.yimpulse = np.array([86., 70.1808, 57.3753, 46.9975, 38.5766, 31.7344, 26.1668, 21.6292, 17.9245, 14.8945]) """System with unspecified timebase""" siso_ss2_dtnone = TSys(StateSpace(ss1.A, ss1.B, ss1.C, 0, None)) siso_ss2_dtnone.t = np.arange(0, 10, 1.) siso_ss2_dtnone.ystep = np.array([0., 86., -72., 230., -360., 806., -1512., 3110., -6120., 12326.]) siso_tf1 = TSys(TransferFunction([1], [1, 2, 1], 0)) siso_tf2 = copy(siso_ss1) siso_tf2.sys = ss2tf(siso_ss1.sys) """MIMO system, contains ``siso_ss1`` twice""" mimo_ss1 = copy(siso_ss1) A = np.zeros((4, 4)) A[:2, :2] = siso_ss1.sys.A A[2:, 2:] = siso_ss1.sys.A B = np.zeros((4, 2)) B[:2, :1] = siso_ss1.sys.B B[2:, 1:] = siso_ss1.sys.B C = np.zeros((2, 4)) C[:1, :2] = siso_ss1.sys.C C[1:, 2:] = siso_ss1.sys.C D = np.zeros((2, 2)) D[:1, :1] = siso_ss1.sys.D D[1:, 1:] = siso_ss1.sys.D mimo_ss1.sys = StateSpace(A, B, C, D) """MIMO system, contains ``siso_ss2`` twice""" mimo_ss2 = copy(siso_ss2) A = np.zeros((4, 4)) A[:2, :2] = siso_ss2.sys.A A[2:, 2:] = siso_ss2.sys.A B = np.zeros((4, 2)) B[:2, :1] = siso_ss2.sys.B B[2:, 1:] = siso_ss2.sys.B C = np.zeros((2, 4)) C[:1, :2] = siso_ss2.sys.C C[1:, 2:] = siso_ss2.sys.C D = np.zeros((2, 2)) mimo_ss2.sys = StateSpace(A, B, C, D, 0) """discrete""" siso_dtf0 = TSys(TransferFunction([1.], [1., 0.], 1.)) siso_dtf0.t = np.arange(4) siso_dtf0.yimpulse = [0., 1., 0., 0.] siso_dtf1 = TSys(TransferFunction([1], [1, 1, 0.25], True)) siso_dtf1.t = np.arange(0, 5, 1) siso_dtf1.ystep = np.array([0. , 0. , 1. , 0. , 0.75]) siso_dtf2 = TSys(TransferFunction([1], [1, 1, 0.25], 0.2)) siso_dtf2.t = np.arange(0, 5, 0.2) siso_dtf2.ystep = np.array( [0. , 0. , 1. , 0. , 0.75 , 0.25 , 0.5625, 0.375 , 0.4844, 0.4219, 0.457 , 0.4375, 0.4482, 0.4424, 0.4456, 0.4438, 0.4448, 0.4443, 0.4445, 0.4444, 0.4445, 0.4444, 0.4445, 0.4444, 0.4444]) """Time step which leads to rounding errors for time vector length""" num = [-0.10966442, 0.12431949] den = [1., -1.86789511, 0.88255018] dt = 0.12493963338370018 siso_dtf3 = TSys(TransferFunction(num, den, dt)) siso_dtf3.t = np.linspace(0, 9*dt, 10) siso_dtf3.ystep = np.array( [ 0. , -0.1097, -0.1902, -0.2438, -0.2729, -0.2799, -0.2674, -0.2377, -0.1934, -0.1368]) """dtf1 converted statically, because Slycot and Scipy produce different realizations, wich means different initial condtions,""" siso_dss1 = copy(siso_dtf1) siso_dss1.sys = StateSpace([[-1., -0.25], [ 1., 0.]], [[1.], [0.]], [[0., 1.]], [[0.]], True) siso_dss1.X0 = [0.5, 1.] siso_dss1.yinitial = np.array([1., 0.5, -0.75, 0.625, -0.4375]) siso_dss2 = copy(siso_dtf2) siso_dss2.sys = tf2ss(siso_dtf2.sys) mimo_dss1 = TSys(StateSpace(ss1.A, ss1.B, ss1.C, ss1.D, True)) mimo_dss1.t = np.arange(0, 5, 0.2) mimo_dss2 = copy(mimo_ss1) mimo_dss2.sys = c2d(mimo_ss1.sys, mimo_ss1.t[1]-mimo_ss1.t[0]) mimo_tf2 = copy(mimo_ss2) tf_ = ss2tf(siso_ss2.sys) mimo_tf2.sys = TransferFunction( [[tf_.num[0][0], [0]], [[0], tf_.num[0][0]]], [[tf_.den[0][0], [1]], [[1], tf_.den[0][0]]], 0) mimo_dtf1 = copy(siso_dtf1) tf_ = siso_dtf1.sys mimo_dtf1.sys = TransferFunction( [[tf_.num[0][0], [0]], [[0], tf_.num[0][0]]], [[tf_.den[0][0], [1]], [[1], tf_.den[0][0]]], True) # for pole cancellation tests pole_cancellation = TSys(TransferFunction( [1.067e+05, 5.791e+04], [10.67, 1.067e+05, 5.791e+04])) no_pole_cancellation = TSys(TransferFunction( [1.881e+06], [188.1, 1.881e+06])) # System Type 1 - Step response not stationary: G(s)=1/s(s+1) siso_tf_type1 = TSys(TransferFunction(1, [1, 1, 0])) siso_tf_type1.step_info = { 'RiseTime': np.NaN, 'SettlingTime': np.NaN, 'SettlingMin': np.NaN, 'SettlingMax': np.NaN, 'Overshoot': np.NaN, 'Undershoot': np.NaN, 'Peak': np.Inf, 'PeakTime': np.Inf, 'SteadyStateValue': np.NaN} # SISO under shoot response and positive final value # G(s)=(-s+1)/(s²+s+1) siso_tf_kpos = TSys(TransferFunction([-1, 1], [1, 1, 1])) siso_tf_kpos.step_info = { 'RiseTime': 1.242, 'SettlingTime': 9.110, 'SettlingMin': 0.90, 'SettlingMax': 1.208, 'Overshoot': 20.840, 'Undershoot': 28.0, 'Peak': 1.208, 'PeakTime': 4.282, 'SteadyStateValue': 1.0} # SISO under shoot response and negative final value # k=-1 G(s)=-(-s+1)/(s²+s+1) siso_tf_kneg = TSys(TransferFunction([1, -1], [1, 1, 1])) siso_tf_kneg.step_info = { 'RiseTime': 1.242, 'SettlingTime': 9.110, 'SettlingMin': -1.208, 'SettlingMax': -0.90, 'Overshoot': 20.840, 'Undershoot': 28.0, 'Peak': 1.208, 'PeakTime': 4.282, 'SteadyStateValue': -1.0} siso_tf_asymptotic_from_neg1 = TSys(TransferFunction([-1, 1], [1, 1])) siso_tf_asymptotic_from_neg1.step_info = { 'RiseTime': 2.197, 'SettlingTime': 4.605, 'SettlingMin': 0.9, 'SettlingMax': 1.0, 'Overshoot': 0, 'Undershoot': 100.0, 'Peak': 1.0, 'PeakTime': 0.0, 'SteadyStateValue': 1.0} siso_tf_asymptotic_from_neg1.kwargs = { 'step_info': {'T': np.arange(0, 5, 1e-3)}} # example from matlab online help # https://www.mathworks.com/help/control/ref/stepinfo.html siso_tf_step_matlab = TSys(TransferFunction([1, 5, 5], [1, 1.65, 5, 6.5, 2])) siso_tf_step_matlab.step_info = { 'RiseTime': 3.8456, 'SettlingTime': 27.9762, 'SettlingMin': 2.0689, 'SettlingMax': 2.6873, 'Overshoot': 7.4915, 'Undershoot': 0, 'Peak': 2.6873, 'PeakTime': 8.0530, 'SteadyStateValue': 2.5} A = [[0.68, -0.34], [0.34, 0.68]] B = [[0.18, -0.05], [0.04, 0.11]] C = [[0, -1.53], [-1.12, -1.10]] D = [[0, 0], [0.06, -0.37]] mimo_ss_step_matlab = TSys(StateSpace(A, B, C, D, 0.2)) mimo_ss_step_matlab.kwargs['step_info'] = {'T': 4.6} mimo_ss_step_matlab.step_info = [[ {'RiseTime': 0.6000, 'SettlingTime': 3.0000, 'SettlingMin': -0.5999, 'SettlingMax': -0.4689, 'Overshoot': 15.5072, 'Undershoot': 0., 'Peak': 0.5999, 'PeakTime': 1.4000, 'SteadyStateValue': -0.5193}, {'RiseTime': 0., 'SettlingTime': 3.6000, 'SettlingMin': -0.2797, 'SettlingMax': -0.1043, 'Overshoot': 118.9918, 'Undershoot': 0, 'Peak': 0.2797, 'PeakTime': .6000, 'SteadyStateValue': -0.1277}], [{'RiseTime': 0.4000, 'SettlingTime': 2.8000, 'SettlingMin': -0.6724, 'SettlingMax': -0.5188, 'Overshoot': 24.6476, 'Undershoot': 11.1224, 'Peak': 0.6724, 'PeakTime': 1, 'SteadyStateValue': -0.5394}, {'RiseTime': 0.0000, # (*) 'SettlingTime': 3.4000, 'SettlingMin': -0.4350, # (*) 'SettlingMax': -0.1485, 'Overshoot': 132.0170, 'Undershoot': 0., 'Peak': 0.4350, 'PeakTime': .2, 'SteadyStateValue': -0.1875}]] # (*): MATLAB gives 0.4 for RiseTime and -0.1034 for # SettlingMin, but it is unclear what 10% and 90% of # the steady state response mean, when the step for # this channel does not start a 0. siso_ss_step_matlab = copy(mimo_ss_step_matlab) siso_ss_step_matlab.sys = siso_ss_step_matlab.sys[1, 0] siso_ss_step_matlab.step_info = siso_ss_step_matlab.step_info[1][0] Ta = [[siso_tf_kpos, siso_tf_kneg, siso_tf_step_matlab], [siso_tf_step_matlab, siso_tf_kpos, siso_tf_kneg]] mimo_tf_step_info = TSys(TransferFunction( [[Ti.sys.num[0][0] for Ti in Tr] for Tr in Ta], [[Ti.sys.den[0][0] for Ti in Tr] for Tr in Ta])) mimo_tf_step_info.step_info = [[Ti.step_info for Ti in Tr] for Tr in Ta] # enforce enough sample points for all channels (they have different # characteristics) mimo_tf_step_info.kwargs['step_info'] = {'T_num': 2000} systems = locals() if isinstance(request.param, str): return systems[request.param] else: return [systems[sys] for sys in request.param]
def testSs2tfStaticSiso(self): """Regression: ss2tf for SISO static gain""" gsiso = ss2tf(ss([], [], [], 0.5)) np.testing.assert_allclose([[[0.5]]], gsiso.num) np.testing.assert_allclose([[[1.]]], gsiso.den)
def siso_tf2(self, siso_ss1): T = copy(siso_ss1) T.sys = ss2tf(siso_ss1.sys) return T
from control import ss, ss2tf from control import bode, nyquist, step_response import scipy.linalg as linalg from numpy import matrix import numpy as np from numpy.linalg import inv import matplotlib.pyplot as plt #----------------------- m = 200.0 A = matrix([[0.0, 1.0], [0.0, 0.0]]) B = matrix([[0.0], [1 / m]]) C = matrix([[1.0, 0.0]]) D = matrix([[0.0]]) G = ss(A, B, C, D) P = ss2tf(G) #------ W = matrix([[1.0]]) V = 1e8 * matrix([[1.0, 0.0], [0.0, 1.0]]) Y = linalg.solve_continuous_are(A.transpose(), C.transpose(), V, W) L = Y * C.transpose() * inv(W) #------------------- R = matrix([[1.0]]) Q = matrix([[1e2, 0.0], [0.0, 1.]]) X = linalg.solve_continuous_are(A, B, Q, R) K = inv(R) * B.transpose() * X # synthesis, controller in state space Ac = A - B * K Bc = matrix([[0.0], [0.0]]) Cc = K Dc = D
def test_printing_mimo(self): """Print MIMO, continuous time""" sys = ss2tf(rss(4, 2, 3)) assert isinstance(str(sys), str) assert isinstance(sys._repr_latex_(), str)
def code_generation(): x = ca.SX.sym('x', 14) x_gz = ca.SX.sym('x_gz', 14) p = ca.SX.sym('p', 16) u = ca.SX.sym('u', 4) t = ca.SX.sym('t') dt = ca.SX.sym('dt') gz_eqs = gazebo_equations() f_state = gz_eqs['state_from_gz'] eqs = rocket_equations() C_FLT_FRB = gz_eqs['C_FLT_FRB'] F_FRB, M_FRB = eqs['force_moment'](x, u, p) F_FLT = ca.mtimes(C_FLT_FRB, F_FRB) M_FLT = ca.mtimes(C_FLT_FRB, M_FRB) f_u_to_fin = ca.Function('rocket_u_to_fin', [u], [u_to_fin(u)], ['u'], ['fin']) f_force_moment = ca.Function('rocket_force_moment', [x, u, p], [F_FLT, M_FLT], ['x', 'u', 'p'], ['F_FLT', 'M_FLT']) u_control = eqs['control'](x, p, t, dt) f_control = ca.Function('rocket_control', [x, p, t, dt], [u_control], ['x', 'p', 't', 'dt'], ['u']) gen = ca.CodeGenerator('casadi_gen.c', { 'main': False, 'mex': False, 'with_header': True, 'with_mem': True }) x0, u0, p0 = do_trim(vt=100, gamma_deg=60, m_fuel=0.8, z=60) x1, u1, p1 = do_trim(vt=100, gamma_deg=75, m_fuel=0.6, z=90) x2, u2, p2 = do_trim(vt=100, gamma_deg=90, m_fuel=0.3, z=100) # x3, u3, p3 = do_trim(vt=100, gamma_deg=85, m_fuel=0, z=100) lin = linearize() import control sys1 = control.ss(*lin(x0, u0, p0)) sys2 = control.ss(*lin(x1, u1, p1)) sys3 = control.ss(*lin(x2, u2, p2)) # sys4 = control.ss(*lin(x3, u3, p3)) G1 = control.ss2tf(sys1[1, 2]) G2 = control.ss2tf(sys2[1, 2]) G3 = control.ss2tf(sys3[1, 2]) # G4 = control.ss2tf(sys4[1, 2]) G1 = control.c2d(G1, .01) G2 = control.c2d(G2, .01) G3 = control.c2d(G3, .01) # G4 = control.c2d(G4, .01) s = control.tf([1, 0], [0, 1]) Hd1 = (20 + 3 / s) Hd2 = (20 + 3 / s) Hd3 = (20 + 3 / s) G1 = control.minreal(G1 * Hd1) G2 = control.minreal(G2 * Hd2) G3 = control.minreal(G3 * Hd3) G1 = control.tf2ss(G1) G2 = control.tf2ss(G2) G3 = control.tf2ss(G3) # G4 = control.minreal(G4) #print (G3) x0 = ca.SX.sym('x0', 8) u = ca.SX.sym('u', 1) x = ca.mtimes(G1.A, x0) + ca.mtimes(G1.B, u) y = ca.mtimes(G1.C, x0) + ca.mtimes(G1.D, u) controller1 = ca.Function('controller1', [x0, u], [x, y]) x = ca.mtimes(G2.A, x0) + ca.mtimes(G2.B, u) y = ca.mtimes(G2.C, x0) + ca.mtimes(G2.D, u) controller2 = ca.Function('controller2', [x0, u], [x, y]) x0 = ca.SX.sym('x', 4) x = ca.mtimes(G3.A, x0) + ca.mtimes(G3.B, u) y = ca.mtimes(G3.C, x0) + ca.mtimes(G3.D, u) controller3 = ca.Function('controller3', [x0, u], [x, y]) gen.add(controller1) gen.add(controller2) gen.add(controller3) gen.add(f_state) gen.add(f_force_moment) gen.add(f_control) gen.add(f_u_to_fin) #gen.generate() return { 'controller1': controller1, 'controller2': controller2, 'controller3': controller3 }
#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Created on Thu Apr 12 18:49:21 2018 @author: eddardd """ import numpy as np import control as ctrl import matplotlib.pyplot as plt from equations import * sys_unstable = linear_equations(sp = 0) tf_unst = ctrl.ss2tf(sys_unstable) nums, dens = ctrl.tfdata(sys_unstable) Tx = ctrl.tf(nums[0][0], dens[0][0]) Ttheta = ctrl.tf(nums[2][0], dens[2][0]) print(ctrl.bode(Ttheta))
''' # 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]]) S = ctl.ss(A, B, C, D) print('S =', S) ######################################################### # Funcao de transferencia ######################################################### G = ctl.ss2tf(S) print('G =', G) ######################################################### # Polos e zeros ######################################################### (p, z) = ctl.pzmap(G) print('polos =', p) print('zeros =', z) plt.show() ######################################################### # Plot da resp no dominio do tempo ao impulso unitario ######################################################### # T, yout = ctl.impulse_response(G) # plt.plot(T, yout)
basis=poly) plot_vehicle_lanechange(traj3) # ####################### # TODO just be a line ####################### # ## Vehicle transfer functions for forward and reverse driving (Example 10.11) # ## 用到动态模型 # Magnitude of the steering input (half maximum) Msteer = vehicle_params['maxsteer'] / 2 # Create a linearized model of the system going forward at 2 m/s forward_lateral = ct.linearize(lateral, [0, 0], [0], params={'velocity': 2}) forward_tf = ct.ss2tf(forward_lateral)[0, 0] print("Forward TF = ", forward_tf) # Create a linearized model of the system going in reverise at 1 m/s reverse_lateral = ct.linearize(lateral, [0, 0], [0], params={'velocity': -2}) reverse_tf = ct.ss2tf(reverse_lateral)[0, 0] print("Reverse TF = ", reverse_tf) # In[ ]: # Configure matplotlib plots to be a bit bigger and optimize layout plt.figure() # Forward motion t, y = ct.step_response(forward_tf * Msteer, np.linspace(0, 4, 500)) plt.plot(t, y, 'b--')
Ki = 1 pid = PID.PID(Kp, Ki, Kd, 0) pid.setKp(6) A = np.matrix('[2 -3; 1 3]') B = np.matrix('[1;-1]') C = np.matrix('[1 0]') D = 0 A2 = np.matrix('[0 1; -2 -3]') B2 = np.matrix('[0;-1]') C2 = np.matrix('[1 0]') D2 = 0 sys = control.ss(A, B, C, D) tf = control.ss2tf(sys) poles = control.pole(sys) acker_k = control.acker(A, B, ["-1", "-1"]) sys_stabil = control.ss(A2, B2, C2, D2) Acl = A - B * acker_k sys2 = control.ss(Acl, B, C, D) pid_ss = pid.create_tf() sys3 = control.series(sys2, pid_ss) sys4 = control.feedback(sys3, 1, -1) while True: value = socket.recv_pyobj() sys_step = control.tf(value[0], [0, 1])
def d2c(sys,method='zoh'): """Continous to discrete conversion with ZOH method Call: sysc=d2c(sys,method='log') Parameters ---------- sys : System in statespace or Tf form method: 'zoh' or 'tustin' Returns ------- sysc: continous system ss or tf """ flag = 0 if isinstance(sys, TransferFunction): sys=tf2ss(sys) flag=1 a=sys.A b=sys.B c=sys.C d=sys.D Ts=sys.dt n=shape(a)[0] nb=shape(b)[1] nc=shape(c)[0] tol=1e-12 if method=='zoh': if n==1: if b[0,0]==1: A=0 B=b/sys.dt C=c D=d else: tmp1=hstack((a,b)) tmp2=hstack((zeros((nb,n)),eye(nb))) tmp=vstack((tmp1,tmp2)) s=logm(tmp) s=s/Ts if norm(imag(s),inf) > sqrt(sp.finfo(float).eps): print('Warning: accuracy may be poor') s=real(s) A=s[0:n,0:n] B=s[0:n,n:n+nb] C=c D=d elif method=='foh': a=mat(a) b=mat(b) c=mat(c) d=mat(d) Id = mat(eye(n)) A = logm(a)/Ts A = real(around(A,12)) Amat = mat(A) B = (a-Id)**(-2)*Amat**2*b*Ts B = real(around(B,12)) Bmat = mat(B) C = c D = d - C*(Amat**(-2)/Ts*(a-Id)-Amat**(-1))*Bmat D = real(around(D,12)) elif method=='tustin': a=mat(a) b=mat(b) c=mat(c) d=mat(d) poles=eigvals(a) if any(abs(poles-1)<200*sp.finfo(float).eps): print('d2c: some poles very close to one. May get bad results.') I=mat(eye(n,n)) tk = 2 / sqrt (Ts) A = (2/Ts)*(a-I)*inv(a+I) iab = inv(I+a)*b B = tk*iab C = tk*(c*inv(I+a)) D = d- (c*iab) else: print('Method not supported') return sysc=StateSpace(A,B,C,D) if flag==1: sysc=ss2tf(sysc) return sysc
from dfs import Mason import control # declares some gains tf_actuator = control.ss2tf(-24, 24, 1, 0) # A state space object float_gain = 0.00140293442430542 # A float gain integrator = control.tf([1], [1, 0]) # A Transger Function object # ====================================================================================================================== # Construct a system mySystem = Mason('mySystem') mySystem.create_node('Input', 'I') mySystem.create_node('actuator') mySystem.connect_node('Input', 'actuator', tf_actuator) mySystem.create_node('alpha') mySystem.connect_node('actuator', 'alpha', float_gain) node_name = mySystem.create_node() mySystem.connect_node('alpha', node_name, theta) mySystem.create_node('output', 'O') mySystem.connect_node(node_name, 'output', integrator) tf_mySystem = mySystem.get_sis_tf() # ====================================================================================================================== # Output is a Transger Function object, once that one of the gais was of this type. See the library "python-control" # If all the gains was float, the output would be float print(tf_mySystem.minreal())