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