diff --git a/control/statesp.py b/control/statesp.py index e0f82c132..1152b5a7d 100644 --- a/control/statesp.py +++ b/control/statesp.py @@ -607,6 +607,107 @@ def feedback(self, other=1, sign=-1): return StateSpace(A, B, C, D, dt) + def lft(self, other, nu=-1, ny=-1): + """Return the Linear Fractional Transformation. + + A definition of the LFT operator can be found in Appendix A.7, + page 512 in the 2nd Edition, Multivariable Feedback Control by + Sigurd Skogestad. + + An alternative definition can be found here: + https://www.mathworks.com/help/control/ref/lft.html + + Parameters + ---------- + other: LTI + The lower LTI system + ny: int, optional + Dimension of (plant) measurement output. + nu: int, optional + Dimension of (plant) control input. + + """ + other = _convertToStateSpace(other) + # maximal values for nu, ny + if ny == -1: + ny = min(other.inputs, self.outputs) + if nu == -1: + nu = min(other.outputs, self.inputs) + # dimension check + # TODO + + # Figure out the sampling time to use + if (self.dt == None and other.dt != None): + dt = other.dt # use dt from second argument + elif (other.dt == None and self.dt != None) or \ + timebaseEqual(self, other): + dt = self.dt # use dt from first argument + else: + raise ValueError("Systems have different time bases") + + # submatrices + A = self.A + B1 = self.B[:, :self.inputs - nu] + B2 = self.B[:, self.inputs - nu:] + C1 = self.C[:self.outputs - ny, :] + C2 = self.C[self.outputs - ny:, :] + D11 = self.D[:self.outputs - ny, :self.inputs - nu] + D12 = self.D[:self.outputs - ny, self.inputs - nu:] + D21 = self.D[self.outputs - ny:, :self.inputs - nu] + D22 = self.D[self.outputs - ny:, self.inputs - nu:] + + # submatrices + Abar = other.A + Bbar1 = other.B[:, :ny] + Bbar2 = other.B[:, ny:] + Cbar1 = other.C[:nu, :] + Cbar2 = other.C[nu:, :] + Dbar11 = other.D[:nu, :ny] + Dbar12 = other.D[:nu, ny:] + Dbar21 = other.D[nu:, :ny] + Dbar22 = other.D[nu:, ny:] + + # well-posed check + F = np.block([[np.eye(ny), -D22], [-Dbar11, np.eye(nu)]]) + if matrix_rank(F) != ny + nu: + raise ValueError("lft not well-posed to working precision.") + + # solve for the resulting ss by solving for [y, u] using [x, + # xbar] and [w1, w2]. + TH = np.linalg.solve(F, np.block( + [[C2, np.zeros((ny, other.states)), D21, np.zeros((ny, other.inputs - ny))], + [np.zeros((nu, self.states)), Cbar1, np.zeros((nu, self.inputs - nu)), Dbar12]] + )) + T11 = TH[:ny, :self.states] + T12 = TH[:ny, self.states: self.states + other.states] + T21 = TH[ny:, :self.states] + T22 = TH[ny:, self.states: self.states + other.states] + H11 = TH[:ny, self.states + other.states: self.states + other.states + self.inputs - nu] + H12 = TH[:ny, self.states + other.states + self.inputs - nu:] + H21 = TH[ny:, self.states + other.states: self.states + other.states + self.inputs - nu] + H22 = TH[ny:, self.states + other.states + self.inputs - nu:] + + Ares = np.block([ + [A + B2.dot(T21), B2.dot(T22)], + [Bbar1.dot(T11), Abar + Bbar1.dot(T12)] + ]) + + Bres = np.block([ + [B1 + B2.dot(H21), B2.dot(H22)], + [Bbar1.dot(H11), Bbar2 + Bbar1.dot(H12)] + ]) + + Cres = np.block([ + [C1 + D12.dot(T21), D12.dot(T22)], + [Dbar21.dot(T11), Cbar2 + Dbar21.dot(T12)] + ]) + + Dres = np.block([ + [D11 + D12.dot(H21), D12.dot(H22)], + [Dbar21.dot(H11), Dbar22 + Dbar21.dot(H12)] + ]) + return StateSpace(Ares, Bres, Cres, Dres, dt) + def minreal(self, tol=0.0): """Calculate a minimal realization, removes unobservable and uncontrollable states""" diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py index 08a8cdc22..4ef2f9eb9 100644 --- a/control/tests/statesp_test.py +++ b/control/tests/statesp_test.py @@ -448,6 +448,53 @@ def empty(shape): np.testing.assert_array_equal(empty((D.shape[0], 0)), g.C) np.testing.assert_array_equal(D, g.D) + def test_lft(self): + """ test lft function with result obtained from matlab implementation""" + # test case + A = [[1, 2, 3], + [1, 4, 5], + [2, 3, 4]] + B = [[0, 2], + [5, 6], + [5, 2]] + C = [[1, 4, 5], + [2, 3, 0]] + D = [[0, 0], + [3, 0]] + P = StateSpace(A, B, C, D) + Ak = [[0, 2, 3], + [2, 3, 5], + [2, 1, 9]] + Bk = [[1, 1], + [2, 3], + [9, 4]] + Ck = [[1, 4, 5], + [2, 3, 6]] + Dk = [[0, 2], + [0, 0]] + K = StateSpace(Ak, Bk, Ck, Dk) + + # case 1 + pk = P.lft(K, 2, 1) + Amatlab = [1, 2, 3, 4, 6, 12, 1, 4, 5, 17, 38, 61, 2, 3, 4, 9, 26, 37, 2, 3, 0, 3, 14, 18, 4, 6, 0, 8, 27, 35, 18, 27, 0, 29, 109, 144] + Bmatlab = [0, 10, 10, 7, 15, 58] + Cmatlab = [1, 4, 5, 0, 0, 0] + Dmatlab = [0] + np.testing.assert_allclose(np.array(pk.A).reshape(-1), Amatlab) + np.testing.assert_allclose(np.array(pk.B).reshape(-1), Bmatlab) + np.testing.assert_allclose(np.array(pk.C).reshape(-1), Cmatlab) + np.testing.assert_allclose(np.array(pk.D).reshape(-1), Dmatlab) + + # case 2 + pk = P.lft(K) + Amatlab = [1, 2, 3, 4, 6, 12, -3, -2, 5, 11, 14, 31, -2, -3, 4, 3, 2, 7, 0.6, 3.4, 5, -0.6, -0.4, 0, 0.8, 6.2, 10, 0.2, -4.2, -4, 7.4, 33.6, 45, -0.4, -8.6, -3] + Bmatlab = [] + Cmatlab = [] + Dmatlab = [] + np.testing.assert_allclose(np.array(pk.A).reshape(-1), Amatlab) + np.testing.assert_allclose(np.array(pk.B).reshape(-1), Bmatlab) + np.testing.assert_allclose(np.array(pk.C).reshape(-1), Cmatlab) + np.testing.assert_allclose(np.array(pk.D).reshape(-1), Dmatlab) class TestRss(unittest.TestCase): """These are tests for the proper functionality of statesp.rss."""