def testCtrbObsvDuality(self): A = np.matrix("1.2 -2.3; 3.4 -4.5") B = np.matrix("5.8 6.9; 8. 9.1") Wc = ctrb(A, B) A = np.transpose(A) C = np.transpose(B) Wo = np.transpose(obsv(A, C)) np.testing.assert_array_almost_equal(Wc, Wo)
def testCtrbObsvDuality(self): A = np.matrix("1.2 -2.3; 3.4 -4.5") B = np.matrix("5.8 6.9; 8. 9.1") Wc = ctrb(A,B); A = np.transpose(A) C = np.transpose(B) Wo = np.transpose(obsv(A,C)); np.testing.assert_array_almost_equal(Wc,Wo)
def testCtrbObsvDuality(self, matarrayin): A = matarrayin([[1.2, -2.3], [3.4, -4.5]]) B = matarrayin([[5.8, 6.9], [8., 9.1]]) Wc = ctrb(A, B) A = np.transpose(A) C = np.transpose(B) Wo = np.transpose(obsv(A, C)) np.testing.assert_array_almost_equal(Wc, Wo)
def testObsvSISO(self, matarrayin): A = matarrayin([[1., 2.], [3., 4.]]) C = matarrayin([[5., 7.]]) Wotrue = np.array([[5., 7.], [26., 38.]]) Wo = obsv(A, C) np.testing.assert_array_almost_equal(Wo, Wotrue) # Make sure default type values are correct assert ismatarrayout(Wo)
def testObsvSISO(self): A = np.array([[1., 2.], [3., 4.]]) C = np.array([[5., 7.]]) Wotrue = np.array([[5., 7.], [26., 38.]]) Wo = obsv(A, C) np.testing.assert_array_almost_equal(Wo, Wotrue) # Make sure default type values are correct self.assertTrue(isinstance(Wo, np.ndarray))
def observable_form(xsys): """Convert a system into observable canonical form Parameters ---------- xsys : StateSpace object System to be transformed, with state `x` Returns ------- zsys : StateSpace object System in observable canonical form, with state `z` T : matrix Coordinate transformation: z = T * x """ # Check to make sure we have a SISO system if not issiso(xsys): raise ControlNotImplemented( "Canonical forms for MIMO systems not yet supported") # Create a new system, starting with a copy of the old one zsys = StateSpace(xsys) # Generate the system matrices for the desired canonical form zsys.C = zeros(shape(xsys.C)) zsys.C[0, 0] = 1 zsys.A = zeros(shape(xsys.A)) Apoly = poly(xsys.A) # characteristic polynomial for i in range(0, xsys.states): zsys.A[i, 0] = -Apoly[i+1] / Apoly[0] if (i+1 < xsys.states): zsys.A[i, i+1] = 1 # Compute the observability matrices for each set of states Wrx = obsv(xsys.A, xsys.C) Wrz = obsv(zsys.A, zsys.C) # Transformation from one form to another Tzx = inv(Wrz) * Wrx # Finally, compute the output matrix zsys.B = Tzx * xsys.B return zsys, Tzx
def test_obsv_siso_deprecated(self): A = np.array([[1., 2.], [3., 4.]]) C = np.array([[5., 7.]]) # Check that default type generates a warning # TODO: remove this check with matrix type is deprecated with warnings.catch_warnings(record=True) as w: use_numpy_matrix(True, warn=False) # warnings off self.assertEqual(len(w), 0) Wo = obsv(A, C) self.assertTrue(isinstance(Wo, np.matrix)) use_numpy_matrix(False)
def testConvert(self): """Test state space to transfer function conversion.""" verbose = self.debug # 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 = ctrb(ssOriginal.A, ssOriginal.B) if (np.linalg.matrix_rank(Cmat) != states): if (verbose): print(" skipping (not reachable)") continue Omat = 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 = \ 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 = tf(num, den) tforig_mag, tforig_phase, tforig_omega = \ 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 = \ 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 = tf(num, den) tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \ 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 compute_gains(Q, R, W, V, dt): """Given LQR Q and R matrices, and Kalman W and V matrices, and sample time, compute optimal feedback gain and optimal filter gains.""" data = np.empty((N, ), dtype=controller_t) # Loop over all speeds for which we have system dynamics for i in range(N): data['theta_R_dot'][i] = theta_R_dot[i] data['dt'][i] = dt # Convert the bike dynamics to discrete time using a zero order hold data['A'][i], data['B'][i], _, _, _ = cont2discrete( (A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt) data['plant_evals_d'][i] = la.eigvals(data['A'][i]) data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt # Bicycle measurement matrices # - steer angle # - roll rate data['C_m'][i] = C_w[i, :2, :] # - yaw rate data['C_z'][i] = C_w[i, 2, :] A = data['A'][i] B = data['B'][i, :, 2].reshape((4, 1)) C_m = data['C_m'][i] C_z = data['C_z'][i] # Controllability from steer torque data['ctrb_plant'][i] = ctrb(A, B) u, s, v = la.svd(data['ctrb_plant'][i]) assert (np.all(s > 1e-13)) # Solve discrete algebraic Ricatti equation associated with LQI problem P_c = dare(A, B, R, Q) # Optimal feedback gain using solution of Ricatti equation K_c = -la.solve(R + dot(B.T, dot(P_c, B)), dot(B.T, dot(P_c, A))) data['K_c'][i] = K_c data['A_c'][i] = A + dot(B, K_c) data['B_c'][i] = B data['controller_evals'][i] = la.eigvals(data['A_c'][i]) data['controller_evals_c'][i] = np.log( data['controller_evals'][i]) / dt assert (np.all(abs(data['controller_evals'][i]) < 1.0)) # Observability from steer angle and roll rate measurement # Note that (A, C_m * A) must be observable in the "current estimator" # formulation data['obsv_plant'][i] = obsv(A, dot(C_m, A)) u, s, v = la.svd(data['obsv_plant'][i]) assert (np.all(s > 1e-13)) # Solve Riccati equation P_e = dare(A.T, C_m.T, V, W) # Compute Kalman gain K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V))) data['K_e'][i] = K_e data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A) data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e)) data['estimator_evals'][i] = la.eigvals(data['A_e'][i]) data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt # Verify that Kalman estimator eigenvalues are stable assert (np.all(abs(data['estimator_evals'][i]) < 1.0)) # Closed loop state space equations A_cl = np.zeros((8, 8)) A_cl[:4, :4] = A A_cl[:4, 4:] = dot(B, K_c) A_cl[4:, :4] = dot(K_e, dot(C_m, A)) A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:] data['A_cl'][i] = A_cl data['closed_loop_evals'][i] = la.eigvals(A_cl) assert (np.all(abs(data['closed_loop_evals'][i]) < 1.0)) B_cl = np.zeros((8, 1)) B_cl[:4, 0] = B.reshape((4, )) B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4, )) data['B_cl'][i] = B_cl C_cl = np.hstack((C_z, np.zeros((1, 4)))) data['C_cl'][i] = C_cl # Transfer functions from r to yaw rate num, den = ss2tf(A_cl, B_cl, C_cl, 0) data['w_r_to_psi_dot'][i], y = freqz(num[0], den) data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi) data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y)) data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi # Open loop transfer function from e to yaw rate (PI loop not closed, # but LQR/LQG loop closed. inner_cl = ss(A_cl, B_cl, C_cl, 0) pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i] * dt]], [[data['Kp_fit'][i]]]) e_to_psi_dot = series(pi_block, inner_cl) num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D) data['w_e_to_psi_dot'][i], y = freqz(num[0], den) data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi) data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y)) data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi return data
def testObsvMIMO(self): A = np.matrix("1. 2.; 3. 4.") C = np.matrix("5. 6.; 7. 8.") Wotrue = np.matrix("5. 6.; 7. 8.; 23. 34.; 31. 46.") Wo = obsv(A, C) np.testing.assert_array_almost_equal(Wo, Wotrue)
def testObsvSISO(self): A = np.matrix("1. 2.; 3. 4.") C = np.matrix("5. 7.") Wotrue = np.matrix("5. 7.; 26. 38.") Wo = obsv(A, C) np.testing.assert_array_almost_equal(Wo, Wotrue)
def testObsvSISO(self): A = np.matrix("1. 2.; 3. 4.") C = np.matrix("5. 7.") Wotrue = np.matrix("5. 7.; 26. 38.") Wo = obsv(A,C) np.testing.assert_array_almost_equal(Wo, Wotrue)
def testObsvMIMO(self, matarrayin): A = matarrayin([[1., 2.], [3., 4.]]) C = matarrayin([[5., 6.], [7., 8.]]) Wotrue = np.array([[5., 6.], [7., 8.], [23., 34.], [31., 46.]]) Wo = obsv(A, C) np.testing.assert_array_almost_equal(Wo, Wotrue)
def testObsvMIMO(self): A = np.matrix("1. 2.; 3. 4.") C = np.matrix("5. 6.; 7. 8.") Wotrue = np.matrix("5. 6.; 7. 8.; 23. 34.; 31. 46.") Wo = obsv(A,C) np.testing.assert_array_almost_equal(Wo, Wotrue)
def testConvert(self): """Test state space to transfer function conversion.""" verbose = self.debug # 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 = ctrb(ssOriginal.A, ssOriginal.B) if (np.linalg.matrix_rank(Cmat) != states): if (verbose): print(" skipping (not reachable)") continue Omat = 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 = \ 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 = tf(num, den) tforig_mag, tforig_phase, tforig_omega = \ 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 = \ 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 = tf(num, den) tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \ 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 compute_gains(Q, R, W, V, dt): """Given LQR Q and R matrices, and Kalman W and V matrices, and sample time, compute optimal feedback gain and optimal filter gains.""" data = np.empty((N,), dtype=controller_t) # Loop over all speeds for which we have system dynamics for i in range(N): data['theta_R_dot'][i] = theta_R_dot[i] data['dt'][i] = dt # Convert the bike dynamics to discrete time using a zero order hold data['A'][i], data['B'][i], _, _, _ = cont2discrete( (A_w[i], B_w[i, :], eye(4), zeros((4, 1))), dt) data['plant_evals_d'][i] = la.eigvals(data['A'][i]) data['plant_evals_c'][i] = np.log(data['plant_evals_d'][i]) / dt # Bicycle measurement matrices # - steer angle # - roll rate data['C_m'][i] = C_w[i, :2, :] # - yaw rate data['C_z'][i] = C_w[i, 2, :] A = data['A'][i] B = data['B'][i, :, 2].reshape((4, 1)) C_m = data['C_m'][i] C_z = data['C_z'][i] # Controllability from steer torque data['ctrb_plant'][i] = ctrb(A, B) u, s, v = la.svd(data['ctrb_plant'][i]) assert(np.all(s > 1e-13)) # Solve discrete algebraic Ricatti equation associated with LQI problem P_c = dare(A, B, R, Q) # Optimal feedback gain using solution of Ricatti equation K_c = -la.solve(R + dot(B.T, dot(P_c, B)), dot(B.T, dot(P_c, A))) data['K_c'][i] = K_c data['A_c'][i] = A + dot(B, K_c) data['B_c'][i] = B data['controller_evals'][i] = la.eigvals(data['A_c'][i]) data['controller_evals_c'][i] = np.log(data['controller_evals'][i]) / dt assert(np.all(abs(data['controller_evals'][i]) < 1.0)) # Observability from steer angle and roll rate measurement # Note that (A, C_m * A) must be observable in the "current estimator" # formulation data['obsv_plant'][i] = obsv(A, dot(C_m, A)) u, s, v = la.svd(data['obsv_plant'][i]) assert(np.all(s > 1e-13)) # Solve Riccati equation P_e = dare(A.T, C_m.T, V, W) # Compute Kalman gain K_e = dot(P_e, dot(C_m.T, la.inv(dot(C_m, dot(P_e, C_m.T)) + V))) data['K_e'][i] = K_e data['A_e'][i] = dot(eye(4) - dot(K_e, C_m), A) data['B_e'][i] = np.hstack((dot(eye(4) - dot(K_e, C_m), B), K_e)) data['estimator_evals'][i] = la.eigvals(data['A_e'][i]) data['estimator_evals_c'][i] = np.log(data['estimator_evals'][i]) / dt # Verify that Kalman estimator eigenvalues are stable assert(np.all(abs(data['estimator_evals'][i]) < 1.0)) # Closed loop state space equations A_cl = np.zeros((8, 8)) A_cl[:4, :4] = A A_cl[:4, 4:] = dot(B, K_c) A_cl[4:, :4] = dot(K_e, dot(C_m, A)) A_cl[4:, 4:] = A - A_cl[4:, :4] + A_cl[:4, 4:] data['A_cl'][i] = A_cl data['closed_loop_evals'][i] = la.eigvals(A_cl) assert(np.all(abs(data['closed_loop_evals'][i]) < 1.0)) B_cl = np.zeros((8, 1)) B_cl[:4, 0] = B.reshape((4,)) B_cl[4:, 0] = dot(eye(4) - dot(K_e, C_m), B).reshape((4,)) data['B_cl'][i] = B_cl C_cl = np.hstack((C_z, np.zeros((1, 4)))) data['C_cl'][i] = C_cl # Transfer functions from r to yaw rate num, den = ss2tf(A_cl, B_cl, C_cl, 0) data['w_r_to_psi_dot'][i], y = freqz(num[0], den) data['w_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi) data['mag_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y)) data['phase_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi # Open loop transfer function from e to yaw rate (PI loop not closed, # but LQR/LQG loop closed. inner_cl = ss(A_cl, B_cl, C_cl, 0) pi_block = ss([[1]], [[1]], [[data['Ki_fit'][i]*dt]], [[data['Kp_fit'][i]]]) e_to_psi_dot = series(pi_block, inner_cl) num, den = ss2tf(e_to_psi_dot.A, e_to_psi_dot.B, e_to_psi_dot.C, e_to_psi_dot.D) data['w_e_to_psi_dot'][i], y = freqz(num[0], den) data['w_e_to_psi_dot'][i] /= (dt * 2.0 * np.pi) data['mag_e_to_psi_dot'][i] = 20.0 * np.log10(abs(y)) data['phase_e_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi return data
def testConvert(self, fixedseed, states, inputs, outputs): """Test state space to transfer function conversion. start with a random SS system and transform to TF then back to SS, check that the matrices are the same. """ ssOriginal = rss(states, outputs, inputs) if verbose: self.printSys(ssOriginal, 1) # Make sure the system is not degenerate Cmat = ctrb(ssOriginal.A, ssOriginal.B) if (np.linalg.matrix_rank(Cmat) != states): pytest.skip("not reachable") Omat = obsv(ssOriginal.A, ssOriginal.C) if (np.linalg.matrix_rank(Omat) != states): pytest.skip("not observable") tfOriginal = tf(ssOriginal) if (verbose): self.printSys(tfOriginal, 2) ssTransformed = ss(tfOriginal) if (verbose): self.printSys(ssTransformed, 3) tfTransformed = tf(ssTransformed) if (verbose): self.printSys(tfTransformed, 4) # Check to see if the state space systems have same dim if (ssOriginal.nstates != ssTransformed.nstates) and verbose: print("WARNING: state space dimension mismatch: %d versus %d" % (ssOriginal.nstates, ssTransformed.nstates)) # 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 = \ 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 = tf(num, den) tforig_mag, tforig_phase, tforig_omega = \ 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 = \ 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, decimal=5) np.testing.assert_array_almost_equal( ssorig_imag, ssxfrm_imag, decimal=5) # Make sure xform'd TF has same frequency response # num = tfTransformed.num[outputNum][inputNum] den = tfTransformed.den[outputNum][inputNum] tfxfrm = tf(num, den) tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \ 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, decimal=5) np.testing.assert_array_almost_equal( ssorig_imag, tfxfrm_imag, decimal=5)