def reachable_form(xsys):
"""Convert a system into reachable canonical form
Parameters
----------
xsys : StateSpace object
System to be transformed, with state `x`
Returns
-------
zsys : StateSpace object
System in reachable 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.B = zeros(shape(xsys.B))
zsys.B[0, 0] = 1.0
zsys.A = zeros(shape(xsys.A))
Apoly = poly(xsys.A) # characteristic polynomial
for i in range(0, xsys.states):
zsys.A[0, i] = -Apoly[i+1] / Apoly[0]
if (i+1 < xsys.states):
zsys.A[i+1, i] = 1.0
# Compute the reachability matrices for each set of states
Wrx = ctrb(xsys.A, xsys.B)
Wrz = ctrb(zsys.A, zsys.B)
if matrix_rank(Wrx) != xsys.states:
raise ValueError("System not controllable to working precision.")
# Transformation from one form to another
Tzx = solve(Wrx.T, Wrz.T).T # matrix right division, Tzx = Wrz * inv(Wrx)
if matrix_rank(Tzx) != xsys.states:
raise ValueError("Transformation matrix singular to working precision.")
# Finally, compute the output matrix
zsys.C = solve(Tzx.T, xsys.C.T).T # matrix right division, zsys.C = xsys.C * inv(Tzx)
return zsys, Tzx
def testAcker(self, fixedseed):
for states in range(1, self.maxStates):
for i in range(self.maxTries):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
sys = rss(states, 1, 1)
if (self.debug):
print(sys)
# Make sure the system is not degenerate
Cmat = ctrb(sys.A, sys.B)
if np.linalg.matrix_rank(Cmat) != states:
if (self.debug):
print(" skipping (not reachable or ill conditioned)")
continue
# Place the poles at random locations
des = rss(states, 1, 1)
poles = pole(des)
# Now place the poles using acker
K = acker(sys.A, sys.B, poles)
new = ss(sys.A - sys.B * K, sys.B, sys.C, sys.D)
placed = pole(new)
# Debugging code
# diff = np.sort(poles) - np.sort(placed)
# if not all(diff < 0.001):
# print("Found a problem:")
# print(sys)
# print("desired = ", poles)
np.testing.assert_array_almost_equal(np.sort(poles),
np.sort(placed),
decimal=4)
def testAcker(self):
for states in range(1, self.maxStates):
for i in range(self.maxTries):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
sys = rss(states, 1, 1)
if (self.debug):
print(sys)
# Make sure the system is not degenerate
Cmat = ctrb(sys.A, sys.B)
if np.linalg.matrix_rank(Cmat) != states:
if (self.debug):
print(" skipping (not reachable or ill conditioned)")
continue
# Place the poles at random locations
des = rss(states, 1, 1);
poles = pole(des)
# Now place the poles using acker
K = acker(sys.A, sys.B, poles)
new = ss(sys.A - sys.B * K, sys.B, sys.C, sys.D)
placed = pole(new)
# Debugging code
# diff = np.sort(poles) - np.sort(placed)
# if not all(diff < 0.001):
# print("Found a problem:")
# print(sys)
# print("desired = ", poles)
np.testing.assert_array_almost_equal(np.sort(poles),
np.sort(placed), decimal=4)
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 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 testCtrbMIMO(self):
A = np.array([[1., 2.], [3., 4.]])
B = np.array([[5., 6.], [7., 8.]])
Wctrue = np.array([[5., 6., 19., 22.], [7., 8., 43., 50.]])
Wc = ctrb(A, B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
# Make sure default type values are correct
self.assertTrue(isinstance(Wc, np.ndarray))
def testCtrbSISO(self):
A = np.array([[1., 2.], [3., 4.]])
B = np.array([[5.], [7.]])
Wctrue = np.array([[5., 19.], [7., 43.]])
Wc = ctrb(A, B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
self.assertTrue(isinstance(Wc, np.ndarray))
self.assertFalse(isinstance(Wc, np.matrix))
def testCtrbMIMO(self, matarrayin):
A = matarrayin([[1., 2.], [3., 4.]])
B = matarrayin([[5., 6.], [7., 8.]])
Wctrue = np.array([[5., 6., 19., 22.], [7., 8., 43., 50.]])
Wc = ctrb(A, B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
# Make sure default type values are correct
assert ismatarrayout(Wc)
def testCtrbSISO(self, matarrayin, matarrayout):
A = matarrayin([[1., 2.], [3., 4.]])
B = matarrayin([[5.], [7.]])
Wctrue = np.array([[5., 19.], [7., 43.]])
with check_deprecated_matrix():
Wc = ctrb(A, B)
assert ismatarrayout(Wc)
np.testing.assert_array_almost_equal(Wc, Wctrue)
def test_ctrb_siso_deprecated(self):
A = np.array([[1., 2.], [3., 4.]])
B = np.array([[5.], [7.]])
# Check that default using np.matrix generates a warning
# TODO: remove this check with matrix type is deprecated
warnings.resetwarnings()
with warnings.catch_warnings(record=True) as w:
use_numpy_matrix(True)
self.assertTrue(issubclass(w[-1].category, UserWarning))
Wc = ctrb(A, B)
self.assertTrue(isinstance(Wc, np.matrix))
self.assertTrue(issubclass(w[-1].category,
PendingDeprecationWarning))
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 testCtrbSISO(self):
A = np.matrix("1. 2.; 3. 4.")
B = np.matrix("5.; 7.")
Wctrue = np.matrix("5. 19.; 7. 43.")
Wc = ctrb(A,B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
def testCtrbMIMO(self):
A = np.matrix("1. 2.; 3. 4.")
B = np.matrix("5. 6.; 7. 8.")
Wctrue = np.matrix("5. 6. 19. 22.; 7. 8. 43. 50.")
Wc = ctrb(A,B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
def testCtrbMIMO(self):
A = np.matrix("1. 2.; 3. 4.")
B = np.matrix("5. 6.; 7. 8.")
Wctrue = np.matrix("5. 6. 19. 22.; 7. 8. 43. 50.")
Wc = ctrb(A, B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
def testCtrbSISO(self):
A = np.matrix("1. 2.; 3. 4.")
B = np.matrix("5.; 7.")
Wctrue = np.matrix("5. 19.; 7. 43.")
Wc = ctrb(A, B)
np.testing.assert_array_almost_equal(Wc, Wctrue)
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)
