dare and care now use scipy routines if slycot not available. fixed bug in non-slycot care. lqr, lqe, dlqr, dlqe now get tested without slycot.

sawyerbfuller · murrayrm · commit 0fa431c5ebb3 · 2021-12-26T12:47:49.000-08:00
diff --git a/control/mateqn.py b/control/mateqn.py
@@ -527,7 +527,8 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None):
         :math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0`
 
     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
+    R are symmetric matrices. If R is None, it is set to the identity
+    matrix. The function returns the solution X, the gain
     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.
 
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -403,6 +403,7 @@ def lqe(*args, **keywords):
     P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method)
     return _ssmatrix(LT.T), _ssmatrix(P), E
 
+
 # contributed by Sawyer B. Fuller <minster@uw.edu>
 def dlqe(A, G, C, QN, RN, NN=None):
     """dlqe(A, G, C, QN, RN, [, N])
@@ -473,10 +474,7 @@ def dlqe(A, G, C, QN, RN, NN=None):
     #    NN = np.zeros(QN.size(0),RN.size(1))
     # NG = G @ NN
 
-    # LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN)
-    # P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN)
-    A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2)
-    QN, RN = np.array(QN, ndmin=2), np.array(RN, ndmin=2)
+    A, G, C, QN, RN = map(np.atleast_2d, (A, G, C, QN, RN))
     P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN)
     return _ssmatrix(LT.T), _ssmatrix(P), E
 
@@ -574,7 +572,7 @@ def lqr(*args, **keywords):
 
     See Also
     --------
-    lqe
+    lqe, dlqr, dlqe
 
     Notes
     -----
@@ -622,6 +620,7 @@ def lqr(*args, **keywords):
     X, L, G = care(A, B, Q, R, N, None, method=method)
     return G, X, L
 
+
 def dlqr(*args, **keywords):
     """dlqr(A, B, Q, R[, N])
 
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -166,7 +166,7 @@ def testAcker(self, fixedseed):
                         continue
 
                 # Place the poles at random locations
-                des = rss(states, 1, 1);
+                des = rss(states, 1, 1)
                 poles = pole(des)
 
                 # Now place the poles using acker
@@ -339,6 +339,11 @@ def test_DLQR_3args(self, matarrayin, matarrayout, method):
         K, S, poles = dlqr(dsys, Q, R, method=method)
         self.check_DLQR(K, S, poles, Q, R)
 
+    def test_DLQR_4args(self, matarrayin, matarrayout):
+        A, B, Q, R = (matarrayin([[X]]) for X in [0., 1., 10., 2.])
+        K, S, poles = dlqr(A, B, Q, R)
+        self.check_DLQR(K, S, poles, Q, R)
+
     def test_lqr_badmethod(self):
         A, B, Q, R = 0, 1, 10, 2
         with pytest.raises(ControlArgument, match="Unknown method"):
@@ -469,8 +474,21 @@ def test_lqe_call_format(self):
         with pytest.raises(ct.ControlDimension, match="incorrect covariance"):
             L, P, E = lqe(sys.A, sys.B, sys.C, R, Q)
 
+    def check_DLQE(self, L, P, poles, G, QN, RN):
+        P_expected = asmatarrayout(G.dot(QN).dot(G))
+        L_expected = asmatarrayout(0)
+        poles_expected = -np.squeeze(np.asarray(L_expected))
+        np.testing.assert_array_almost_equal(P, P_expected)
+        np.testing.assert_array_almost_equal(L, L_expected)
+        np.testing.assert_array_almost_equal(poles, poles_expected)
+
+    def test_DLQE(self, matarrayin):
+        A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.])
+        L, P, poles = dlqe(A, G, C, QN, RN)
+        self.check_DLQE(L, P, poles, G, QN, RN)
+
     def test_care(self, matarrayin):
-        """Test stabilizing and anti-stabilizing feedbacks, continuous"""
+        """Test stabilizing feedback, continuous"""
         A = matarrayin(np.diag([1, -1]))
         B = matarrayin(np.identity(2))
         Q = matarrayin(np.identity(2))
@@ -489,7 +507,7 @@ def test_care(self, matarrayin):
                 X, L, G = care(A, B, Q, R, S, E, stabilizing=False)
 
     def test_dare(self, matarrayin):
-        """Test stabilizing and anti-stabilizing feedbacks, discrete"""
+        """Test stabilizing feedback, discrete"""
         A = matarrayin(np.diag([0.5, 2]))
         B = matarrayin(np.identity(2))
         Q = matarrayin(np.identity(2))
