def test_multioutput(self):
# Regression test for gh-2669.
# 4 states
A = np.array([[-1.0, 0.0, 1.0, 0.0], [-1.0, 0.0, 2.0, 0.0],
[-4.0, 0.0, 3.0, 0.0], [-8.0, 8.0, 0.0, 4.0]])
# 1 input
B = np.array([[0.3], [0.0], [7.0], [0.0]])
# 3 outputs
C = np.array([[0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0],
[8.0, 8.0, 0.0, 0.0]])
D = np.array([[0.0], [0.0], [1.0]])
# Get the transfer functions for all the outputs in one call.
b_all, a = ss2tf(A, B, C, D)
# Get the transfer functions for each output separately.
b0, a0 = ss2tf(A, B, C[0], D[0])
b1, a1 = ss2tf(A, B, C[1], D[1])
b2, a2 = ss2tf(A, B, C[2], D[2])
# Check that we got the same results.
assert_allclose(a0, a, rtol=1e-13)
assert_allclose(a1, a, rtol=1e-13)
assert_allclose(a2, a, rtol=1e-13)
assert_allclose(b_all, np.vstack((b0, b1, b2)), rtol=1e-13, atol=1e-14)
def test_simo_round_trip(self):
# See gh-5753
tf = ([[1, 2], [1, 1]], [1, 2])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2]], rtol=1e-13)
assert_allclose(B, [[1]], rtol=1e-13)
assert_allclose(C, [[0], [-1]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 2], [1, 1]], rtol=1e-13)
assert_allclose(den, [1, 2], rtol=1e-13)
tf = ([[1, 0, 1], [1, 1, 1]], [1, 1, 1])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-1, -1], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[-1, 0], [0, 0]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 0, 1], [1, 1, 1]], rtol=1e-13)
assert_allclose(den, [1, 1, 1], rtol=1e-13)
tf = ([[1, 2, 3], [1, 2, 3]], [1, 2, 3, 4])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2, -3, -4], [1, 0, 0], [0, 1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2, 3], [1, 2, 3]], rtol=1e-13)
assert_allclose(D, [[0], [0]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, 2, 3], [0, 1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 2, 3, 4], rtol=1e-13)
tf = ([1, [2, 3]], [1, 6])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6]], rtol=1e-31)
assert_allclose(B, [[1]], rtol=1e-31)
assert_allclose(C, [[1], [-9]], rtol=1e-31)
assert_allclose(D, [[0], [2]], rtol=1e-31)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1], [2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6], rtol=1e-13)
tf = ([[1, -3], [1, 2, 3]], [1, 6, 5])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6, -5], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, -3], [-4, -2]], rtol=1e-13)
assert_allclose(D, [[0], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, -3], [1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6, 5], rtol=1e-13)
def test_simo_round_trip(self):
# See gh-5753
tf = ([[1, 2], [1, 1]], [1, 2])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2]], rtol=1e-13)
assert_allclose(B, [[1]], rtol=1e-13)
assert_allclose(C, [[0], [-1]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 2], [1, 1]], rtol=1e-13)
assert_allclose(den, [1, 2], rtol=1e-13)
tf = ([[1, 0, 1], [1, 1, 1]], [1, 1, 1])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-1, -1], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[-1, 0], [0, 0]], rtol=1e-13)
assert_allclose(D, [[1], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[1, 0, 1], [1, 1, 1]], rtol=1e-13)
assert_allclose(den, [1, 1, 1], rtol=1e-13)
tf = ([[1, 2, 3], [1, 2, 3]], [1, 2, 3, 4])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-2, -3, -4], [1, 0, 0], [0, 1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2, 3], [1, 2, 3]], rtol=1e-13)
assert_allclose(D, [[0], [0]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, 2, 3], [0, 1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 2, 3, 4], rtol=1e-13)
tf = ([1, [2, 3]], [1, 6])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6]], rtol=1e-31)
assert_allclose(B, [[1]], rtol=1e-31)
assert_allclose(C, [[1], [-9]], rtol=1e-31)
assert_allclose(D, [[0], [2]], rtol=1e-31)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1], [2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6], rtol=1e-13)
tf = ([[1, -3], [1, 2, 3]], [1, 6, 5])
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[-6, -5], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, -3], [-4, -2]], rtol=1e-13)
assert_allclose(D, [[0], [1]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0, 1, -3], [1, 2, 3]], rtol=1e-13)
assert_allclose(den, [1, 6, 5], rtol=1e-13)
def to_tf(self, iu, iy):
"""Return a transfer function given input and output names.
**Parameters:**
- *iu*: Index or name of the input
This must be specified unless the system has only one input.
- *iy*: Index or name of the output
This must be specified unless the system has only one output.
**Example:**
>>> lin = LinRes('examples/PID.mat')
>>> lin.to_tf()
(array([[ 11., 102., 200.]]), array([ 1., 100., 0.]))
"""
# Return the TF.
return ss2tf(self.sys.A,
self.sys.B,
self.sys.C[iy, :],
self.sys.D[iy, :],
input=iu)
def makeLGS(filt, transposed=False):
"""
Factory function to make a LGS Realization
Option
- transposed: (boolean) indicates if the realization is transposed
Returns
- a dictionary of necessary infos to build the Realization
"""
# We compute (Phi,K,L,D) from the initial state-space
(Phi, K, L, D) = PhiKLD(filt.dSS)
num, den = signal.ss2tf(Phi, K, L, D)
# We compute alphas with JSS-transformation
alpha = JSS_trans(den)
# We compute the PhiKLD_in
(Phi_in, K_in, L_in, D) = PhiKLD_in(alpha, Phi, K, L, D)
# We can compute the (A_in, B_in, C_in, d) abd tge A_in decomposition
(A_in, B_in, C_in, d) = ABCd_in(Phi_in, K_in, L_in, D)
Ad = A_decomposition_LGS(alpha, Phi_in)
# Then, we deduce J to S matrices
JtoS = Matrice_JtoS_LGS(Ad, A_in, B_in, C_in, d)
# TODO: use transposed ???
# return useful infos to build the Realization
return {"JtoS": JtoS}
def ss_to_zpk(A, B, C):
"""Convert a state-space system to sets of Zero-Pole-Gain objects.
Parameters
----------
A: (n,n) array_like
State-space dynamics matrix
B: (n,m) array_like
Input matrix.
C: (n,m) array_like
Output matrix
"""
zpks = []
for im in range(B.shape[1]):
zpks.append([])
for ip in range(C.shape[0]):
try:
num, den = signal.ss2tf(
A, B[:, im].reshape((-1, 1)),
C[ip, :].reshape((1, -1)), np.zeros((1, 1)))
nu2 = num
# strip leading (close to zeros) from num
while np.allclose(nu2[:, 0], 0, 1e-14) and \
nu2.shape[-1] > 1:
nu2 = nu2[:, 1:]
# to zpk
z, p, k = signal.tf2zpk(nu2, den)
zpks[-1].append(ZPK(z, p, k))
except ValueError:
raise RuntimeWarning("cannot analyse state-space")
return zpks
def test_all_int_arrays(self):
A = [[0, 1, 0], [0, 0, 1], [-3, -4, -2]]
B = [[0], [0], [1]]
C = [[5, 1, 0]]
D = [[0]]
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[0.0, 0.0, 1.0, 5.0]], rtol=1e-13, atol=1e-14)
assert_allclose(den, [1.0, 2.0, 4.0, 3.0], rtol=1e-13)
def __init__(self, *args):
if len(args) not in [2, 4]:
raise ValueError("2 (num, den) or 4 (A, B, C, D) arguments "
"expected, not {}.".format((len(args))))
if len(args) == 2:
super().__init__(args[0], args[1])
else:
A, B, C, D = args
n, d = signal.ss2tf(A, B, C, D)
super().__init__(n, d)
def H_s(self,num, type):
ABCD = cir.Circuit.StateSpace(self, num, type)
HStp=ss2tf(ABCD[0],ABCD[1],ABCD[2],ABCD[3])
H1S_zn = np.zeros(len(HStp[1]) + 1)
for i in range(len(HStp[1])):
H1S_zn[i] = HStp[1][i]
H1S = HStp[0][0], H1S_zn
HS = HStp[0][0], HStp[1]
# Возвращает H(s), H1(s)
return HS, H1S,ABCD
def to_dTF(self):
"""
Transform a SISO state-space into a transfer function
"""
if self._p != 1 or self._q != 1:
raise ValueError(
'dSS: the state-space must be SISO to be converted in transfer function'
)
from fixif.LTI import dTF
num, den = ss2tf(self._A, self._B, self._C, self._D)
return dTF(num[0], den)
def test_gbt_with_sio_tf_and_zpk(self):
"""Test method='gbt' with alpha=0.25 for tf and zpk cases."""
# State space coefficients for the continuous SIO system.
A = -1.0
B = 1.0
C = 1.0
D = 0.5
# The continuous transfer function coefficients.
cnum, cden = ss2tf(A, B, C, D)
# Continuous zpk representation
cz, cp, ck = ss2zpk(A, B, C, D)
h = 1.0
alpha = 0.25
# Explicit formulas, in the scalar case.
Ad = (1 + (1 - alpha) * h * A) / (1 - alpha * h * A)
Bd = h * B / (1 - alpha * h * A)
Cd = C / (1 - alpha * h * A)
Dd = D + alpha * C * Bd
# Convert the explicit solution to tf
dnum, dden = ss2tf(Ad, Bd, Cd, Dd)
# Compute the discrete tf using cont2discrete.
c2dnum, c2dden, dt = d2c((cnum, cden), h, method='gbt', alpha=alpha)
assert_allclose(dnum, c2dnum)
assert_allclose(dden, c2dden)
# Convert explicit solution to zpk.
dz, dp, dk = ss2zpk(Ad, Bd, Cd, Dd)
# Compute the discrete zpk using cont2discrete.
c2dz, c2dp, c2dk, dt = d2c((cz, cp, ck), h, method='gbt', alpha=alpha)
assert_allclose(dz, c2dz)
assert_allclose(dp, c2dp)
assert_allclose(dk, c2dk)
def test_gbt_with_sio_tf_and_zpk(self):
"""Test method='gbt' with alpha=0.25 for tf and zpk cases."""
# State space coefficients for the continuous SIO system.
A = -1.0
B = 1.0
C = 1.0
D = 0.5
# The continuous transfer function coefficients.
cnum, cden = ss2tf(A, B, C, D)
# Continuous zpk representation
cz, cp, ck = ss2zpk(A, B, C, D)
h = 1.0
alpha = 0.25
# Explicit formulas, in the scalar case.
Ad = (1 + (1 - alpha) * h * A) / (1 - alpha * h * A)
Bd = h * B / (1 - alpha * h * A)
Cd = C / (1 - alpha * h * A)
Dd = D + alpha * C * Bd
# Convert the explicit solution to tf
dnum, dden = ss2tf(Ad, Bd, Cd, Dd)
# Compute the discrete tf using cont2discrete.
c2dnum, c2dden, dt = c2d((cnum, cden), h, method='gbt', alpha=alpha)
assert_allclose(dnum, c2dnum)
assert_allclose(dden, c2dden)
# Convert explicit solution to zpk.
dz, dp, dk = ss2zpk(Ad, Bd, Cd, Dd)
# Compute the discrete zpk using cont2discrete.
c2dz, c2dp, c2dk, dt = c2d((cz, cp, ck), h, method='gbt', alpha=alpha)
assert_allclose(dz, c2dz)
assert_allclose(dp, c2dp)
assert_allclose(dk, c2dk)
def _ss2tf(A, B, C, D):
# https://github.com/scipy/scipy/issues/5760
if not (len(A) or len(B) or len(C)):
D = np.asarray(D).flatten()
if len(D) != 1:
raise ValueError("D must be scalar for zero-order models")
return (D[0], 1.) # pragma: no cover; solved in scipy>=0.18rc2
nums, den = ss2tf(A, B, C, D)
if len(nums) != 1:
# TODO: support MIMO systems
# https://github.com/scipy/scipy/issues/5753
raise NotImplementedError("System must be SISO")
return nums[0], den
def test_zero_order_round_trip(self):
# See gh-5760
tf = (2, 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0]], rtol=1e-13)
assert_allclose(D, [[2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
tf = ([[5], [2]], 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0], [0]], rtol=1e-13)
assert_allclose(D, [[5], [2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[5, 0], [2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
def test_basic(self):
# Test a round trip through tf2ss and ss2tf.
b = np.array([1.0, 3.0, 5.0])
a = np.array([1.0, 2.0, 3.0])
A, B, C, D = tf2ss(b, a)
assert_allclose(A, [[-2, -3], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2]], rtol=1e-13)
assert_allclose(D, [[1]], rtol=1e-14)
bb, aa = ss2tf(A, B, C, D)
assert_allclose(bb[0], b, rtol=1e-13)
assert_allclose(aa, a, rtol=1e-13)
def test_basic(self):
# Test a round trip through tf2ss and ss2tf.
b = np.array([1.0, 3.0, 5.0])
a = np.array([1.0, 2.0, 3.0])
A, B, C, D = tf2ss(b, a)
assert_allclose(A, [[-2, -3], [1, 0]], rtol=1e-13)
assert_allclose(B, [[1], [0]], rtol=1e-13)
assert_allclose(C, [[1, 2]], rtol=1e-13)
assert_allclose(D, [[1]], rtol=1e-14)
bb, aa = ss2tf(A, B, C, D)
assert_allclose(bb[0], b, rtol=1e-13)
assert_allclose(aa, a, rtol=1e-13)
def test_zero_order_round_trip(self):
# See gh-5760
tf = (2, 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0]], rtol=1e-13)
assert_allclose(D, [[2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
tf = ([[5], [2]], 1)
A, B, C, D = tf2ss(*tf)
assert_allclose(A, [[0]], rtol=1e-13)
assert_allclose(B, [[0]], rtol=1e-13)
assert_allclose(C, [[0], [0]], rtol=1e-13)
assert_allclose(D, [[5], [2]], rtol=1e-13)
num, den = ss2tf(A, B, C, D)
assert_allclose(num, [[5, 0], [2, 0]], rtol=1e-13)
assert_allclose(den, [1, 0], rtol=1e-13)
def test_multioutput(self):
# Regression test for gh-2669.
# 4 states
A = np.array([[-1.0, 0.0, 1.0, 0.0],
[-1.0, 0.0, 2.0, 0.0],
[-4.0, 0.0, 3.0, 0.0],
[-8.0, 8.0, 0.0, 4.0]])
# 1 input
B = np.array([[0.3],
[0.0],
[7.0],
[0.0]])
# 3 outputs
C = np.array([[0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0],
[8.0, 8.0, 0.0, 0.0]])
D = np.array([[0.0],
[0.0],
[1.0]])
# Get the transfer functions for all the outputs in one call.
b_all, a = ss2tf(A, B, C, D)
# Get the transfer functions for each output separately.
b0, a0 = ss2tf(A, B, C[0], D[0])
b1, a1 = ss2tf(A, B, C[1], D[1])
b2, a2 = ss2tf(A, B, C[2], D[2])
# Check that we got the same results.
assert_allclose(a0, a, rtol=1e-13)
assert_allclose(a1, a, rtol=1e-13)
assert_allclose(a2, a, rtol=1e-13)
assert_allclose(b_all, np.vstack((b0, b1, b2)), rtol=1e-13, atol=1e-14)
def _ss2tf(A, B, C, D):
# https://github.com/scipy/scipy/issues/5760
if not (len(A) or len(B) or len(C)):
D = np.asarray(D).flatten()
if len(D) != 1:
raise ValueError("D must be scalar for zero-order models")
return (D[0], 1.) # pragma: no cover; solved in scipy>=0.18rc2
nums, den = ss2tf(A, B, C, D)
if len(nums) != 1:
# TODO: support MIMO/SIMO/MISO systems
# https://github.com/scipy/scipy/issues/5753
raise NotImplementedError("System (%s, %s, %s, %s) must be SISO to "
"convert to transfer function" %
(A, B, C, D))
return nums[0], den
def ss2tf(ss, u_index, y_index):
"""
Compute the transfer function of a given input and output of a state-space
"""
nums, den = signal.ss2tf(ss.A, ss.B, ss.C, ss.D, u_index)
num = nums[y_index]
tf = TransferFunction(num, den)
tf.name = (ss.output_label[y_index] + '/' + ss.input_label[u_index] +
' transfer function of ' + ss.name)
tf.output_label[0] = ss.output_label[y_index]
tf.output_units[0] = ss.output_units[y_index]
tf.input_label[0] = ss.input_label[u_index]
tf.input_units[0] = ss.input_units[u_index]
return tf
def convert2TF(self):
'''
Returns
-------
TYPE TransferFunction object
DESCRIPTION. Convert SS model to TF model
'''
num_coef, den_coef = signal.ss2tf(self.A, self.B, self.C, self.D)
if np.rank(num_coef) == 1:
self.tf = TransferFunction(num_coef, den_coef)
return self.tf
elif np.rank(num_coef) == 2:
self.tfs = []
for i in range(len(num_coef)):
self.tfs.append(TransferFunction(num_coef[i], den_coef))
return self.tfs
def makeLCW(filt, transposed=False):
"""Retourne la forme SIF de la structure LCW correspondant au filtre donné"""
# Par la fonction de scipy.signal on obtient le state-space associé
# On calcule ensuite (Phi,K,L,D) qu'on reconverti en fonction de transfert
(Phi, K, L, D) = PhiKLD(filt.dSS)
num, den = signal.ss2tf(Phi, K, L, D)
# On calcule les alpha par la JSS-transformation
alpha = JSS_trans(den)
# On calcule les PhiKLD_in
(Phi_in, K_in, L_in, D) = PhiKLD_in(alpha, Phi, K, L, D)
# On peut calculer les (A,B,C,d)_in et la
# décomposition de A_in en produit de matrice.
(A_ib, B_ib, C_ib, d) = ABCd_in(Phi_in, K_in, L_in, D)
(As, Bs, Cs, d) = ABCd_star(A_ib, B_ib, C_ib, d, Phi_in, K_in)
Ad = A_decomposition_LCW(alpha, Phi_in)
# On construit les matrices qui forment Z
JtoS = Matrice_JtoS_LCW(Ad, As, Bs, Cs, d)
# TODO: use transposed ???
# return useful infos to build the Realization
return {"JtoS": JtoS}
def to_tf(self, iu, iy):
"""Return a transfer function given input and output names.
**Parameters:**
- *iu*: Index or name of the input
This must be specified unless the system has only one input.
- *iy*: Index or name of the output
This must be specified unless the system has only one output.
**Example:**
>>> lin = LinRes('examples/PID.mat')
>>> lin.to_tf()
(array([[ 11., 102., 200.]]), array([ 1., 100., 0.]))
"""
# Return the TF.
return ss2tf(self.sys.A, self.sys.B,
self.sys.C[iy, :], self.sys.D[iy, :], input=iu)
def form_PI_cl(data):
A = np.array([[1.0]])
B = np.array([[1.0]])
for i in range(N):
C = np.array([[dt*data['Ki_fit'][i]]])
D = np.array([[data['Kp_fit'][i]]])
pi_block = ss(A, B, C, D)
bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
pc = series(pi_block, bike_block)
cl = feedback(pc, 1, sign=-1)
data['yr_cl_evals'][i] = la.eigvals(cl.A)
assert(np.all(abs(data['yr_cl_evals'][i]) < 1.0))
data['A_yr_cl'][i] = cl.A
data['B_yr_cl'][i] = cl.B
data['C_yr_cl'][i] = cl.C
assert(cl.D == 0)
num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_psi_r_to_psi_dot'][i] = np.unwrap(np.angle(y)) * 180.0 / np.pi
def form_PI_cl(data):
A = np.array([[1.0]])
B = np.array([[1.0]])
for i in range(N):
C = np.array([[dt * data['Ki_fit'][i]]])
D = np.array([[data['Kp_fit'][i]]])
pi_block = ss(A, B, C, D)
bike_block = ss(data['A_cl'][i], data['B_cl'][i], data['C_cl'][i], 0)
pc = series(pi_block, bike_block)
cl = feedback(pc, 1, sign=-1)
data['yr_cl_evals'][i] = la.eigvals(cl.A)
assert (np.all(abs(data['yr_cl_evals'][i]) < 1.0))
data['A_yr_cl'][i] = cl.A
data['B_yr_cl'][i] = cl.B
data['C_yr_cl'][i] = cl.C
assert (cl.D == 0)
num, den = ss2tf(cl.A, cl.B, cl.C, cl.D)
data['w_psi_r_to_psi_dot'][i], y = freqz(num[0], den)
data['w_psi_r_to_psi_dot'][i] /= (dt * 2.0 * np.pi)
data['mag_psi_r_to_psi_dot'][i] = 20.0 * np.log10(abs(y))
data['phase_psi_r_to_psi_dot'][i] = np.unwrap(
np.angle(y)) * 180.0 / np.pi
#Matriz C
C = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]])
#Matriz D
D = np.array([[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0]])
################################################
################################################
#Calculo das variables e funcions de transferencia
#Sistema e funcions de transferencia
sys = signal.lti(A, B, C, D)
num1, den1 = signal.ss2tf(A, B, C, D, 0)
num2, den2 = signal.ss2tf(A, B, C, D, 1)
num3, den3 = signal.ss2tf(A, B, C, D, 2)
transf_11 = cnt.tf(num1[0, :], den1)
transf_12 = cnt.tf(num1[1, :], den1)
transf_13 = cnt.tf(num1[2, :], den1)
transf_21 = cnt.tf(num2[0, :], den2)
transf_22 = cnt.tf(num2[1, :], den2)
transf_23 = cnt.tf(num2[2, :], den2)
transf_31 = cnt.tf(num3[0, :], den3)
transf_32 = cnt.tf(num3[1, :], den3)
transf_33 = cnt.tf(num3[2, :], den3)
def calculateQTF(ABCDr):
"""Calculate noise and signal transfer functions for a quadrature modulator
**Parameters:**
ABCDr : ndarray
The ABCD matrix, in real form. You may call :func:`mapQtoR` to convert
an imaginary (quadrature) ABCD matrix to a real one.
**Returns:**
ntf, stf, intf, istf : tuple of zpk tuples
The quadrature noise and signal transfer functions.
:raises RuntimeError: if the supplied ABCD matrix results in denominator mismatches.
"""
A, B, C, D = partitionABCD(ABCDr, 4)
#Construct an ABCD description of the closed-loop system
# sys is a tuple in A, B, C, D form
Acl = A + np.dot(B[:, 2:4], C)
Bcl = B
Ccl = C
Dcl = np.hstack((D[:, 0:2], np.eye(2)))
#sys = (A + np.dot(B[:, 2:4], C), B, C,
# np.hstack((D[:, 0:2], np.eye(2))))
#Calculate the 2x4 matrix of transfer functions
tfs = np.empty((2, 4), dtype=object)
# Each tf is a tuple in num, den form
# tf[i, j] corresponds to the TF from input j to output i
for i in range(2):
for j in range(4):
tfs[i, j] = ss2tf(Acl, Bcl, Ccl[i, :], Dcl[i, :], input=j)
#Reduce these to NTF, STF, INTF and ISTF
if any(tfs[0, 2][1] != tfs[1, 3][1]):
raise RuntimeError('TF Denominator mismatch. Location 1')
ntf_x = (0.5 * (tfs[0, 2][0] + tfs[1, 3][0]), tfs[0, 2][1])
intf_x = (0.5 * (tfs[0, 2][0] - tfs[1, 3][0]), tfs[0, 2][1])
if any(tfs[0, 3][1] != tfs[1, 2][1]):
raise RuntimeError('TF Denominator mismatch. Location 2')
ntf_y = (0.5 * (tfs[1, 2][0] - tfs[0, 3][0]), tfs[1, 2][1])
intf_y = (0.5 * (tfs[1, 2][0] + tfs[0, 3][0]), tfs[1, 2][1])
if any(ntf_x[1] != ntf_y[1]):
raise RuntimeError('TF Denominator mismatch. Location 3')
if any(tfs[0, 0][1] != tfs[1, 1][1]):
raise RuntimeError('TF Denominator mismatch. Location 4')
stf_x = (0.5 * (tfs[0, 0][0] + tfs[1, 1][0]), tfs[0, 0][1])
istf_x = (0.5 * (tfs[0, 0][0] - tfs[1, 1][0]), tfs[0, 0][1])
if any(tfs[0, 1][1] != tfs[1, 0][1]):
raise RuntimeError('TF Denominator mismatch. Location 5')
stf_y = (0.5 * (tfs[1, 0][0] - tfs[0, 1][0]), tfs[1, 0][1])
istf_y = (0.5 * (tfs[1, 0][0] + tfs[0, 1][0]), tfs[1, 0][1])
if any(stf_x[1] != stf_y[1]):
raise RuntimeError('TF Denominator mismatch. Location 6')
# suppress warnings about complex TFs
#warning('off')
ntf = cancelPZ(tf2zpk(ntf_x[0] + 1j * ntf_y[0], ntf_x[1]))
intf = cancelPZ(tf2zpk(intf_x[0] + 1j * intf_y[0], intf_x[1]))
stf = cancelPZ(tf2zpk(stf_x[0] + 1j * stf_y[0], ntf_x[1]))
istf = cancelPZ(tf2zpk(istf_x[0] + 1j * istf_y[0], intf_x[1]))
#warning('on')
return ntf, stf, intf, istf
def test_state_operators_1(self):
#test the state space operators to see if they correspond to the correct transfer function
success = True
#position and field arguments
n_f = 6
n_r = 3
n_x, n_y, n_z = map(int, np.random.randint(low=1, high=50, size=3))
del_t = 1.0e-12
#build random tensors
inf_x = np.float32(np.random.rand(n_x, n_y, n_z))
w_0 = np.float32(np.random.rand(n_x, n_y, n_z, n_f // 2, n_r)) * 10**12
damp = 0.2 * w_0
del_x = np.float32(np.random.rand(n_x, n_y, n_z, n_f // 2, n_r))
#produce operators
sta_ope, _ = ELECTRIC_DISPERSION_OPERATORS(w_0, damp, del_x, del_t,
inf_x)
a = sta_ope[0]
b = sta_ope[1]
c = sta_ope[2]
d = sta_ope[3]
#secondary constants
beta = np.sqrt(w_0**2 - damp**2)
A_e = np.exp(-damp * del_t) * np.cos(beta * del_t)
B_e = np.exp(-damp * del_t) * np.sin(beta * del_t)
K1 = del_x * (1 - 2 * A_e + A_e**2 + B_e**2)
#primary constants
a_1 = 2 * A_e
a_2 = -(A_e**2 + B_e**2)
b_1 = -2 * K1
b_2 = 2 * K1
#calculate the lorentz transfer function
tran_num_coeff, tran_den_coeff = LORENTZ_TRANSFER_FUNCTION(
a_1, a_2, b_1, b_2)
with tf.Session() as sess:
d = sess.run(d)
c = sess.run(c)
b = sess.run(b)
a = sess.run(a)
B = b[0, 0, 0, 0, :, np.newaxis]
for i in range(len(b[0, 0, 0, 0, :])):
B[i, 0] = b[0, 0, 0, 0, i]
A = a[0, 0, 0, 0, :, :]
C = [c[0, 0, 0, 0, :]]
D = d[0, 0, 0, 0]
numden = ss2tf(A, B, C, D)
num = numden[0]
den = numden[1]
np.amax(num - tran_num_coeff[0, 0, 0, 0]) < 5.0e-7 and np.amax(
den - tran_den_coeff[0, 0, 0, 0]) < 5.0e-7 and success
self.assertEqual(success, True)
def filter(self, *filt, **kwargs):
"""Apply the given filter to this `Spectrum`.
Recognised filter arguments are converted into the standard
``(numerator, denominator)`` representation before being applied
to this `Spectrum`.
Parameters
----------
*filt
one of:
- :class:`scipy.signal.lti`
- ``(numerator, denominator)`` polynomials
- ``(zeros, poles, gain)``
- ``(A, B, C, D)`` 'state-space' representation
Returns
-------
result : `Spectrum`
the filtered version of the input `Spectrum`
See also
--------
scipy.signal.zpk2tf
for details on converting ``(zeros, poles, gain)`` into
transfer function format
scipy.signal.ss2tf
for details on converting ``(A, B, C, D)`` to transfer function
format
scipy.signal.freqs
for details on the filtering calculation
Examples
--------
To apply a zpk filter with a pole at 0 Hz, a zero at 100 Hz and
a gain of 25::
>>> data2 = data.filter([100], [0], 25)
Raises
------
ValueError
If ``filt`` arguments cannot be interpreted properly
"""
# parse filter
if len(filt) == 1 and isinstance(filt[0], signal.lti):
filt = filt[0]
a = filt.den
b = filt.num
elif len(filt) == 2:
b, a = filt
elif len(filt) == 3:
b, a = signal.zpk2tf(*filt)
elif len(filt) == 4:
b, a = signal.ss2tf(*filt)
else:
raise ValueError("Cannot interpret filter arguments. Please give "
"either a signal.lti object, or a tuple in zpk "
"or ba format. See scipy.signal docs for "
"details.")
# parse keyword args
inplace = kwargs.pop('inplace', False)
if kwargs:
raise TypeError("Spectrum.filter() got an unexpected keyword "
"argument '%s'" % list(kwargs.keys())[0])
fresp = abs(signal.freqs(b, a, self.frequencies * 2 * pi)[1])
if inplace:
self *= fresp
return self
else:
new = self * fresp
return new
def filter(self, *filt):
"""Apply the given filter to this `TimeSeries`.
All recognised filter arguments are converted either into cascading
second-order sections (if scipy >= 0.16 is installed), or into the
``(numerator, denominator)`` representation before being applied
to this `TimeSeries`.
.. note::
All filters are presumed to be digital (Z-domain), if you have
an analog ZPK (in Hertz or in rad/s) you should be using
`TimeSeries.zpk` instead.
.. note::
When using `scipy` < 0.16 some higher-order filters may be
unstable. With `scipy` >= 0.16 higher-order filters are
decomposed into second-order-sections, and so are much more stable.
Parameters
----------
*filt
one of:
- :class:`scipy.signal.lti`
- `MxN` `numpy.ndarray` of second-order-sections
(`scipy` >= 0.16 only)
- ``(numerator, denominator)`` polynomials
- ``(zeros, poles, gain)``
- ``(A, B, C, D)`` 'state-space' representation
Returns
-------
result : `TimeSeries`
the filtered version of the input `TimeSeries`
See also
--------
TimeSeries.zpk
for instructions on how to filter using a ZPK with frequencies
in Hertz
scipy.signal.sosfilter
for details on the second-order section filtering method
(`scipy` >= 0.16 only)
scipy.signal.lfilter
for details on the filtering method
Raises
------
ValueError
If ``filt`` arguments cannot be interpreted properly
"""
sos = None
# single argument given
if len(filt) == 1:
filt = filt[0]
# detect LTI
if isinstance(filt, signal.lti):
filt = filt
a = filt.den
b = filt.num
# detect SOS
elif isinstance(filt, numpy.ndarray) and filt.ndim == 2:
sos = filt
# detect taps
else:
b = filt
a = [1]
# detect TF
elif len(filt) == 2:
b, a = filt
elif len(filt) == 3:
try:
sos = signal.zpk2sos(*filt)
except AttributeError:
b, a = signal.zpk2tf(*filt)
elif len(filt) == 4:
try:
zpk = signal.ss2zpk(*filt)
sos = signal.zpk2sos(zpk)
except AttributeError:
b, a = signal.ss2tf(*filt)
else:
raise ValueError("Cannot interpret filter arguments. Please "
"give either a signal.lti object, or a "
"tuple in zpk or ba format. See "
"scipy.signal docs for details.")
if sos is not None:
new = signal.sosfilt(sos, self, axis=0).view(self.__class__)
else:
new = signal.lfilter(b, a, self, axis=0).view(self.__class__)
new.__dict__ = self.copy_metadata()
return new
D_true,
A_ML,
B_ML,
C_ML,
D_ML,
w_plot,
save=True)
# convert to transfer function
num_samples = np.shape(A_traces)[0]
# for i in range(num_samples):
tf_nums = np.zeros((num_samples, 7))
tf_dens = np.zeros((num_samples, 7))
for i in range(num_samples):
tf = ss2tf(A_traces[i, :, :], np.expand_dims(B_traces[i, :], 1),
np.expand_dims(C_traces[i, :], 0), float(D_traces[i]))
tf_nums[i, :] = tf[0]
tf_dens[i, :] = tf[1]
tf_num_mean = np.mean(tf_nums, 0)
tf_den_mean = np.mean(tf_dens, 0)
w, mag_mean, phase_mean = signal.bode((tf_num_mean, tf_den_mean))
#
# plt.semilogx(w,mag_mean)
# plt.show()
hmc_sysid_results = {
"A_traces": A_traces,
"B_traces": B_traces,
def _get_num_den(arg, input=0):
"""Utility method to convert the input arg to a (num, den) representation.
**Parameters:**
arg, which may be:
* ZPK tuple,
* num, den tuple,
* A, B, C, D tuple,
* a scipy LTI object,
* a sequence of the tuples of any of the above types.
input : scalar
In case the system has multiple inputs, which input is to be
considered. Input `0` means first input, and so on.
**Returns:**
The sequence of ndarrays num, den
**Raises:**
TypeError, ValueError
.. warn: support for MISO transfer functions is experimental.
"""
num, den = None, None
if isinstance(arg, np.ndarray):
# ABCD matrix
A, B, C, D = partitionABCD(arg)
num, den = ss2tf(A, B, C, D, input=input)
elif isinstance(arg, lti):
arx = arg.to_tf()
num, den = arx.num, arx.den
elif _is_num_den(arg):
num, den = carray(arg[0]).squeeze(), carray(arg[1]).squeeze()
elif _is_zpk(arg):
num, den = zpk2tf(*arg)
elif _is_A_B_C_D(arg):
num, den = ss2tf(*arg, input=input)
elif isinstance(arg, collections.Iterable):
ri = 0
for i in arg:
# Note we do not check if the user has assembled a list with
# mismatched representations.
if hasattr(i, 'B'): # lti
iis = i.B.shape[1]
if input < ri + iis:
num, den = ss2tf(i.A, i.B, i.C, i.D, input=input - ri)
break
else:
ri += iis
else:
sys = lti(*i)
iis = sys.B.shape[1]
if input < ri + iis:
num, den = ss2tf(sys.A,
sys.B,
sys.C,
sys.D,
input=input - ri)
break
else:
ri += iis
if (num, den) == (None, None):
raise ValueError("The LTI representation does not have enough" +
"inputs: max %d, looking for input %d" %
(ri - 1, input))
else:
raise TypeError("Unknown LTI representation: %s" % arg)
if len(num.shape) > 1:
num = num.squeeze()
if len(den.shape) > 1:
den = den.squeeze()
# default accuracy: sqrt_ps
sqrt_eps = np.sqrt(eps)
while len(num.shape) and len(num):
if abs(num[0]) < sqrt_eps:
num = num[1:]
else:
break
while len(den.shape) and len(den):
if abs(den[0]) < sqrt_eps:
den = den[1:]
else:
break
den = np.atleast_1d(den)
num = np.atleast_1d(num)
return num, den
def filter(self, *filt, **kwargs):
"""Apply the given filter to this `FrequencySeries`.
Recognised filter arguments are converted into the standard
``(numerator, denominator)`` representation before being applied
to this `FrequencySeries`.
.. note::
Unlike the related
:meth:`TimeSeries.filter <gwpy.timeseries.TimeSeries.filter>`
method, here all frequency information (e.g. frequencies of
poles or zeros in a ZPK) is assumed to be in Hertz.
Parameters
----------
*filt
one of:
- :class:`scipy.signal.lti`
- ``(numerator, denominator)`` polynomials
- ``(zeros, poles, gain)``
- ``(A, B, C, D)`` 'state-space' representation
Returns
-------
result : `FrequencySeries`
the filtered version of the input `FrequencySeries`
See also
--------
FrequencySeries.zpk
for information on filtering in zero-pole-gain format
scipy.signal.zpk2tf
for details on converting ``(zeros, poles, gain)`` into
transfer function format
scipy.signal.ss2tf
for details on converting ``(A, B, C, D)`` to transfer function
format
scipy.signal.freqs
for details on the filtering calculation
Raises
------
ValueError
If ``filt`` arguments cannot be interpreted properly
"""
# parse filter
if len(filt) == 1 and isinstance(filt[0], signal.lti):
filt = filt[0]
a = filt.den
b = filt.num
elif len(filt) == 2:
b, a = filt
elif len(filt) == 3:
b, a = signal.zpk2tf(*filt)
elif len(filt) == 4:
b, a = signal.ss2tf(*filt)
else:
raise ValueError("Cannot interpret filter arguments. Please give "
"either a signal.lti object, or a tuple in zpk "
"or ba format. See scipy.signal docs for "
"details.")
# parse keyword args
inplace = kwargs.pop('inplace', False)
if kwargs:
raise TypeError("FrequencySeries.filter() got an unexpected "
"keyword argument '%s'" % list(kwargs.keys())[0])
fresp = abs(signal.freqs(b, a, self.frequencies.value)[1])
if inplace:
self.value *= fresp
return self
else:
new = (self.value * fresp).view(type(self))
new.__dict__ = deepcopy(self.__dict__)
return new
def filter(self, *filt, **kwargs):
"""Apply the given filter to this `Spectrum`.
Recognised filter arguments are converted into the standard
``(numerator, denominator)`` representation before being applied
to this `Spectrum`.
.. note::
Unlike the related
:meth:`TimeSeries.filter <gwpy.timeseries.TimeSeries.filter>`
method, here all frequency information (e.g. frequencies of
poles or zeros in a ZPK) is assumed to be in Hertz.
Parameters
----------
*filt
one of:
- :class:`scipy.signal.lti`
- ``(numerator, denominator)`` polynomials
- ``(zeros, poles, gain)``
- ``(A, B, C, D)`` 'state-space' representation
Returns
-------
result : `Spectrum`
the filtered version of the input `Spectrum`
See also
--------
Spectrum.zpk
for information on filtering in zero-pole-gain format
scipy.signal.zpk2tf
for details on converting ``(zeros, poles, gain)`` into
transfer function format
scipy.signal.ss2tf
for details on converting ``(A, B, C, D)`` to transfer function
format
scipy.signal.freqs
for details on the filtering calculation
Raises
------
ValueError
If ``filt`` arguments cannot be interpreted properly
"""
# parse filter
if len(filt) == 1 and isinstance(filt[0], signal.lti):
filt = filt[0]
a = filt.den
b = filt.num
elif len(filt) == 2:
b, a = filt
elif len(filt) == 3:
b, a = signal.zpk2tf(*filt)
elif len(filt) == 4:
b, a = signal.ss2tf(*filt)
else:
raise ValueError("Cannot interpret filter arguments. Please give "
"either a signal.lti object, or a tuple in zpk "
"or ba format. See scipy.signal docs for "
"details.")
# parse keyword args
inplace = kwargs.pop('inplace', False)
if kwargs:
raise TypeError("Spectrum.filter() got an unexpected keyword "
"argument '%s'" % list(kwargs.keys())[0])
fresp = abs(signal.freqs(b, a, self.frequencies.value)[1])
if inplace:
self.value *= fresp
return self
else:
new = (self.value * fresp).view(type(self))
new.__dict__ = deepcopy(self.__dict__)
return new
[1,2],
[2,0]
])
B = np.array([
[1],
[2]
])
C = np.array([
[1,0]
])
D = 0
sys = signal.StateSpace(A,B,C,D) #State space to time domain conversion
a,b = signal.ss2tf(A,B,C,D) #Importing Num. and Den. of T.F. from State Space Rep.
#rounding off
num = np.around(a[0],decimals = 0)
den = np.around(b,decimals = 0)
s = sp.symbols('s')
H_s = sp.Poly(num,s)/sp.Poly(den,s) # Getting polynomial expressions
print("THE TRANSFER FUNCTION OF THE SYSTEM ")
print("H(s) =",H_s)
import numpy as np
from scipy import signal
dvec = np.array([1, 2, 3, 4])
A1 = np.array([-dvec, [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
B1 = np.array([[1, 0, 0,
0]]).T # wrong dimension, this requires D has one column
B1 = np.eye(4)
C1 = np.array([[1, 2, 3, 4]])
D1 = np.zeros((1, 4))
print signal.ss2tf(A1, B1, C1, D1)
#same as http://en.wikipedia.org/wiki/State_space_(controls)#Canonical_realizations
signal.ss2tf(*signal.tf2ss(*signal.ss2tf(A1, B1, C1, D1)))
np.testing.assert_almost_equal(
signal.ss2tf(*signal.tf2ss(*signal.ss2tf(A1, B1, C1, D1)))[0],
signal.ss2tf(A1, B1, C1, D1)[0])
'''
dx_t = A x_t + B u_t
y_t = C x_t + D u_t
>>> dvec = np.array([1,2,3,4])
>>> A = np.array([-dvec,[1,0,0,0],[0,1,0,0],[0,0,1,0]])
>>> B = np.array([[1,0,0,0]]).T # wrong dimension, this requires D has one column
>>> B = np.eye(4)
>>> C = np.array([[1,2,3,4]])
>>> D = np.zeros((1,4))
>>> num, den = signal.ss2tf(A,B,C,D)
>>> print num
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 check_matrix_shapes(self, p, q, r):
ss2tf(np.zeros((p, p)),
np.zeros((p, q)),
np.zeros((r, p)),
np.zeros((r, q)), 0)
def calculateQTF(ABCDr):
"""Calculate noise and signal transfer functions for a quadrature modulator
**Parameters:**
ABCDr : ndarray
The ABCD matrix, in real form. You may call :func:`mapQtoR` to convert
an imaginary (quadrature) ABCD matrix to a real one.
**Returns:**
ntf, stf, intf, istf : tuple of zpk tuples
The quadrature noise and signal transfer functions.
:raises RuntimeError: if the supplied ABCD matrix results in denominator mismatches.
"""
A, B, C, D = partitionABCD(ABCDr, 4)
#Construct an ABCD description of the closed-loop system
# sys is a tuple in A, B, C, D form
Acl = A + np.dot(B[:, 2:4], C)
Bcl = B
Ccl = C
Dcl = np.hstack((D[:, 0:2], np.eye(2)))
#sys = (A + np.dot(B[:, 2:4], C), B, C,
# np.hstack((D[:, 0:2], np.eye(2))))
#Calculate the 2x4 matrix of transfer functions
tfs = np.empty((2, 4), dtype=object)
# Each tf is a tuple in num, den form
# tf[i, j] corresponds to the TF from input j to output i
for i in range(2):
for j in range(4):
tfs[i, j] = ss2tf(Acl, Bcl, Ccl[i, :], Dcl[i, :], input=j)
#Reduce these to NTF, STF, INTF and ISTF
if any(tfs[0, 2][1] != tfs[1, 3][1]):
raise RuntimeError('TF Denominator mismatch. Location 1')
ntf_x = (0.5 * (tfs[0, 2][0] + tfs[1, 3][0]), tfs[0, 2][1])
intf_x = (0.5 * (tfs[0, 2][0] - tfs[1, 3][0]), tfs[0, 2][1])
if any(tfs[0, 3][1] != tfs[1, 2][1]):
raise RuntimeError('TF Denominator mismatch. Location 2')
ntf_y = (0.5 * (tfs[1, 2][0] - tfs[0, 3][0]), tfs[1, 2][1])
intf_y = (0.5 * (tfs[1, 2][0] + tfs[0, 3][0]), tfs[1, 2][1])
if any(ntf_x[1] != ntf_y[1]):
raise RuntimeError('TF Denominator mismatch. Location 3')
if any(tfs[0, 0][1] != tfs[1, 1][1]):
raise RuntimeError('TF Denominator mismatch. Location 4')
stf_x = (0.5 * (tfs[0, 0][0] + tfs[1, 1][0]), tfs[0, 0][1])
istf_x = (0.5 * (tfs[0, 0][0] - tfs[1, 1][0]), tfs[0, 0][1])
if any(tfs[0, 1][1] != tfs[1, 0][1]):
raise RuntimeError('TF Denominator mismatch. Location 5')
stf_y = (0.5 * (tfs[1, 0][0] - tfs[0, 1][0]), tfs[1, 0][1])
istf_y = (0.5 * (tfs[1, 0][0] + tfs[0, 1][0]), tfs[1, 0][1])
if any(stf_x[1] != stf_y[1]):
raise RuntimeError('TF Denominator mismatch. Location 6')
# suppress warnings about complex TFs
#warning('off')
ntf = cancelPZ(tf2zpk(ntf_x[0] + 1j*ntf_y[0], ntf_x[1]))
intf = cancelPZ(tf2zpk(intf_x[0] + 1j*intf_y[0], intf_x[1]))
stf = cancelPZ(tf2zpk(stf_x[0] + 1j*stf_y[0], ntf_x[1]))
istf = cancelPZ(tf2zpk(istf_x[0] + 1j*istf_y[0], intf_x[1]))
#warning('on')
return ntf, stf, intf, istf
def check_matrix_shapes(self, p, q, r):
ss2tf(np.zeros((p, p)), np.zeros((p, q)), np.zeros((r, p)),
np.zeros((r, q)), 0)
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
#Coded by SRIKANTH #15th April, 2020 #Released under GNU GPL import numpy as np import matplotlib.pyplot as plt from scipy.signal import ss2tf A = [[-4, -1.5], [4, 0]] B = [[4], [0]] # 2-dimensional column vector C = [[1.5, 0.625]] # 2-dimensional row vector D = [[0]] ss2tf(A, B, C, D) sys = ss2tf(A, B, C, D) print(sys)
show() # ## Geração da Função de Transferência # # Devido algumas diferenças toleráveis na computação de valores muito grandes os arrays com numeradores e o denominadores gerados foram **arredondados para 6 casas decimais**. Isto deixa mais clara a representação da função de transferência. # In[22]: from scipy.signal import ss2tf from control import tf from numpy import around # In[23]: (num, den) = ss2tf(Ap, Bp, Cp, Ep, 0) n_of_decimals = 6 num = around(num, n_of_decimals) den = around(den, n_of_decimals) Gi_vg = tf(num[0], den) Gv_vg = tf(num[1], den) # In[24]: print(Gi_vg) print(Gv_vg) # In[25]:
def _get_num_den(arg, input=0):
"""Utility method to convert the input arg to a (num, den) representation.
**Parameters:**
arg, which may be:
* ZPK tuple,
* num, den tuple,
* A, B, C, D tuple,
* a scipy LTI object,
* a sequence of the tuples of any of the above types.
input : scalar
In case the system has multiple inputs, which input is to be
considered. Input `0` means first input, and so on.
**Returns:**
The sequence of ndarrays num, den
**Raises:**
TypeError, ValueError
.. warn: support for MISO transfer functions is experimental.
"""
num, den = None, None
if isinstance(arg, np.ndarray):
# ABCD matrix
A, B, C, D = partitionABCD(arg)
num, den = ss2tf(A, B, C, D, input=input)
elif hasattr(arg, '__class__') and arg.__class__.__name__ == 'lti':
num, den = arg.num, arg.den
elif _is_num_den(arg):
num, den = carray(arg[0]).squeeze(), carray(arg[1]).squeeze()
elif _is_zpk(arg):
num, den = zpk2tf(*arg)
elif _is_A_B_C_D(arg):
num, den = ss2tf(*arg, input=input)
elif isinstance(arg, collections.Iterable):
ri = 0
for i in arg:
# Note we do not check if the user has assembled a list with
# mismatched representations.
if hasattr(i, 'B'): # lti
iis = i.B.shape[1]
if input < ri + iis:
num, den = ss2tf(i.A, i.B, i.C, i.D, input=input - ri)
break
else:
ri += iis
else:
sys = lti(*i)
iis = sys.B.shape[1]
if input < ri + iis:
num, den = ss2tf(
sys.A, sys.B, sys.C, sys.D, input=input - ri)
break
else:
ri += iis
if (num, den) == (None, None):
raise ValueError("The LTI representation does not have enough" +
"inputs: max %d, looking for input %d" %
(ri - 1, input))
else:
raise TypeError("Unknown LTI representation: %s" % arg)
if len(num.shape) > 1:
num = num.squeeze()
if len(den.shape) > 1:
den = den.squeeze()
# default accuracy: sqrt_ps
sqrt_eps = np.sqrt(eps)
while len(num.shape) and len(num):
if abs(num[0]) < sqrt_eps:
num = num[1:]
else:
break
while len(den.shape) and len(den):
if abs(den[0]) < sqrt_eps:
den = den[1:]
else:
break
den = np.atleast_1d(den)
num = np.atleast_1d(num)
return num, den