def test_dare(self):
        A = array([[-0.6, 0],[-0.1, -0.4]])
        Q = array([[2, 1],[1, 0]])
        B = array([[2, 1],[0, 1]])
        R = array([[1, 0],[0, 1]])

        X, L, G = dare(A, B, Q, R)
        # print("The solution obtained is", X)
        Gref = solve(B.T @ X @ B + R, B.T @ X @ A)
        assert_array_almost_equal(Gref, G)
        assert_array_almost_equal(
            X, A.T @ X @ A - A.T @ X @ B @ Gref + Q)
        # check for stable closed loop
        lam = eigvals(A - B @ G)
        assert_array_less(abs(lam), 1.0)

        A = array([[1, 0],[-1, 1]])
        Q = array([[0, 1],[1, 1]])
        B = array([[1],[0]])
        R = 2

        X, L, G = dare(A, B, Q, R)
        # print("The solution obtained is", X)
        AtXA = A.T @ X @ A
        AtXB = A.T @ X @ B
        BtXA = B.T @ X @ A
        BtXB = B.T @ X @ B
        assert_array_almost_equal(
            X, AtXA - AtXB @ solve(BtXB + R, BtXA) + Q)
        assert_array_almost_equal(BtXA / (BtXB + R), G)
        # check for stable closed loop
        lam = eigvals(A - B @ G)
        assert_array_less(abs(lam), 1.0)
    def test_dare_g(self):
        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 3]])
        B = array([[1, 5], [2, 4]])
        R = array([[1, 0], [0, 1]])
        S = array([[1, 0], [2, 0]])
        E = array([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-dot(E.T,dot(X,E)) - \
            dot( dot(A.T,dot(X,B))+S , dot( inv(dot(B.T,dot(X,B)) + R) ,
            dot(B.T,dot(X,A))+S.T)) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( inv( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) + S.T ) , G)

        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 3]])
        B = array([[1], [2]])
        R = 1
        S = array([[1], [2]])
        E = array([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-dot(E.T,dot(X,E)) - \
            dot( dot(A.T,dot(X,B))+S , dot( inv(dot(B.T,dot(X,B)) + R) ,
            dot(B.T,dot(X,A))+S.T)) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( 1 / ( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) + S.T ) , G)
Exemple #3
0
    def test_dare(self):
        A = matrix([[-0.6, 0], [-0.1, -0.4]])
        Q = matrix([[2, 1], [1, 0]])
        B = matrix([[2, 1], [0, 1]])
        R = matrix([[1, 0], [0, 1]])

        X, L, G = dare(A, B, Q, R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A -
            X - A.T * X * B * inv(B.T * X * B + R) * B.T * X * A + Q,
            zeros((2, 2)))
        assert_array_almost_equal(inv(B.T * X * B + R) * B.T * X * A, G)
        # check for stable closed loop
        lam = eigvals(A - B * G)
        assert_array_less(abs(lam), 1.0)

        A = matrix([[1, 0], [-1, 1]])
        Q = matrix([[0, 1], [1, 1]])
        B = matrix([[1], [0]])
        R = 2

        X, L, G = dare(A, B, Q, R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A -
            X - A.T * X * B * inv(B.T * X * B + R) * B.T * X * A + Q,
            zeros((2, 2)))
        assert_array_almost_equal(B.T * X * A / (B.T * X * B + R), G)
        # check for stable closed loop
        lam = eigvals(A - B * G)
        assert_array_less(abs(lam), 1.0)
Exemple #4
0
    def test_dare(self):
        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 0]])
        B = array([[2, 1], [0, 1]])
        R = array([[1, 0], [0, 1]])

        X, L, G = dare(A, B, Q, R)
        # print("The solution obtained is", X)
        Gref = solve(B.T.dot(X).dot(B) + R, B.T.dot(X).dot(A))
        assert_array_almost_equal(Gref, G)
        assert_array_almost_equal(
            A.T.dot(X).dot(A) - X - A.T.dot(X).dot(B).dot(Gref) + Q,
            zeros((2, 2)))
        # check for stable closed loop
        lam = eigvals(A - B.dot(G))
        assert_array_less(abs(lam), 1.0)

        A = array([[1, 0], [-1, 1]])
        Q = array([[0, 1], [1, 1]])
        B = array([[1], [0]])
        R = 2

        X, L, G = dare(A, B, Q, R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T.dot(X).dot(A) - X - A.T.dot(X).dot(B) *
            solve(B.T.dot(X).dot(B) + R,
                  B.T.dot(X).dot(A)) + Q, zeros((2, 2)))
        assert_array_almost_equal(
            B.T.dot(X).dot(A) / (B.T.dot(X).dot(B) + R), G)
        # check for stable closed loop
        lam = eigvals(A - B.dot(G))
        assert_array_less(abs(lam), 1.0)
    def test_dare_g(self):
        A = array([[-0.6, 0],[-0.1, -0.4]])
        Q = array([[2, 1],[1, 3]])
        B = array([[1, 5],[2, 4]])
        R = array([[1, 0],[0, 1]])
        S = array([[1, 0],[2, 0]])
        E = array([[2, 1],[1, 2]])

        X,L,G = dare(A,B,Q,R,S,E)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-dot(E.T,dot(X,E)) - \
            dot( dot(A.T,dot(X,B))+S , dot( inv(dot(B.T,dot(X,B)) + R) ,
            dot(B.T,dot(X,A))+S.T)) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( inv( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) + S.T ) , G)

        A = array([[-0.6, 0],[-0.1, -0.4]])
        Q = array([[2, 1],[1, 3]])
        B = array([[1],[2]])
        R = 1
        S = array([[1],[2]])
        E = array([[2, 1],[1, 2]])

        X,L,G = dare(A,B,Q,R,S,E)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-dot(E.T,dot(X,E)) - \
            dot( dot(A.T,dot(X,B))+S , dot( inv(dot(B.T,dot(X,B)) + R) ,
            dot(B.T,dot(X,A))+S.T)) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( 1 / ( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) + S.T ) , G)
    def test_dare(self):
        A = matrix([[-0.6, 0],[-0.1, -0.4]])
        Q = matrix([[2, 1],[1, 0]])
        B = matrix([[2, 1],[0, 1]])
        R = matrix([[1, 0],[0, 1]])

        X,L,G = dare(A,B,Q,R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - X -
            A.T * X * B * solve(B.T * X * B + R, B.T * X * A) + Q, zeros((2,2)))
        assert_array_almost_equal(solve(B.T * X * B + R, B.T * X * A), G)
        # check for stable closed loop
        lam = eigvals(A - B * G)
        assert_array_less(abs(lam), 1.0)

        A = matrix([[1, 0],[-1, 1]])
        Q = matrix([[0, 1],[1, 1]])
        B = matrix([[1],[0]])
        R = 2

        X,L,G = dare(A,B,Q,R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - X -
            A.T * X * B * solve(B.T *  X * B + R, B.T * X * A) + Q, zeros((2,2)))
        assert_array_almost_equal(B.T * X * A / (B.T * X * B + R), G)
        # check for stable closed loop
        lam = eigvals(A - B * G)
        assert_array_less(abs(lam), 1.0)
 def test_dare(self):
     #discrete-time
     A = np.diag([0.5,2])
     B = np.identity(2)
     Q = np.identity(2)
     R = np.identity(2)
     S = 0 * B
     E = np.identity(2)
     X, L , G = dare(A, B, Q, R, S, E, stabilizing=True)
     assert np.all(np.abs(L) < 1)
     X, L , G = dare(A, B, Q, R, S, E, stabilizing=False)
     assert np.all(np.abs(L) > 1)
Exemple #8
0
 def test_dare(self, matarrayin):
     """Test stabilizing and anti-stabilizing feedbacks, discrete"""
     A = matarrayin(np.diag([0.5, 2]))
     B = matarrayin(np.identity(2))
     Q = matarrayin(np.identity(2))
     R = matarrayin(np.identity(2))
     S = matarrayin(np.zeros((2, 2)))
     E = matarrayin(np.identity(2))
     X, L, G = dare(A, B, Q, R, S, E, stabilizing=True)
     assert np.all(np.abs(L) < 1)
     X, L, G = dare(A, B, Q, R, S, E, stabilizing=False)
     assert np.all(np.abs(L) > 1)
 def test_dare(self):
     #discrete-time
     A = np.diag([0.5,2])
     B = np.identity(2)
     Q = np.identity(2)
     R = np.identity(2)
     S = 0 * B
     E = np.identity(2)
     X, L , G = dare(A, B, Q, R, S, E, stabilizing=True)
     assert np.all(np.abs(L) < 1)
     X, L , G = dare(A, B, Q, R, S, E, stabilizing=False)
     assert np.all(np.abs(L) > 1)
    def test_dare_compare(self):
        A = np.array([[-0.6, 0], [-0.1, -0.4]])
        Q = np.array([[2, 1], [1, 0]])
        B = np.array([[2, 1], [0, 1]])
        R = np.array([[1, 0], [0, 1]])
        S = np.zeros((A.shape[0], B.shape[1]))
        E = np.eye(A.shape[0])

        # Solve via scipy
        X_scipy, L_scipy, G_scipy = dare(A, B, Q, R, method='scipy')

        # Solve via slycot
        if ct.slycot_check():
            X_slicot, L_slicot, G_slicot = dare(
                A, B, Q, R, S, E, method='scipy')
            np.testing.assert_almost_equal(X_scipy, X_slicot)
            np.testing.assert_almost_equal(L_scipy, L_slicot)
            np.testing.assert_almost_equal(G_scipy, G_slicot)
    def test_dare(self, matarrayin, stabilizing):
        """Test stabilizing and anti-stabilizing feedback, discrete"""
        A = matarrayin(np.diag([0.5, 2]))
        B = matarrayin(np.identity(2))
        Q = matarrayin(np.identity(2))
        R = matarrayin(np.identity(2))
        S = matarrayin(np.zeros((2, 2)))
        E = matarrayin(np.identity(2))

        X, L, G = dare(A, B, Q, R, S, E, stabilizing=stabilizing)
        sgn = {True: -1, False: 1}[stabilizing]
        assert np.all(sgn * (np.abs(L) - 1) > 0)
Exemple #12
0
    def test_dare_g(self):
        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 3]])
        B = array([[1, 5], [2, 4]])
        R = array([[1, 0], [0, 1]])
        S = array([[1, 0], [2, 0]])
        E = array([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print("The solution obtained is", X)
        Gref = solve(B.T.dot(X).dot(B) + R, B.T.dot(X).dot(A) + S.T)
        assert_array_almost_equal(Gref, G)
        assert_array_almost_equal(
            A.T.dot(X).dot(A) - E.T.dot(X).dot(E) -
            (A.T.dot(X).dot(B) + S).dot(Gref) + Q, zeros((2, 2)))
        # check for stable closed loop
        lam = eigvals(A - B.dot(G), E)
        assert_array_less(abs(lam), 1.0)

        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 3]])
        B = array([[1], [2]])
        R = 1
        S = array([[1], [2]])
        E = array([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T.dot(X).dot(A) - E.T.dot(X).dot(E) -
            (A.T.dot(X).dot(B) + S).dot(
                solve(B.T.dot(X).dot(B) + R,
                      B.T.dot(X).dot(A) + S.T)) + Q, zeros((2, 2)))
        assert_array_almost_equal(
            (B.T.dot(X).dot(A) + S.T) / (B.T.dot(X).dot(B) + R), G)
        # check for stable closed loop
        lam = eigvals(A - B.dot(G), E)
        assert_array_less(abs(lam), 1.0)
Exemple #13
0
    def test_dare_g(self):
        A = matrix([[-0.6, 0], [-0.1, -0.4]])
        Q = matrix([[2, 1], [1, 3]])
        B = matrix([[1, 5], [2, 4]])
        R = matrix([[1, 0], [0, 1]])
        S = matrix([[1, 0], [2, 0]])
        E = matrix([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - E.T * X * E -
            (A.T * X * B + S) * inv(B.T * X * B + R) * (B.T * X * A + S.T) + Q,
            zeros((2, 2)))
        assert_array_almost_equal(
            inv(B.T * X * B + R) * (B.T * X * A + S.T), G)
        # check for stable closed loop
        lam = eigvals(A - B * G, E)
        assert_array_less(abs(lam), 1.0)

        A = matrix([[-0.6, 0], [-0.1, -0.4]])
        Q = matrix([[2, 1], [1, 3]])
        B = matrix([[1], [2]])
        R = 1
        S = matrix([[1], [2]])
        E = matrix([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - E.T * X * E -
            (A.T * X * B + S) * inv(B.T * X * B + R) * (B.T * X * A + S.T) + Q,
            zeros((2, 2)))
        assert_array_almost_equal((B.T * X * A + S.T) / (B.T * X * B + R), G)
        # check for stable closed loop
        lam = eigvals(A - B * G, E)
        assert_array_less(abs(lam), 1.0)
    def test_dare(self):
        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 0]])
        B = array([[2, 1], [0, 1]])
        R = array([[1, 0], [0, 1]])

        X, L, G = dare(A, B, Q, R)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-X-dot(dot(dot(A.T,dot(X,B)) , \
            inv(dot(B.T,dot(X,B))+R)) , dot(B.T,dot(X,A))) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( inv( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) ) , G)

        A = array([[1, 0], [-1, 1]])
        Q = array([[0, 1], [1, 1]])
        B = array([[1], [0]])
        R = 2

        X, L, G = dare(A, B, Q, R)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-X-dot(dot(dot(A.T,dot(X,B)) , \
            inv(dot(B.T,dot(X,B))+R)) , dot(B.T,dot(X,A))) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( 1 / ( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) ) , G)
    def test_dare(self):
        A = array([[-0.6, 0],[-0.1, -0.4]])
        Q = array([[2, 1],[1, 0]])
        B = array([[2, 1],[0, 1]])
        R = array([[1, 0],[0, 1]])

        X,L,G = dare(A,B,Q,R)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-X-dot(dot(dot(A.T,dot(X,B)) , \
            inv(dot(B.T,dot(X,B))+R)) , dot(B.T,dot(X,A))) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( inv( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) ) , G)

        A = array([[1, 0],[-1, 1]])
        Q = array([[0, 1],[1, 1]])
        B = array([[1],[0]])
        R = 2

        X,L,G = dare(A,B,Q,R)
        # print "The solution obtained is", X
        assert_array_almost_equal(dot(A.T,dot(X,A))-X-dot(dot(dot(A.T,dot(X,B)) , \
            inv(dot(B.T,dot(X,B))+R)) , dot(B.T,dot(X,A))) + Q , zeros((2,2)) )
        assert_array_almost_equal( dot( 1 / ( dot(B.T,dot(X,B)) + R) , \
            dot(B.T,dot(X,A)) ) , G)
    def test_dare_g(self):
        A = matrix([[-0.6, 0],[-0.1, -0.4]])
        Q = matrix([[2, 1],[1, 3]])
        B = matrix([[1, 5],[2, 4]])
        R = matrix([[1, 0],[0, 1]])
        S = matrix([[1, 0],[2, 0]])
        E = matrix([[2, 1],[1, 2]])

        X,L,G = dare(A,B,Q,R,S,E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - E.T * X * E -
            (A.T * X * B + S) * solve(B.T * X * B + R, B.T * X * A + S.T) + Q,
            zeros((2,2)) )
        assert_array_almost_equal(solve(B.T * X * B + R, B.T * X * A + S.T), G)
        # check for stable closed loop
        lam = eigvals(A - B * G, E)
        assert_array_less(abs(lam), 1.0)

        A = matrix([[-0.6, 0],[-0.1, -0.4]])
        Q = matrix([[2, 1],[1, 3]])
        B = matrix([[1],[2]])
        R = 1
        S = matrix([[1],[2]])
        E = matrix([[2, 1],[1, 2]])

        X,L,G = dare(A,B,Q,R,S,E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - E.T * X * E -
            (A.T * X * B + S) * solve(B.T * X * B + R, B.T * X * A + S.T) + Q,
            zeros((2,2)) )
        assert_array_almost_equal((B.T * X * A + S.T) / (B.T * X * B + R), G)
        # check for stable closed loop
        lam = eigvals(A - B * G, E)
        assert_array_less(abs(lam), 1.0)
    def test_dare_g(self):
        A = array([[-0.6, 0],[-0.1, -0.4]])
        Q = array([[2, 1],[1, 3]])
        B = array([[1, 5],[2, 4]])
        R = array([[1, 0],[0, 1]])
        S = array([[1, 0],[2, 0]])
        E = array([[2, 1],[1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print("The solution obtained is", X)
        Gref = solve(B.T @ X @ B + R, B.T @ X @ A + S.T)
        assert_array_almost_equal(Gref, G)
        assert_array_almost_equal(
            E.T @ X @ E,
            A.T @ X @ A - (A.T @ X @ B + S) @ Gref + Q)
        # check for stable closed loop
        lam = eigvals(A - B @ G, E)
        assert_array_less(abs(lam), 1.0)
    def test_dare_g2(self):
        A = array([[-0.6, 0], [-0.1, -0.4]])
        Q = array([[2, 1], [1, 3]])
        B = array([[1], [2]])
        R = 1
        S = array([[1], [2]])
        E = array([[2, 1], [1, 2]])

        X, L, G = dare(A, B, Q, R, S, E)
        # print("The solution obtained is", X)
        AtXA = A.T @ X @ A
        AtXB = A.T @ X @ B
        BtXA = B.T @ X @ A
        BtXB = B.T @ X @ B
        EtXE = E.T @ X @ E
        assert_array_almost_equal(
            EtXE, AtXA - (AtXB + S)  @ solve(BtXB + R, BtXA + S.T) + Q)
        assert_array_almost_equal((BtXA + S.T) / (BtXB + R), G)
        # check for stable closed loop
        lam = eigvals(A - B @ G, E)
        assert_array_less(abs(lam), 1.0)
    def test_raise(self):
        """ Test exception raise for invalid inputs """

        # correct shapes and forms
        A = array([[1, 0], [-1, -1]])
        Q = array([[2, 1], [1, 2]])
        C = array([[1, 0], [0, 1]])
        E = array([[2, 1], [1, 2]])

        # these fail
        Afq = array([[1, 0, 0], [-1, -1, 0]])
        Qfq = array([[2, 1, 0], [1, 2, 0]])
        Qfs = array([[2, 1], [-1, 2]])
        Cfd = array([[1, 0, 0], [0, 1, 0]])
        Efq = array([[2, 1, 0], [1, 2, 0]])

        for cdlyap in [lyap, dlyap]:
            with pytest.raises(ControlDimension):
                cdlyap(Afq, Q)
            with pytest.raises(ControlDimension):
                cdlyap(A, Qfq)
            with pytest.raises(ControlArgument):
                cdlyap(A, Qfs)
            with pytest.raises(ControlDimension):
                cdlyap(Afq, Q, C)
            with pytest.raises(ControlDimension):
                cdlyap(A, Qfq, C)
            with pytest.raises(ControlDimension):
                cdlyap(A, Q, Cfd)
            with pytest.raises(ControlDimension):
                cdlyap(A, Qfq, None, E)
            with pytest.raises(ControlDimension):
                cdlyap(A, Q, None, Efq)
            with pytest.raises(ControlArgument):
                cdlyap(A, Qfs, None, E)
            with pytest.raises(ControlArgument):
                cdlyap(A, Q, C, E)

        B = array([[1, 0], [0, 1]])
        Bf = array([[1, 0], [0, 1], [1, 1]])
        R = Q
        Rfs = Qfs
        Rfq = Qfq
        S = array([[0, 0], [0, 0]])
        Sf = array([[0, 0, 0], [0, 0, 0]])
        E = array([[2, 1], [1, 2]])
        Ef = array([[2, 1], [1, 2], [1, 2]])

        with pytest.raises(ControlDimension):
            care(Afq, B, Q)
        with pytest.raises(ControlDimension):
            care(A, B, Qfq)
        with pytest.raises(ControlDimension):
            care(A, Bf, Q)
        with pytest.raises(ControlDimension):
            care(1, B, 1)
        with pytest.raises(ControlArgument):
            care(A, B, Qfs)
        with pytest.raises(ControlArgument):
            dare(A, B, Q, Rfs)
        for cdare in [care, dare]:
            with pytest.raises(ControlDimension):
                cdare(Afq, B, Q, R, S, E)
            with pytest.raises(ControlDimension):
                cdare(A, B, Qfq, R, S, E)
            with pytest.raises(ControlDimension):
                cdare(A, Bf, Q, R, S, E)
            with pytest.raises(ControlDimension):
                cdare(A, B, Q, R, S, Ef)
            with pytest.raises(ControlDimension):
                cdare(A, B, Q, Rfq, S, E)
            with pytest.raises(ControlDimension):
                cdare(A, B, Q, R, Sf, E)
            with pytest.raises(ControlArgument):
                cdare(A, B, Qfs, R, S, E)
            with pytest.raises(ControlArgument):
                cdare(A, B, Q, Rfs, S, E)