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