Fix bug in dare so that it returns a stabilizing solution

cwrowley · cwrowley · commit 7cf9630146a9 · 2014-08-16T16:34:28.000-04:00
The new implementation calls the routine scipy.linalg.solve_discrete_are(A, B, Q, R) which is included in scipy versions >= 0.11. The old implementation using slycot did satisfy the Riccati equation, but did not return a stabilizing solution. The scipy implementation apparently works correctly, though. This change fixes #8. Unit tests now make sure closed-loop eigenvalues lie inside the unit circle. Note: the scipy implementation handles only the case S = 0, E = I, the default values. If S and E are specified, the old routine (using slycot) is called. This passes the existing tests, but the tests include only one simple case, so it would be good to test this more extensively.
diff --git a/control/mateqn.py b/control/mateqn.py
@@ -42,7 +42,8 @@
 """
 
 from numpy.linalg import inv
-from scipy import shape, size, asarray, copy, zeros, eye, dot
+from scipy import shape, size, asarray, asmatrix, copy, zeros, eye, dot
+from scipy.linalg import eigvals, solve_discrete_are
 from .exception import ControlSlycot, ControlArgument
 
 #### Lyapunov equation solvers lyap and dlyap
@@ -667,7 +668,6 @@ def care(A,B,Q,R=None,S=None,E=None):
     else:
         raise ControlArgument("Invalid set of input parameters.")
 
-
 def dare(A,B,Q,R,S=None,E=None):
     """ (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati
     equation
@@ -688,8 +688,19 @@ def dare(A,B,Q,R,S=None,E=None):
     where A, Q and E are square matrices of the same dimension. Further, Q and
     R are symmetric matrices. The function returns the solution X, the gain
     matrix G = (B^T X B + R)^-1 (B^T X A + S^T) and the closed loop
-    eigenvalues L, i.e., the eigenvalues of A - B G , E. """
-
+    eigenvalues L, i.e., the eigenvalues of A - B G , E.
+    """
+    if S is not None or E is not None:
+        return dare_old(A, B, Q, R, S, E)
+    else:
+        Rmat = asmatrix(R)
+        Qmat = asmatrix(Q)
+        X = solve_discrete_are(A, B, Qmat, Rmat)
+        G = inv(B.T.dot(X).dot(B) + Rmat) * B.T.dot(X).dot(A)
+        L = eigvals(A - B.dot(G))
+        return X, L, G
+
+def dare_old(A,B,Q,R,S=None,E=None):
     # Make sure we can import required slycot routine
     try:
         from slycot import sb02md
diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py
@@ -44,8 +44,9 @@
 
 import unittest
 from numpy import matrix
-from numpy.testing import assert_array_almost_equal
-from numpy.linalg import inv
+from numpy.testing import assert_array_almost_equal, assert_array_less
+# need scipy version of eigvals for generalized eigenvalue problem
+from scipy.linalg import inv, eigvals
 from scipy import zeros,dot
 from control.mateqn import lyap,dlyap,care,dare
 from control.exception import slycot_check
@@ -178,6 +179,9 @@ def test_dare(self):
             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]])
@@ -190,6 +194,9 @@ def test_dare(self):
             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_g(self):
         A = matrix([[-0.6, 0],[-0.1, -0.4]])
@@ -206,6 +213,9 @@ def test_dare_g(self):
             (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]])
@@ -221,6 +231,9 @@ def test_dare_g(self):
             (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 suite():
     return unittest.TestLoader().loadTestsFromTestCase(TestMatrixEquations)
