add method='scipy' to lqr()

murrayrm · murrayrm · commit 91a0455bc86d · 2021-12-26T06:30:19.000-08:00
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -475,6 +475,10 @@ def lqr(*args, **keywords):
         State and input weight matrices
     N : 2D array, optional
         Cross weight matrix
+    method : str, optional
+        Set the method used for computing the result.  Current methods are
+        'slycot' and 'scipy'.  If set to None (default), try 'slycot' first
+        and then 'scipy'.
 
     Returns
     -------
@@ -498,14 +502,23 @@ def lqr(*args, **keywords):
     --------
     >>> K, S, E = lqr(sys, Q, R, [N])
     >>> K, S, E = lqr(A, B, Q, R, [N])
+
     """
 
-    # Make sure that SLICOT is installed
-    try:
-        from slycot import sb02md
-        from slycot import sb02mt
-    except ImportError:
-        raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")
+    # Figure out what method to use
+    method = keywords.get('method', None)
+    if method == 'slycot' or method is None:
+        # Make sure that SLICOT is installed
+        try:
+            from slycot import sb02md
+            from slycot import sb02mt
+            method = 'slycot'
+        except ImportError:
+            if method == 'slycot':
+                raise ControlSlycot(
+                    "can't find slycot module 'sb02md' or 'sb02nt'")
+            else:
+                method = 'scipy'
 
     #
     # Process the arguments and figure out what inputs we received
@@ -546,18 +559,28 @@ def lqr(*args, **keywords):
           N.shape[0] != nstates or N.shape[1] != ninputs):
         raise ControlDimension("incorrect weighting matrix dimensions")
 
-    # Compute the G matrix required by SB02MD
-    A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = \
-        sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')
+    if method == 'slycot':
+        # Compute the G matrix required by SB02MD
+        A_b, B_b, Q_b, R_b, L_b, ipiv, oufact, G = \
+            sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N')
 
-    # Call the SLICOT function
-    X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'C')
+        # Call the SLICOT function
+        X, rcond, w, S, U, A_inv = sb02md(nstates, A_b, G, Q_b, 'C')
 
-    # Now compute the return value
-    # We assume that R is positive definite and, hence, invertible
-    K = np.linalg.solve(R, B.T @ X + N.T)
-    S = X
-    E = w[0:nstates]
+        # Now compute the return value
+        # We assume that R is positive definite and, hence, invertible
+        K = np.linalg.solve(R, np.dot(B.T, X) + N.T)
+        S = X
+        E = w[0:nstates]
+
+    elif method == 'scipy':
+        import scipy as sp
+        S = sp.linalg.solve_continuous_are(A, B, Q, R, s=N)
+        K = np.linalg.solve(R, B.T @ S + N.T)
+        E, _ = np.linalg.eig(A - B @ K)
+
+    else:
+        raise ValueError("unknown method: %s" % method)
 
     return _ssmatrix(K), _ssmatrix(S), E
 
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -8,7 +8,7 @@
 
 import control as ct
 from control import lqe, pole, rss, ss, tf
-from control.exception import ControlDimension
+from control.exception import ControlDimension, ControlSlycot, slycot_check
 from control.mateqn import care, dare
 from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker
 from control.tests.conftest import (slycotonly, check_deprecated_matrix,
@@ -306,19 +306,34 @@ def check_LQR(self, K, S, poles, Q, R):
         np.testing.assert_array_almost_equal(poles, poles_expected)
 
 
-    @slycotonly
-    def test_LQR_integrator(self, matarrayin, matarrayout):
+    @pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
+    def test_LQR_integrator(self, matarrayin, matarrayout, method):
+        if method == 'slycot' and not slycot_check():
+            return
         A, B, Q, R = (matarrayin([[X]]) for X in [0., 1., 10., 2.])
-        K, S, poles = lqr(A, B, Q, R)
+        K, S, poles = lqr(A, B, Q, R, method=method)
         self.check_LQR(K, S, poles, Q, R)
 
-    @slycotonly
-    def test_LQR_3args(self, matarrayin, matarrayout):
+    @pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
+    def test_LQR_3args(self, matarrayin, matarrayout, method):
+        if method == 'slycot' and not slycot_check():
+            return
         sys = ss(0., 1., 1., 0.)
         Q, R = (matarrayin([[X]]) for X in [10., 2.])
-        K, S, poles = lqr(sys, Q, R)
+        K, S, poles = lqr(sys, Q, R, method=method)
         self.check_LQR(K, S, poles, Q, R)
 
+    def test_lqr_badmethod(self):
+        A, B, Q, R = 0, 1, 10, 2
+        with pytest.raises(ValueError, match="unknown"):
+            K, S, poles = lqr(A, B, Q, R, method='nosuchmethod')
+
+    def test_lqr_slycot_not_installed(self):
+        A, B, Q, R = 0, 1, 10, 2
+        if not slycot_check():
+            with pytest.raises(ControlSlycot, match="can't find slycot"):
+                K, S, poles = lqr(A, B, Q, R, method='slycot')
+
     @slycotonly
     @pytest.mark.xfail(reason="warning not implemented")
     def testLQR_warning(self):
