Merge pull request #281 from dbacc/master

murrayrm · web-flow · commit 1dd0ca24d9f7 · 2019-04-06T19:27:17.000-07:00
Added option for computing anti-stabilizing solutions of Riccati equations
diff --git a/control/mateqn.py b/control/mateqn.py
@@ -411,7 +411,7 @@ def dlyap(A,Q,C=None,E=None):
 
 #### Riccati equation solvers care and dare
 
-def care(A,B,Q,R=None,S=None,E=None):
+def care(A, B, Q, R=None, S=None, E=None, stabilizing=True):
     """ (X,L,G) = care(A,B,Q,R=None) solves the continuous-time algebraic Riccati
     equation
 
@@ -527,7 +527,11 @@ def care(A,B,Q,R=None,S=None,E=None):
             raise e
 
         try:
-            X,rcond,w,S_o,U,A_inv = sb02md(n,A,G,Q,'C')
+            if stabilizing:
+                sort = 'S'
+            else:
+                sort = 'U'
+            X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort)
         except ValueError as ve:
             if ve.info < 0 or ve.info > 5:
                 e = ValueError(ve.message)
@@ -613,8 +617,12 @@ def care(A,B,Q,R=None,S=None,E=None):
         # Solve the generalized algebraic Riccati equation by calling the
         # Slycot function sg02ad
         try:
-            rcondu,X,alfar,alfai,beta,S_o,T,U,iwarn = \
-                    sg02ad('C','B','N','U','N','N','S','R',n,m,0,A,E,B,Q,R,S)
+            if stabilizing:
+                sort = 'S'
+            else:
+                sort = 'U'
+            rcondu, X, alfar, alfai, beta, S_o, T, U, iwarn = \
+                sg02ad('C', 'B', 'N', 'U', 'N', 'N', sort, 'R', n, m, 0, A, E, B, Q, R, S)
         except ValueError as ve:
             if ve.info < 0 or ve.info > 7:
                 e = ValueError(ve.message)
@@ -671,7 +679,7 @@ 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):
+def dare(A, B, Q, R, S=None, E=None, stabilizing=True):
     """ (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati
     equation
 
@@ -692,8 +700,8 @@ def dare(A,B,Q,R,S=None,E=None):
     matrix :math:`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.
     """
-    if S is not None or E is not None:
-        return dare_old(A, B, Q, R, S, E)
+    if S is not None or E is not None or not stabilizing:
+        return dare_old(A, B, Q, R, S, E, stabilizing)
     else:
         Rmat = asmatrix(R)
         Qmat = asmatrix(Q)
@@ -702,7 +710,7 @@ def dare(A,B,Q,R,S=None,E=None):
         L = eigvals(A - B.dot(G))
         return X, L, G
 
-def dare_old(A,B,Q,R,S=None,E=None):
+def dare_old(A, B, Q, R, S=None, E=None, stabilizing=True):
     # Make sure we can import required slycot routine
     try:
         from slycot import sb02md
@@ -795,7 +803,12 @@ def dare_old(A,B,Q,R,S=None,E=None):
             raise e
 
         try:
-            X,rcond,w,S,U,A_inv = sb02md(n,A,G,Q,'D')
+            if stabilizing:
+                sort = 'S'
+            else:
+                sort = 'U'
+
+            X, rcond, w, S, U, A_inv = sb02md(n, A, G, Q, 'D', sort=sort)
         except ValueError as ve:
             if ve.info < 0 or ve.info > 5:
                 e = ValueError(ve.message)
@@ -884,8 +897,12 @@ def dare_old(A,B,Q,R,S=None,E=None):
         # Solve the generalized algebraic Riccati equation by calling the
         # Slycot function sg02ad
         try:
-            rcondu,X,alfar,alfai,beta,S_o,T,U,iwarn = \
-                    sg02ad('D','B','N','U','N','N','S','R',n,m,0,A,E,B,Q,R,S)
+            if stabilizing:
+                sort = 'S'
+            else:
+                sort = 'U'
+            rcondu, X, alfar, alfai, beta, S_o, T, U, iwarn = \
+                sg02ad('D', 'B', 'N', 'U', 'N', 'N', sort, 'R', n, m, 0, A, E, B, Q, R, S)
         except ValueError as ve:
             if ve.info < 0 or ve.info > 7:
                 e = ValueError(ve.message)
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -9,6 +9,7 @@
 from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker
 from control.matlab import *
 from control.exception import slycot_check, ControlDimension
+from control.mateqn import care, dare
 
 class TestStatefbk(unittest.TestCase):
     """Test state feedback functions"""
@@ -298,6 +299,37 @@ def test_LQR_3args(self):
         K, S, poles = lqr(sys, Q, R)
         self.check_LQR(K, S, poles, Q, R)
 
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    def test_care(self):
+        #unit test for stabilizing and anti-stabilizing feedbacks
+        #continuous-time
+
+        A = np.diag([1,-1])
+        B = np.identity(2)
+        Q = np.identity(2)
+        R = np.identity(2)
+        S = 0 * B
+        E = np.identity(2)
+        X, L , G = care(A, B, Q, R, S, E, stabilizing=True)
+        assert np.all(np.real(L) < 0)
+        X, L , G = care(A, B, Q, R, S, E, stabilizing=False)
+        assert np.all(np.real(L) > 0)
+
+    @unittest.skipIf(not slycot_check(), "slycot not installed")
+    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_suite():
    return unittest.TestLoader().loadTestsFromTestCase(TestStatefbk)
 
