added option for computing anti-stabilizing solutions of Riccati equations

Daniel Bankmann · Daniel Bankmann · commit cf77e2350ac0 · 2019-03-21T15:36:18.000+01:00
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)
