Merge pull request #673 from murrayrm/enh-lqe

bnavigator · web-flow · commit 81a4f38e39ef · 2021-12-02T12:13:12.000+01:00
Improved lqe calling functionality
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -44,8 +44,10 @@
 
 from . import statesp
 from .mateqn import care
-from .statesp import _ssmatrix
-from .exception import ControlSlycot, ControlArgument, ControlDimension
+from .statesp import _ssmatrix, _convert_to_statespace
+from .lti import LTI
+from .exception import ControlSlycot, ControlArgument, ControlDimension, \
+    ControlNotImplemented
 
 # Make sure we have access to the right slycot routines
 try:
@@ -257,8 +259,8 @@ def place_varga(A, B, p, dtime=False, alpha=None):
 
 
 # contributed by Sawyer B. Fuller <minster@uw.edu>
-def lqe(A, G, C, QN, RN, NN=None):
-    """lqe(A, G, C, QN, RN, [, N])
+def lqe(*args, **keywords):
+    """lqe(A, G, C, Q, R, [, N])
 
     Linear quadratic estimator design (Kalman filter) for continuous-time
     systems. Given the system
@@ -270,25 +272,38 @@ def lqe(A, G, C, QN, RN, NN=None):
 
     with unbiased process noise w and measurement noise v with covariances
 
-    .. math::       E{ww'} = QN,    E{vv'} = RN,    E{wv'} = NN
+    .. math::       E{ww'} = Q,    E{vv'} = R,    E{wv'} = N
 
     The lqe() function computes the observer gain matrix L such that the
     stationary (non-time-varying) Kalman filter
 
     .. math:: x_e = A x_e + B u + L(y - C x_e - D u)
 
     produces a state estimate x_e that minimizes the expected squared error
-    using the sensor measurements y. The noise cross-correlation `NN` is
+    using the sensor measurements y. The noise cross-correlation `N` is
     set to zero when omitted.
 
+    The function can be called with either 3, 4, 5, or 6 arguments:
+
+    * ``L, P, E = lqe(sys, Q, R)``
+    * ``L, P, E = lqe(sys, Q, R, N)``
+    * ``L, P, E = lqe(A, G, C, Q, R)``
+    * ``L, P, E = lqe(A, B, C, Q, R, N)``
+
+    where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are
+    2D arrays or matrices of appropriate dimension.
+
     Parameters
     ----------
-    A, G : 2D array_like
-        Dynamics and noise input matrices
-    QN, RN : 2D array_like
+    A, G, C : 2D array_like
+        Dynamics, process noise (disturbance), and output matrices
+    sys : LTI (StateSpace or TransferFunction)
+        Linear I/O system, with the process noise input taken as the system
+        input.
+    Q, R : 2D array_like
         Process and sensor noise covariance matrices
-    NN : 2D array, optional
-        Cross covariance matrix
+    N : 2D array, optional
+        Cross covariance matrix.  Not currently implemented.
 
     Returns
     -------
@@ -326,11 +341,60 @@ def lqe(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)
-    P, E, LT = care(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN)
+    #
+    # Process the arguments and figure out what inputs we received
+    #
+
+    # Get the system description
+    if (len(args) < 3):
+        raise ControlArgument("not enough input arguments")
+
+    try:
+        sys = args[0]           # Treat the first argument as a system
+        if isinstance(sys, LTI):
+            # Convert LTI system to state space
+            sys = _convert_to_statespace(sys)
+
+        # Extract A, G (assume disturbances come through input), and C
+        A = np.array(sys.A, ndmin=2, dtype=float)
+        G = np.array(sys.B, ndmin=2, dtype=float)
+        C = np.array(sys.C, ndmin=2, dtype=float)
+        index = 1
+
+    except AttributeError:
+        # Arguments should be A and B matrices
+        A = np.array(args[0], ndmin=2, dtype=float)
+        G = np.array(args[1], ndmin=2, dtype=float)
+        C = np.array(args[2], ndmin=2, dtype=float)
+        index = 3
+
+    # Get the weighting matrices (converting to matrices, if needed)
+    Q = np.array(args[index], ndmin=2, dtype=float)
+    R = np.array(args[index+1], ndmin=2, dtype=float)
+
+    # Get the cross-covariance matrix, if given
+    if (len(args) > index + 2):
+        N = np.array(args[index+2], ndmin=2, dtype=float)
+        raise ControlNotImplemented("cross-covariance not implemented")
+
+    else:
+        N = np.zeros((Q.shape[0], R.shape[1]))
+
+    # Check dimensions for consistency
+    nstates = A.shape[0]
+    ninputs = G.shape[1]
+    noutputs = C.shape[0]
+    if (A.shape[0] != nstates or A.shape[1] != nstates or
+        G.shape[0] != nstates or C.shape[1] != nstates):
+        raise ControlDimension("inconsistent system dimensions")
+
+    elif (Q.shape[0] != ninputs or Q.shape[1] != ninputs or
+          R.shape[0] != noutputs or R.shape[1] != noutputs or
+          N.shape[0] != ninputs or N.shape[1] != noutputs):
+        raise ControlDimension("incorrect covariance matrix dimensions")
+
+    # P, E, LT = care(A.T, C.T, G @ Q @ G.T, R)
+    P, E, LT = care(A.T, C.T, np.dot(np.dot(G, Q), G.T), R)
     return _ssmatrix(LT.T), _ssmatrix(P), E
 
 
@@ -394,17 +458,17 @@ def lqr(*args, **keywords):
 
     The function can be called with either 3, 4, or 5 arguments:
 
-    * ``lqr(sys, Q, R)``
-    * ``lqr(sys, Q, R, N)``
-    * ``lqr(A, B, Q, R)``
-    * ``lqr(A, B, Q, R, N)``
+    * ``K, S, E = lqr(sys, Q, R)``
+    * ``K, S, E = lqr(sys, Q, R, N)``
+    * ``K, S, E = lqr(A, B, Q, R)``
+    * ``K, S, E = lqr(A, B, Q, R, N)``
 
     where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are
-    2d arrays or matrices of appropriate dimension.
+    2D arrays or matrices of appropriate dimension.
 
     Parameters
     ----------
-    A, B : 2D array
+    A, B : 2D array_like
         Dynamics and input matrices
     sys : LTI (StateSpace or TransferFunction)
         Linear I/O system
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -6,6 +6,7 @@
 import numpy as np
 import pytest
 
+import control as ct
 from control import lqe, pole, rss, ss, tf
 from control.exception import ControlDimension
 from control.mateqn import care, dare
@@ -338,6 +339,39 @@ def testLQR_warning(self):
         with pytest.warns(UserWarning):
             (K, S, E) = lqr(A, B, Q, R, N)
 
+    @slycotonly
+    def test_lqr_call_format(self):
+        # Create a random state space system for testing
+        sys = rss(2, 3, 2)
+
+        # Weighting matrices
+        Q = np.eye(sys.nstates)
+        R = np.eye(sys.ninputs)
+        N = np.zeros((sys.nstates, sys.ninputs))
+
+        # Standard calling format
+        Kref, Sref, Eref = lqr(sys.A, sys.B, Q, R)
+
+        # Call with system instead of matricees
+        K, S, E = lqr(sys, Q, R)
+        np.testing.assert_array_almost_equal(Kref, K)
+        np.testing.assert_array_almost_equal(Sref, S)
+        np.testing.assert_array_almost_equal(Eref, E)
+
+        # Pass a cross-weighting matrix
+        K, S, E = lqr(sys, Q, R, N)
+        np.testing.assert_array_almost_equal(Kref, K)
+        np.testing.assert_array_almost_equal(Sref, S)
+        np.testing.assert_array_almost_equal(Eref, E)
+
+        # Inconsistent system dimensions
+        with pytest.raises(ct.ControlDimension, match="inconsistent system"):
+            K, S, E = lqr(sys.A, sys.C, Q, R)
+
+        # incorrect covariance matrix dimensions
+        with pytest.raises(ct.ControlDimension, match="incorrect weighting"):
+            K, S, E = lqr(sys.A, sys.B, sys.C, R, Q)
+
     def check_LQE(self, L, P, poles, G, QN, RN):
         P_expected = asmatarrayout(np.sqrt(G.dot(QN.dot(G).dot(RN))))
         L_expected = asmatarrayout(P_expected / RN)
@@ -352,6 +386,43 @@ def test_LQE(self, matarrayin):
         L, P, poles = lqe(A, G, C, QN, RN)
         self.check_LQE(L, P, poles, G, QN, RN)
 
+    @slycotonly
+    def test_lqe_call_format(self):
+        # Create a random state space system for testing
+        sys = rss(4, 3, 2)
+
+        # Covariance matrices
+        Q = np.eye(sys.ninputs)
+        R = np.eye(sys.noutputs)
+        N = np.zeros((sys.ninputs, sys.noutputs))
+
+        # Standard calling format
+        Lref, Pref, Eref = lqe(sys.A, sys.B, sys.C, Q, R)
+
+        # Call with system instead of matricees
+        L, P, E = lqe(sys, Q, R)
+        np.testing.assert_array_almost_equal(Lref, L)
+        np.testing.assert_array_almost_equal(Pref, P)
+        np.testing.assert_array_almost_equal(Eref, E)
+
+        # Compare state space and transfer function (SISO only)
+        sys_siso = rss(4, 1, 1)
+        L_ss, P_ss, E_ss = lqe(sys_siso, np.eye(1), np.eye(1))
+        L_tf, P_tf, E_tf = lqe(tf(sys_siso), np.eye(1), np.eye(1))
+        np.testing.assert_array_almost_equal(E_ss, E_tf)
+
+        # Make sure we get an error if we specify N
+        with pytest.raises(ct.ControlNotImplemented):
+            L, P, E = lqe(sys, Q, R, N)
+
+        # Inconsistent system dimensions
+        with pytest.raises(ct.ControlDimension, match="inconsistent system"):
+            L, P, E = lqe(sys.A, sys.C, sys.B, Q, R)
+
+        # incorrect covariance matrix dimensions
+        with pytest.raises(ct.ControlDimension, match="incorrect covariance"):
+            L, P, E = lqe(sys.A, sys.B, sys.C, R, Q)
+
     @slycotonly
     def test_care(self, matarrayin):
         """Test stabilizing and anti-stabilizing feedbacks, continuous"""
