diff --git a/control/robust.py b/control/robust.py index 0b9b98c41..2cc1f9d3d 100644 --- a/control/robust.py +++ b/control/robust.py @@ -171,3 +171,191 @@ def hinfsyn(P,nmeas,ncon): CL = StateSpace(Ac, Bc, Cc, Dc) return K, CL, gam, rcond + + +def _size_as_needed(w, wname, n): + """Convert LTI object to appropriately sized StateSpace object. + + Intended for use in .robust only + + Parameters + ---------- + w: None, 1x1 LTI object, or mxn LTI object + wname: name of w, for error message + n: number of inputs to w + + Returns + ------- + w_: processed weighting function, a StateSpace object: + - if w is None, empty StateSpace object + - if w is scalar, w_ will be w * eye(n) + - otherwise, w as StateSpace object + + Raises + ------ + ValueError + - if w is not None or scalar, and doesn't have n inputs + + See Also + -------- + augw + """ + from . import append, ss + if w is not None: + if not isinstance(w,StateSpace): + w = ss(w) + if 1==w.inputs and 1==w.outputs: + w = append(*(w,)*n) + else: + if w.inputs != n: + msg=("{}: weighting function has {} inputs, expected {}". + format(wname,w.inputs,n)) + raise ValueError(msg) + else: + w = ss([],[],[],[]) + return w + + +def augw(g,w1=None,w2=None,w3=None): + """Augment plant for mixed sensitivity problem. + + Parameters + ---------- + g: LTI object, ny-by-nu + w1: weighting on S; None, scalar, or k1-by-ny LTI object + w2: weighting on KS; None, scalar, or k2-by-nu LTI object + w3: weighting on T; None, scalar, or k3-by-ny LTI object + p: augmented plant; StateSpace object + + If a weighting is None, no augmentation is done for it. At least + one weighting must not be None. + + If a weighting w is scalar, it will be replaced by I*w, where I is + ny-by-ny for w1 and w3, and nu-by-nu for w2. + + Returns + ------- + p: plant augmented with weightings, suitable for submission to hinfsyn or h2syn. + + Raises + ------ + ValueError + - if all weightings are None + + See Also + -------- + h2syn, hinfsyn, mixsyn + """ + + from . import append, ss, connect + + if w1 is None and w2 is None and w3 is None: + raise ValueError("At least one weighting must not be None") + ny = g.outputs + nu = g.inputs + + w1,w2,w3 = [_size_as_needed(w,wname,n) + for w,wname,n in zip((w1,w2,w3), + ('w1','w2','w3'), + (ny,nu,ny))] + + if not isinstance(g,StateSpace): + g = ss(g) + + # w u + # z1 [ w1 | -w1*g ] + # z2 [ 0 | w2 ] + # z3 [ 0 | w3*g ] + # [------+--------- ] + # v [ I | -g ] + + # error summer: inputs are -y and r=w + Ie = ss([],[],[],np.eye(ny)) + # control: needed to "distribute" control input + Iu = ss([],[],[],np.eye(nu)) + + sysall = append(w1,w2,w3,Ie,g,Iu) + + niw1 = w1.inputs + niw2 = w2.inputs + niw3 = w3.inputs + + now1 = w1.outputs + now2 = w2.outputs + now3 = w3.outputs + + q = np.zeros((niw1+niw2+niw3+ny+nu,2)) + q[:,0] = np.arange(1,q.shape[0]+1) + + # Ie -> w1 + q[:niw1,1] = np.arange(1+now1+now2+now3, + 1+now1+now2+now3+niw1) + + # Iu -> w2 + q[niw1:niw1+niw2,1] = np.arange(1+now1+now2+now3+2*ny, + 1+now1+now2+now3+2*ny+niw2) + + # y -> w3 + q[niw1+niw2:niw1+niw2+niw3,1] = np.arange(1+now1+now2+now3+ny, + 1+now1+now2+now3+ny+niw3) + + # -y -> Iy; note the leading - + q[niw1+niw2+niw3:niw1+niw2+niw3+ny,1] = -np.arange(1+now1+now2+now3+ny, + 1+now1+now2+now3+2*ny) + + # Iu -> G + q[niw1+niw2+niw3+ny:niw1+niw2+niw3+ny+nu,1] = np.arange(1+now1+now2+now3+2*ny, + 1+now1+now2+now3+2*ny+nu) + + # input indices: to Ie and Iu + ii = np.hstack((np.arange(1+now1+now2+now3, + 1+now1+now2+now3+ny), + np.arange(1+now1+now2+now3+ny+nu, + 1+now1+now2+now3+ny+nu+nu))) + + # output indices + oi = np.arange(1,1+now1+now2+now3+ny) + + p = connect(sysall,q,ii,oi) + + return p + +def mixsyn(g,w1=None,w2=None,w3=None): + """Mixed-sensitivity H-infinity synthesis. + + mixsyn(g,w1,w2,w3) -> k,cl,info + + Parameters + ---------- + g: LTI; the plant for which controller must be synthesized + w1: weighting on s = (1+g*k)**-1; None, or scalar or k1-by-ny LTI + w2: weighting on k*s; None, or scalar or k2-by-nu LTI + w3: weighting on t = g*k*(1+g*k)**-1; None, or scalar or k3-by-ny LTI + At least one of w1, w2, and w3 must not be None. + + Returns + ------- + k: synthesized controller; StateSpace object + cl: closed system mapping evaluation inputs to evaluation outputs; if p is the augmented plant, with + [z] = [p11 p12] [w], then cl is the system from w->z with u=-k*y. StateSpace object. + [y] [p21 g] [u] + + info: tuple with entries, in order, + gamma: scalar; H-infinity norm of cl + rcond: array; estimates of reciprocal condition numbers + computed during synthesis. See hinfsyn for details + + If a weighting w is scalar, it will be replaced by I*w, where I is + ny-by-ny for w1 and w3, and nu-by-nu for w2. + + See Also + -------- + hinfsyn, augw + """ + nmeas = g.outputs + ncon = g.inputs + p = augw(g,w1,w2,w3) + + k,cl,gamma,rcond=hinfsyn(p,nmeas,ncon) + info = gamma,rcond + return k,cl,info diff --git a/control/statesp.py b/control/statesp.py index 4abc4175f..d47c8be1f 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -69,6 +69,29 @@ __all__ = ['StateSpace', 'ss', 'rss', 'drss', 'tf2ss', 'ssdata'] + +def _matrix(a): + """Wrapper around numpy.matrix that reshapes empty matrices to be 0x0 + + Parameters + ---------- + a: sequence passed to numpy.matrix + + Returns + ------- + am: result of numpy.matrix(a), except if a is empty, am will be 0x0. + + numpy.matrix([]) has size 1x0; for empty StateSpace objects, we + need 0x0 matrices, so use this instead of numpy.matrix in this + module. + """ + from numpy import matrix + am = matrix(a) + if (1,0) == am.shape: + am.shape = (0,0) + return am + + class StateSpace(LTI): """StateSpace(A, B, C, D[, dt]) @@ -130,7 +153,7 @@ def __init__(self, *args): else: raise ValueError("Needs 1 or 4 arguments; received %i." % len(args)) - A, B, C, D = map(matrix, [A, B, C, D]) + A, B, C, D = [_matrix(M) for M in (A, B, C, D)] # TODO: use super here? LTI.__init__(self, inputs=D.shape[1], outputs=D.shape[0], dt=dt) @@ -336,7 +359,7 @@ def __rmul__(self, other): # try to treat this as a matrix try: - X = matrix(other) + X = _matrix(other) C = X * self.C D = X * self.D return StateSpace(self.A, self.B, C, D, self.dt) @@ -727,11 +750,9 @@ def _convertToStateSpace(sys, **kw): # If this is a matrix, try to create a constant feedthrough try: - D = matrix(sys) - outputs, inputs = D.shape - - return StateSpace(0., zeros((1, inputs)), zeros((outputs, 1)), D) - except Exception(e): + D = _matrix(sys) + return StateSpace([], [], [], D) + except Exception as e: print("Failure to assume argument is matrix-like in" \ " _convertToStateSpace, result %s" % e) diff --git a/control/tests/robust_test.py b/control/tests/robust_test.py index d559be448..e948f8e2c 100644 --- a/control/tests/robust_test.py +++ b/control/tests/robust_test.py @@ -42,5 +42,331 @@ def testH2syn(self): np.testing.assert_array_almost_equal(k.C, [[-1]]) np.testing.assert_array_almost_equal(k.D, [[0]]) + +class TestAugw(unittest.TestCase): + "Test control.robust.augw" + + # tolerance for system equality + TOL = 1e-8 + + def siso_almost_equal(self,g,h): + """siso_almost_equal(g,h) -> None + Raises AssertionError if g and h, two SISO LTI objects, are not almost equal""" + from control import tf, minreal + gmh = tf(minreal(g-h,verbose=False)) + if not (gmh.num[0][0]z1 should be w1 + self.siso_almost_equal(w1,p[0,0]) + # w->v should be 1 + self.siso_almost_equal(ss([],[],[],[1]),p[1,0]) + # u->z1 should be -w1*g + self.siso_almost_equal(-w1*g,p[0,1]) + # u->v should be -g + self.siso_almost_equal(-g,p[1,1]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW2(self): + "SISO plant with KS weighting" + from control import augw, ss + g = ss([-1.],[1.],[1.],[1.]) + w2 = ss([-2],[1.],[1.],[2.]) + p = augw(g,w2=w2) + self.assertEqual(2,p.outputs) + self.assertEqual(2,p.inputs) + # w->z2 should be 0 + self.siso_almost_equal(ss([],[],[],0),p[0,0]) + # w->v should be 1 + self.siso_almost_equal(ss([],[],[],[1]),p[1,0]) + # u->z2 should be w2 + self.siso_almost_equal(w2,p[0,1]) + # u->v should be -g + self.siso_almost_equal(-g,p[1,1]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW3(self): + "SISO plant with T weighting" + from control import augw, ss + g = ss([-1.],[1.],[1.],[1.]) + w3 = ss([-2],[1.],[1.],[2.]) + p = augw(g,w3=w3) + self.assertEqual(2,p.outputs) + self.assertEqual(2,p.inputs) + # w->z3 should be 0 + self.siso_almost_equal(ss([],[],[],0),p[0,0]) + # w->v should be 1 + self.siso_almost_equal(ss([],[],[],[1]),p[1,0]) + # u->z3 should be w3*g + self.siso_almost_equal(w3*g,p[0,1]) + # u->v should be -g + self.siso_almost_equal(-g,p[1,1]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSisoW123(self): + "SISO plant with all weights" + from control import augw, ss + g = ss([-1.],[1.],[1.],[1.]) + w1 = ss([-2.],[2.],[1.],[2.]) + w2 = ss([-3.],[3.],[1.],[3.]) + w3 = ss([-4.],[4.],[1.],[4.]) + p = augw(g,w1,w2,w3) + self.assertEqual(4,p.outputs) + self.assertEqual(2,p.inputs) + # w->z1 should be w1 + self.siso_almost_equal(w1,p[0,0]) + # w->z2 should be 0 + self.siso_almost_equal(0,p[1,0]) + # w->z3 should be 0 + self.siso_almost_equal(0,p[2,0]) + # w->v should be 1 + self.siso_almost_equal(ss([],[],[],[1]),p[3,0]) + # u->z1 should be -w1*g + self.siso_almost_equal(-w1*g,p[0,1]) + # u->z2 should be w2 + self.siso_almost_equal(w2,p[1,1]) + # u->z3 should be w3*g + self.siso_almost_equal(w3*g,p[2,1]) + # u->v should be -g + self.siso_almost_equal(-g,p[3,1]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW1(self): + "MIMO plant with S weighting" + from control import augw, ss + g = ss([[-1.,-2],[-3,-4]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]]) + w1 = ss([-2],[2.],[1.],[2.]) + p = augw(g,w1) + self.assertEqual(4,p.outputs) + self.assertEqual(4,p.inputs) + # w->z1 should be diag(w1,w1) + self.siso_almost_equal(w1,p[0,0]) + self.siso_almost_equal(0, p[0,1]) + self.siso_almost_equal(0, p[1,0]) + self.siso_almost_equal(w1,p[1,1]) + # w->v should be I + self.siso_almost_equal(1, p[2,0]) + self.siso_almost_equal(0, p[2,1]) + self.siso_almost_equal(0, p[3,0]) + self.siso_almost_equal(1, p[3,1]) + # u->z1 should be -w1*g + self.siso_almost_equal(-w1*g[0,0],p[0,2]) + self.siso_almost_equal(-w1*g[0,1],p[0,3]) + self.siso_almost_equal(-w1*g[1,0],p[1,2]) + self.siso_almost_equal(-w1*g[1,1],p[1,3]) + # # u->v should be -g + self.siso_almost_equal(-g[0,0],p[2,2]) + self.siso_almost_equal(-g[0,1],p[2,3]) + self.siso_almost_equal(-g[1,0],p[3,2]) + self.siso_almost_equal(-g[1,1],p[3,3]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW2(self): + "MIMO plant with KS weighting" + from control import augw, ss + g = ss([[-1.,-2],[-3,-4]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]]) + w2 = ss([-2],[2.],[1.],[2.]) + p = augw(g,w2=w2) + self.assertEqual(4,p.outputs) + self.assertEqual(4,p.inputs) + # w->z2 should be 0 + self.siso_almost_equal(0, p[0,0]) + self.siso_almost_equal(0, p[0,1]) + self.siso_almost_equal(0, p[1,0]) + self.siso_almost_equal(0, p[1,1]) + # w->v should be I + self.siso_almost_equal(1, p[2,0]) + self.siso_almost_equal(0, p[2,1]) + self.siso_almost_equal(0, p[3,0]) + self.siso_almost_equal(1, p[3,1]) + # u->z2 should be w2 + self.siso_almost_equal(w2, p[0,2]) + self.siso_almost_equal(0, p[0,3]) + self.siso_almost_equal(0, p[1,2]) + self.siso_almost_equal(w2, p[1,3]) + # # u->v should be -g + self.siso_almost_equal(-g[0,0], p[2,2]) + self.siso_almost_equal(-g[0,1], p[2,3]) + self.siso_almost_equal(-g[1,0], p[3,2]) + self.siso_almost_equal(-g[1,1], p[3,3]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW3(self): + "MIMO plant with T weighting" + from control import augw, ss + g = ss([[-1.,-2],[-3,-4]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]]) + w3 = ss([-2],[2.],[1.],[2.]) + p = augw(g,w3=w3) + self.assertEqual(4,p.outputs) + self.assertEqual(4,p.inputs) + # w->z3 should be 0 + self.siso_almost_equal(0, p[0,0]) + self.siso_almost_equal(0, p[0,1]) + self.siso_almost_equal(0, p[1,0]) + self.siso_almost_equal(0, p[1,1]) + # w->v should be I + self.siso_almost_equal(1, p[2,0]) + self.siso_almost_equal(0, p[2,1]) + self.siso_almost_equal(0, p[3,0]) + self.siso_almost_equal(1, p[3,1]) + # u->z3 should be w3*g + self.siso_almost_equal(w3*g[0,0], p[0,2]) + self.siso_almost_equal(w3*g[0,1], p[0,3]) + self.siso_almost_equal(w3*g[1,0], p[1,2]) + self.siso_almost_equal(w3*g[1,1], p[1,3]) + # # u->v should be -g + self.siso_almost_equal(-g[0,0], p[2,2]) + self.siso_almost_equal(-g[0,1], p[2,3]) + self.siso_almost_equal(-g[1,0], p[3,2]) + self.siso_almost_equal(-g[1,1], p[3,3]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testMimoW123(self): + "MIMO plant with all weights" + from control import augw, ss, append + g = ss([[-1.,-2],[-3,-4]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]], + [[1.,0.],[0.,1.]]) + # this should be expaned to w1*I + w1 = ss([-2.],[2.],[1.],[2.]) + # diagonal weighting + w2 = append(ss([-3.],[3.],[1.],[3.]), ss([-4.],[4.],[1.],[4.])) + # full weighting + w3 = ss([[-4.,-5],[-6,-7]], + [[2.,3.],[5.,7.]], + [[11.,13.],[17.,19.]], + [[23.,29.],[31.,37.]]) + p = augw(g,w1,w2,w3) + self.assertEqual(8,p.outputs) + self.assertEqual(4,p.inputs) + # w->z1 should be w1 + self.siso_almost_equal(w1, p[0,0]) + self.siso_almost_equal(0, p[0,1]) + self.siso_almost_equal(0, p[1,0]) + self.siso_almost_equal(w1, p[1,1]) + # w->z2 should be 0 + self.siso_almost_equal(0, p[2,0]) + self.siso_almost_equal(0, p[2,1]) + self.siso_almost_equal(0, p[3,0]) + self.siso_almost_equal(0, p[3,1]) + # w->z3 should be 0 + self.siso_almost_equal(0, p[4,0]) + self.siso_almost_equal(0, p[4,1]) + self.siso_almost_equal(0, p[5,0]) + self.siso_almost_equal(0, p[5,1]) + # w->v should be I + self.siso_almost_equal(1, p[6,0]) + self.siso_almost_equal(0, p[6,1]) + self.siso_almost_equal(0, p[7,0]) + self.siso_almost_equal(1, p[7,1]) + + # u->z1 should be -w1*g + self.siso_almost_equal(-w1*g[0,0], p[0,2]) + self.siso_almost_equal(-w1*g[0,1], p[0,3]) + self.siso_almost_equal(-w1*g[1,0], p[1,2]) + self.siso_almost_equal(-w1*g[1,1], p[1,3]) + # u->z2 should be w2 + self.siso_almost_equal(w2[0,0], p[2,2]) + self.siso_almost_equal(w2[0,1], p[2,3]) + self.siso_almost_equal(w2[1,0], p[3,2]) + self.siso_almost_equal(w2[1,1], p[3,3]) + # u->z3 should be w3*g + w3g = w3*g; + self.siso_almost_equal(w3g[0,0], p[4,2]) + self.siso_almost_equal(w3g[0,1], p[4,3]) + self.siso_almost_equal(w3g[1,0], p[5,2]) + self.siso_almost_equal(w3g[1,1], p[5,3]) + # u->v should be -g + self.siso_almost_equal(-g[0,0], p[6,2]) + self.siso_almost_equal(-g[0,1], p[6,3]) + self.siso_almost_equal(-g[1,0], p[7,2]) + self.siso_almost_equal(-g[1,1], p[7,3]) + + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testErrors(self): + "Error cases handled" + from control import augw,ss + # no weights + g1by1 = ss(-1,1,1,0) + g2by2 = ss(-np.eye(2),np.eye(2),np.eye(2),np.zeros((2,2))) + self.assertRaises(ValueError,augw,g1by1) + # mismatched size of weight and plant + self.assertRaises(ValueError,augw,g1by1,w1=g2by2) + self.assertRaises(ValueError,augw,g1by1,w2=g2by2) + self.assertRaises(ValueError,augw,g1by1,w3=g2by2) + +class TestMixsyn(unittest.TestCase): + "Test control.robust.mixsyn" + # it's a relatively simple wrapper; compare results with augw, hinfsyn + @unittest.skipIf(not slycot_check(), "slycot not installed") + def testSiso(self): + "mixsyn with SISO system" + from control import tf, augw, hinfsyn, mixsyn + from control import ss + # Skogestad+Postlethwaite, Multivariable Feedback Control, 1st Ed., Example 2.11 + s = tf([1, 0], 1) + # plant + g = 200/(10*s+1)/(0.05*s+1)**2 + # sensitivity weighting + M = 1.5 + wb = 10 + A = 1e-4 + w1 = (s/M+wb)/(s+wb*A) + # KS weighting + w2 = tf(1, 1) + + p = augw(g, w1, w2) + kref, clref, gam, rcond = hinfsyn(p, 1, 1) + ktest, cltest, info = mixsyn(g, w1, w2) + # check similar to S+P's example + np.testing.assert_allclose(gam, 1.37, atol = 1e-2) + + # mixsyn is a convenience wrapper around augw and hinfsyn, so + # results will be exactly the same. Given than, use the lazy + # but fragile testing option. + np.testing.assert_allclose(ktest.A, kref.A) + np.testing.assert_allclose(ktest.B, kref.B) + np.testing.assert_allclose(ktest.C, kref.C) + np.testing.assert_allclose(ktest.D, kref.D) + + np.testing.assert_allclose(cltest.A, clref.A) + np.testing.assert_allclose(cltest.B, clref.B) + np.testing.assert_allclose(cltest.C, clref.C) + np.testing.assert_allclose(cltest.D, clref.D) + + np.testing.assert_allclose(gam, info[0]) + + np.testing.assert_allclose(rcond, info[1]) + + if __name__ == "__main__": unittest.main() diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 245b396c6..4c54c137c 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -350,6 +350,28 @@ def test_minrealStaticGain(self): np.testing.assert_array_equal(g1.D, g2.D) + def test_Empty(self): + """Regression: can we create an empty StateSpace object?""" + g1=StateSpace([],[],[],[]) + self.assertEqual(0,g1.states) + self.assertEqual(0,g1.inputs) + self.assertEqual(0,g1.outputs) + + + def test_MatrixToStateSpace(self): + """_convertToStateSpace(matrix) gives ss([],[],[],D)""" + D = np.matrix([[1,2,3],[4,5,6]]) + g = _convertToStateSpace(D) + def empty(shape): + m = np.matrix([]) + m.shape = shape + return m + np.testing.assert_array_equal(empty((0,0)), g.A) + np.testing.assert_array_equal(empty((0,D.shape[1])), g.B) + np.testing.assert_array_equal(empty((D.shape[0],0)), g.C) + np.testing.assert_array_equal(D,g.D) + + class TestRss(unittest.TestCase): """These are tests for the proper functionality of statesp.rss.""" diff --git a/doc/control.rst b/doc/control.rst index 294701f8f..ec35f6626 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -104,6 +104,7 @@ Control system synthesis h2syn hinfsyn lqr + mixsyn place Model simplification tools @@ -125,22 +126,23 @@ Utility functions and conversions .. autosummary:: :toctree: generated/ - unwrap - db2mag - mag2db + augw + canonical_form damp + db2mag isctime isdtime issiso issys - pade - sample_system - canonical_form + mag2db observable_form + pade reachable_form + sample_system ss2tf ssdata tf2ss tfdata timebase timebaseEqual + unwrap diff --git a/examples/robust_mimo.py b/examples/robust_mimo.py new file mode 100644 index 000000000..5270579d8 --- /dev/null +++ b/examples/robust_mimo.py @@ -0,0 +1,180 @@ +"""Demonstrate mixed-sensitivity H-infinity design for a MIMO plant. + +Based on Example 3.8 from Multivariable Feedback Control, Skogestad +and Postlethwaite, 1st Edition. +""" + +import numpy as np +import matplotlib.pyplot as plt + +from control import tf, ss, mixsyn, feedback, step_response + +def weighting(wb,m,a): + """weighting(wb,m,a) -> wf + wb - design frequency (where |wf| is approximately 1) + m - high frequency gain of 1/wf; should be > 1 + a - low frequency gain of 1/wf; should be < 1 + wf - SISO LTI object + """ + s = tf([1,0],[1]) + return (s/m+wb)/(s+wb*a) + + +def plant(): + """plant() -> g + g - LTI object; 2x2 plant with a RHP zero, at s=0.5. + """ + den = [0.2,1.2,1] + gtf=tf([[[1],[1]], + [[2,1],[2]]], + [[den,den], + [den,den]]) + return ss(gtf) + + +# as of this writing (2017-07-01), python-control doesn't have an +# equivalent to Matlab's sigma function, so use a trivial stand-in. +def triv_sigma(g,w): + """triv_sigma(g,w) -> s + g - LTI object, order n + w - frequencies, length m + s - (m,n) array of singular values of g(1j*w)""" + m,p,_ = g.freqresp(w) + sjw = (m * np.exp(1j*p*np.pi/180)).transpose(2,0,1) + sv = np.linalg.svd(sjw,compute_uv=False) + return sv + + +def analysis(): + """Plot open-loop responses for various inputs""" + g=plant() + + t = np.linspace(0,10,101) + _, yu1 = step_response(g,t,input=0) + _, yu2 = step_response(g,t,input=1) + + yu1 = yu1 + yu2 = yu2 + + # linear system, so scale and sum previous results to get the + # [1,-1] response + yuz = yu1 - yu2 + + plt.figure(1) + plt.subplot(1,3,1) + plt.plot(t,yu1[0],label='$y_1$') + plt.plot(t,yu1[1],label='$y_2$') + plt.xlabel('time') + plt.ylabel('output') + plt.ylim([-1.1,2.1]) + plt.legend() + plt.title('o/l response to input [1,0]') + + plt.subplot(1,3,2) + plt.plot(t,yu2[0],label='$y_1$') + plt.plot(t,yu2[1],label='$y_2$') + plt.xlabel('time') + plt.ylabel('output') + plt.ylim([-1.1,2.1]) + plt.legend() + plt.title('o/l response to input [0,1]') + + plt.subplot(1,3,3) + plt.plot(t,yuz[0],label='$y_1$') + plt.plot(t,yuz[1],label='$y_2$') + plt.xlabel('time') + plt.ylabel('output') + plt.ylim([-1.1,2.1]) + plt.legend() + plt.title('o/l response to input [1,-1]') + + +def synth(wb1,wb2): + """synth(wb1,wb2) -> k,gamma + wb1: S weighting frequency + wb2: KS weighting frequency + k: controller + gamma: H-infinity norm of 'design', that is, of evaluation system + with loop closed through design + """ + g = plant() + wu = ss([],[],[],np.eye(2)) + wp1 = ss(weighting(wb=wb1, m=1.5, a=1e-4)) + wp2 = ss(weighting(wb=wb2, m=1.5, a=1e-4)) + wp = wp1.append(wp2) + k,_,info = mixsyn(g,wp,wu) + return k, info.gamma + + +def step_opposite(g,t): + """reponse to step of [-1,1]""" + _, yu1 = step_response(g,t,input=0) + _, yu2 = step_response(g,t,input=1) + return yu1 - yu2 + + +def design(): + """Show results of designs""" + # equal weighting on each output + k1, gam1 = synth(0.25,0.25) + # increase "bandwidth" of output 2 by moving crossover weighting frequency 100 times higher + k2, gam2 = synth(0.25,25) + # now weight output 1 more heavily + # won't plot this one, just want gamma + _, gam3 = synth(25,0.25) + + print('design 1 gamma {:.3g} (Skogestad: 2.80)'.format(gam1)) + print('design 2 gamma {:.3g} (Skogestad: 2.92)'.format(gam2)) + print('design 3 gamma {:.3g} (Skogestad: 6.73)'.format(gam3)) + + # do the designs + g = plant() + w = np.logspace(-2,2,101) + I = ss([],[],[],np.eye(2)) + s1 = I.feedback(g*k1) + s2 = I.feedback(g*k2) + + # frequency response + sv1 = triv_sigma(s1,w) + sv2 = triv_sigma(s2,w) + + plt.figure(2) + + plt.subplot(1,2,1) + plt.semilogx(w, 20*np.log10(sv1[:,0]), label=r'$\sigma_1(S_1)$') + plt.semilogx(w, 20*np.log10(sv1[:,1]), label=r'$\sigma_2(S_1)$') + plt.semilogx(w, 20*np.log10(sv2[:,0]), label=r'$\sigma_1(S_2)$') + plt.semilogx(w, 20*np.log10(sv2[:,1]), label=r'$\sigma_2(S_2)$') + plt.ylim([-60,10]) + plt.ylabel('magnitude [dB]') + plt.xlim([1e-2,1e2]) + plt.xlabel('freq [rad/s]') + plt.legend() + plt.title('Singular values of S') + + # time response + + # in design 1, both outputs have an inverse initial response; in + # design 2, output 2 does not, and is very fast, while output 1 + # has a larger initial inverse response than in design 1 + time = np.linspace(0,10,301) + t1 = (g*k1).feedback(I) + t2 = (g*k2).feedback(I) + + y1 = step_opposite(t1,time) + y2 = step_opposite(t2,time) + + plt.subplot(1,2,2) + plt.plot(time, y1[0], label='des. 1 $y_1(t))$') + plt.plot(time, y1[1], label='des. 1 $y_2(t))$') + plt.plot(time, y2[0], label='des. 2 $y_1(t))$') + plt.plot(time, y2[1], label='des. 2 $y_2(t))$') + plt.xlabel('time [s]') + plt.ylabel('response [1]') + plt.legend() + plt.title('c/l response to reference [1,-1]') + + +analysis() +design() +plt.show() diff --git a/examples/robust_siso.py b/examples/robust_siso.py new file mode 100644 index 000000000..39945919c --- /dev/null +++ b/examples/robust_siso.py @@ -0,0 +1,102 @@ +"""Demonstrate mixed-sensitivity H-infinity design for a SISO plant. + +Based on Example 2.11 from Multivariable Feedback Control, Skogestad +and Postlethwaite, 1st Edition. +""" + +import numpy as np +import matplotlib.pyplot as plt + +from control import tf, ss, mixsyn, feedback, step_response + +s = tf([1, 0], 1) +# the plant +g = 200/(10*s+1)/(0.05*s+1)**2 +# disturbance plant +gd = 100/(10*s+1) + +# first design +# sensitivity weighting +M = 1.5 +wb = 10 +A = 1e-4 +ws1 = (s/M+wb)/(s+wb*A) +# KS weighting +wu = tf(1, 1) + +k1, cl1, info1 = mixsyn(g, ws1, wu) + +# sensitivity (S) and complementary sensitivity (T) functions for +# design 1 +s1 = feedback(1,g*k1) +t1 = feedback(g*k1,1) + +# second design +# this weighting differs from the text, where A**0.5 is used; if you use that, +# the frequency response doesn't match the figure. The time responses +# are similar, though. +ws2 = (s/M**0.5+wb)**2/(s+wb*A)**2 +# the KS weighting is the same as for the first design + +k2, cl2, info2 = mixsyn(g, ws2, wu) + +# S and T for design 2 +s2 = feedback(1,g*k2) +t2 = feedback(g*k2,1) + +# frequency response +omega = np.logspace(-2,2,101) +ws1mag,_,_ = ws1.freqresp(omega) +s1mag,_,_ = s1.freqresp(omega) +ws2mag,_,_ = ws2.freqresp(omega) +s2mag,_,_ = s2.freqresp(omega) + +plt.figure(1) +# text uses log-scaled absolute, but dB are probably more familiar to most control engineers +plt.semilogx(omega,20*np.log10(s1mag.flat),label='$S_1$') +plt.semilogx(omega,20*np.log10(s2mag.flat),label='$S_2$') +# -1 in logspace is inverse +plt.semilogx(omega,-20*np.log10(ws1mag.flat),label='$1/w_{P1}$') +plt.semilogx(omega,-20*np.log10(ws2mag.flat),label='$1/w_{P2}$') + +plt.ylim([-80,10]) +plt.xlim([1e-2,1e2]) +plt.xlabel('freq [rad/s]') +plt.ylabel('mag [dB]') +plt.legend() +plt.title('Sensitivity and sensitivity weighting frequency responses') + +# time response +time = np.linspace(0,3,201) +_,y1 = step_response(t1, time) +_,y2 = step_response(t2, time) + +# gd injects into the output (that is, g and gd are summed), and the +# closed loop mapping from output disturbance->output is S. +_,y1d = step_response(s1*gd, time) +_,y2d = step_response(s2*gd, time) + +plt.figure(2) +plt.subplot(1,2,1) +plt.plot(time,y1,label='$y_1(t)$') +plt.plot(time,y2,label='$y_2(t)$') + +plt.ylim([-0.1,1.5]) +plt.xlim([0,3]) +plt.xlabel('time [s]') +plt.ylabel('signal [1]') +plt.legend() +plt.title('Tracking response') + +plt.subplot(1,2,2) +plt.plot(time,y1d,label='$y_1(t)$') +plt.plot(time,y2d,label='$y_2(t)$') + +plt.ylim([-0.1,1.5]) +plt.xlim([0,3]) +plt.xlabel('time [s]') +plt.ylabel('signal [1]') +plt.legend() +plt.title('Disturbance response') + +plt.show() pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy