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())
