initial implementation, docstrings for refgain pattern

murrayrm · murrayrm · commit 9ed48173b1b4 · 2024-12-06T14:45:22.000-08:00
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -583,7 +583,7 @@ def dlqr(*args, **kwargs):
 # Function to create an I/O sytems representing a state feedback controller
 def create_statefbk_iosystem(
         sys, gain, feedfwd_gain=None, integral_action=None, estimator=None,
-        controller_type=None, xd_labels=None, ud_labels=None,
+        controller_type=None, xd_labels=None, ud_labels=None, ref_labels=None,
         feedfwd_pattern='trajgen', gainsched_indices=None,
         gainsched_method='linear', control_indices=None, state_indices=None,
         name=None, inputs=None, outputs=None, states=None, **kwargs):
@@ -645,24 +645,15 @@ def create_statefbk_iosystem(
         If an I/O system is given, the error e = x - xd is passed to the
         system and the output is used as the feedback compensation term.
 
-    xd_labels, ud_labels : str or list of str, optional
-        Set the name of the signals to use for the desired state and
-        inputs.  If a single string is specified, it should be a format
-        string using the variable `i` as an index.  Otherwise, a list of
-        strings matching the size of `x_d` and `u_d`, respectively, should
-        be used.  Default is "xd[{i}]" for xd_labels and "ud[{i}]" for
-        ud_labels.  These settings can also be overridden using the
-        `inputs` keyword.
-
-    feedfwd_pattern : str, optional
-        If set to 'refgain', the reference gain design pattern is used to
-        create the controller instead of the trajectory generation pattern.
-
     feedfwd_gain : array_like, optional
         Specify the feedforward gain, `k_f`.  Used only for the reference
         gain design pattern.  If not given and if `sys` is a `StateSpace`
         (linear) system, will be computed as -1/(C (A-BK)^{-1}) B.
 
+    feedfwd_pattern : str, optional
+        If set to 'refgain', the reference gain design pattern is used to
+        create the controller instead of the trajectory generation pattern.
+
     integral_action : ndarray, optional
         If this keyword is specified, the controller can include integral
         action in addition to state feedback.  The value of the
@@ -703,17 +694,19 @@ def create_statefbk_iosystem(
     Returns
     -------
     ctrl : NonlinearIOSystem
-        Input/output system representing the controller.  This system
-        takes as inputs the desired state `x_d`, the desired input
-        `u_d`, and either the system state `x` or the estimated state
-        `xhat`.  It outputs the controller action `u` according to the
-        formula `u = u_d - K(x - x_d)`.  If the keyword
-        `integral_action` is specified, then an additional set of
-        integrators is included in the control system (with the gain
-        matrix `K` having the integral gains appended after the state
-        gains).  If a gain scheduled controller is specified, the gain
-        (proportional and integral) are evaluated using the scheduling
-        variables specified by `gainsched_indices`.
+        Input/output system representing the controller.  For the 'trajgen'
+        design patter (default), this system takes as inputs the desired
+        state `x_d`, the desired input `u_d`, and either the system state
+        `x` or the estimated state `xhat`.  It outputs the controller
+        action `u` according to the formula `u = u_d - K(x - x_d)`.  For
+        the 'refgain' design patter, the system takes as inputs the
+        reference input `r` and the system or estimated state. If the
+        keyword `integral_action` is specified, then an additional set of
+        integrators is included in the control system (with the gain matrix
+        `K` having the integral gains appended after the state gains).  If
+        a gain scheduled controller is specified, the gain (proportional
+        and integral) are evaluated using the scheduling variables
+        specified by `gainsched_indices`.
 
     clsys : NonlinearIOSystem
         Input/output system representing the closed loop system.  This
@@ -739,6 +732,15 @@ def create_statefbk_iosystem(
         specified as either integer offsets or as estimator/system output
         signal names.  If not specified, defaults to the system states.
 
+    xd_labels, ud_labels, ref_labels : str or list of str, optional
+        Set the name of the signals to use for the desired state and inputs
+        or the reference inputs (for the 'refgain' design pattern).  If a
+        single string is specified, it should be a format string using the
+        variable `i` as an index.  Otherwise, a list of strings matching
+        the size of `x_d` and `u_d`, respectively, should be used.  Default
+        is "xd[{i}]" for xd_labels and "ud[{i}]" for ud_labels.  These
+        settings can also be overridden using the `inputs` keyword.
+
     inputs, outputs, states : str, or list of str, optional
         List of strings that name the individual signals of the transformed
         system.  If not given, the inputs, outputs, and states are the same
@@ -835,14 +837,18 @@ def create_statefbk_iosystem(
         # Check for gain scheduled controller
         if len(gain) != 2:
             raise ControlArgument("gain must be a 2-tuple for gain scheduling")
+        elif feedfwd_pattern != 'trajgen':
+            raise NotImplementedError(
+                "Gain scheduling is not implemented for pattern "
+                f"'{feedfwd_pattern}'")
         gains, points = gain[0:2]
 
         # Stack gains and points if past as a list
         gains = np.stack(gains)
         points = np.stack(points)
         gainsched = True
 
-    elif isinstance(gain, NonlinearIOSystem):
+    elif isinstance(gain, NonlinearIOSystem) and feedfwd_pattern != 'refgain':
         if controller_type not in ['iosystem', None]:
             raise ControlArgument(
                 f"incompatible controller type '{controller_type}'")
@@ -874,7 +880,10 @@ def create_statefbk_iosystem(
         if inputs is None:
             inputs = xd_labels + ud_labels + estimator.output_labels
     elif feedfwd_pattern == 'refgain':
-        raise NotImplementedError("reference gain pattern not yet implemented")
+        ref_labels = _process_labels(ref_labels, 'r', [
+            f'r[{i}]' for i in range(sys_ninputs)])
+        if inputs is None:
+            inputs = ref_labels + estimator.output_labels
     else:
         raise NotImplementedError(f"unknown pattern '{feedfwd_pattern}'")
 
@@ -1006,7 +1015,32 @@ def _control_output(t, states, inputs, params):
         if controller_type not in ['linear', 'iosystem']:
             raise ControlArgument(
                 "refgain design pattern only supports linear controllers")
-        raise NotImplementedError("reference gain pattern not yet implemented")
+
+        if feedfwd_gain is None:
+            raise ControlArgument(
+                "'feedfwd_gain' required for reference gain pattern")
+
+        # Check to make sure the reference gain is valid
+        Kf = np.atleast_2d(feedfwd_gain)
+        if Kf.ndim != 2 or Kf.shape[0] != sys.ninputs or \
+           Kf.shape[1] != sys.ninputs:
+            raise ControlArgument("feedfwd_gain is not the right shape")
+
+        # Create the matrices implementing the controller
+        # [r, x]->[u]: u = k_f r - K_p x - K_i \int(C x - r)
+        if isctime(sys):
+            # Continuous time: integrator
+            A_lqr = np.zeros((C.shape[0], C.shape[0]))
+        else:
+            # Discrete time: summer
+            A_lqr = np.eye(C.shape[0])
+        B_lqr = np.hstack([-np.eye(C.shape[0], sys_ninputs), C])
+        C_lqr = -K[:, sys_nstates:]                     # integral gain (opt)
+        D_lqr = np.hstack([Kf, -K])
+
+        ctrl = ss(
+            A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name,
+            inputs=inputs, outputs=outputs, states=states)
 
     else:
         raise ControlArgument(f"unknown controller_type '{controller_type}'")
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -56,7 +56,7 @@ def testCtrbT(self):
         Wctrue = np.array([[5., 6.], [7., 8.]])
         Wc = ctrb(A, B, t=t)
         np.testing.assert_array_almost_equal(Wc, Wctrue)
-        
+
     def testObsvSISO(self):
         A = np.array([[1., 2.], [3., 4.]])
         C = np.array([[5., 7.]])
@@ -70,7 +70,7 @@ def testObsvMIMO(self):
         Wotrue = np.array([[5., 6.], [7., 8.], [23., 34.], [31., 46.]])
         Wo = obsv(A, C)
         np.testing.assert_array_almost_equal(Wo, Wotrue)
-        
+
     def testObsvT(self):
         A = np.array([[1., 2.], [3., 4.]])
         C = np.array([[5., 6.], [7., 8.]])
@@ -128,15 +128,14 @@ def testGramRc(self):
         C = np.array([[4., 5.], [6., 7.]])
         D = np.array([[13., 14.], [15., 16.]])
         sys = ss(A, B, C, D)
-        Rctrue = np.array([[4.30116263, 5.6961343], 
-                           [0., 0.23249528]])
+        Rctrue = np.array([[4.30116263, 5.6961343], [0., 0.23249528]])
         Rc = gram(sys, 'cf')
         np.testing.assert_array_almost_equal(Rc, Rctrue)
         sysd = ct.c2d(sys, 0.2)
         Rctrue = np.array([[1.91488054, 2.53468814],
                            [0.        , 0.10290372]])
         Rc = gram(sysd, 'cf')
-        np.testing.assert_array_almost_equal(Rc, Rctrue)        
+        np.testing.assert_array_almost_equal(Rc, Rctrue)
 
     @slycotonly
     def testGramWo(self):
@@ -149,7 +148,7 @@ def testGramWo(self):
         Wo = gram(sys, 'o')
         np.testing.assert_array_almost_equal(Wo, Wotrue)
         sysd = ct.c2d(sys, 0.2)
-        Wotrue = np.array([[ 1305.369179, -440.046414], 
+        Wotrue = np.array([[ 1305.369179, -440.046414],
                            [ -440.046414,  333.034844]])
         Wo = gram(sysd, 'o')
         np.testing.assert_array_almost_equal(Wo, Wotrue)
@@ -184,7 +183,7 @@ def testGramRo(self):
         Rotrue = np.array([[ 36.12989315, -12.17956588],
                            [  0.        ,  13.59018097]])
         Ro = gram(sysd, 'of')
-        np.testing.assert_array_almost_equal(Ro, Rotrue) 
+        np.testing.assert_array_almost_equal(Ro, Rotrue)
 
     def testGramsys(self):
         sys = tf([1.], [1., 1., 1.])
@@ -1143,3 +1142,39 @@ def test_gainsched_errors(unicycle):
         ctrl, clsys = ct.create_statefbk_iosystem(
             unicycle, (gains, points),
             gainsched_indices=[3, 2], gainsched_method='unknown')
+
+
+@pytest.mark.parametrize("ninputs, Kf", [
+    (1, 1), (1, None),
+    (2, np.diag([1, 1])), (2, None),
+])
+def test_refgain_pattern(ninputs, Kf):
+    sys = ct.rss(2, 2, ninputs, strictly_proper=True)
+    sys.C = np.eye(2)
+
+    K, _, _ = ct.lqr(sys.A, sys.B, np.eye(sys.nstates), np.eye(sys.ninputs))
+    if Kf is None:
+        # Make sure we get an error if we don't specify Kf
+        with pytest.raises(ControlArgument, match="'feedfwd_gain' required"):
+            ctrl, clsys = ct.create_statefbk_iosystem(
+                sys, K, Kf, feedfwd_pattern='refgain')
+
+        # Now compute the gain to give unity zero frequency gain
+        C = np.eye(ninputs, sys.nstates)
+        Kf = -np.linalg.inv(
+            C @ np.linalg.inv(sys.A - sys.B @ K) @ sys.B)
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            sys, K, Kf, feedfwd_pattern='refgain')
+
+        np.testing.assert_almost_equal(
+            C @ clsys(0)[0:sys.nstates], np.eye(ninputs))
+
+    else:
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            sys, K, Kf, feedfwd_pattern='refgain')
+
+    manual = ct.feedback(sys, K) * Kf
+    np.testing.assert_almost_equal(clsys.A, manual.A)
+    np.testing.assert_almost_equal(clsys.B, manual.B)
+    np.testing.assert_almost_equal(clsys.C[:sys.nstates, :], manual.C)
+    np.testing.assert_almost_equal(clsys.D[:sys.nstates, :], manual.D)
