add feedfwd keyword arguments (no functionality yet)

murrayrm · murrayrm · commit 8780bdce2615 · 2024-12-06T14:35:38.000-08:00
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -39,24 +39,25 @@
 #
 # $Id$
 
-# External packages and modules
+import warnings
+
 import numpy as np
 import scipy as sp
-import warnings
 
 from . import statesp
-from .mateqn import care, dare, _check_shape
-from .statesp import StateSpace, _ssmatrix, _convert_to_statespace, ss
+from .config import _process_legacy_keyword
+from .exception import ControlArgument, ControlDimension, \
+    ControlNotImplemented, ControlSlycot
+from .iosys import _process_indices, _process_labels, isctime, isdtime
 from .lti import LTI
-from .iosys import isdtime, isctime, _process_indices, _process_labels
+from .mateqn import _check_shape, care, dare
 from .nlsys import NonlinearIOSystem, interconnect
-from .exception import ControlSlycot, ControlArgument, ControlDimension, \
-    ControlNotImplemented
-from .config import _process_legacy_keyword
+from .statesp import StateSpace, _convert_to_statespace, _ssmatrix, ss
 
 # Make sure we have access to the right slycot routines
 try:
     from slycot import sb03md57
+
     # wrap without the deprecation warning
     def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None):
         ret = sb03md57(A, U, C, dico, job, fact, trana, ldwork)
@@ -581,8 +582,9 @@ def dlqr(*args, **kwargs):
 
 # Function to create an I/O sytems representing a state feedback controller
 def create_statefbk_iosystem(
-        sys, gain, integral_action=None, estimator=None, controller_type=None,
-        xd_labels=None, ud_labels=None, gainsched_indices=None,
+        sys, gain, feedfwd_gain=None, integral_action=None, estimator=None,
+        controller_type=None, xd_labels=None, ud_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):
     r"""Create an I/O system using a (full) state feedback controller.
@@ -592,7 +594,7 @@ def create_statefbk_iosystem(
 
     .. math:: u = u_d - K_p (x - x_d) - K_i \int(C x - C x_d)
 
-    It can be called in the form::
+    by calling
 
         ctrl, clsys = ct.create_statefbk_iosystem(sys, K)
 
@@ -608,6 +610,18 @@ def create_statefbk_iosystem(
 
     where :math:`\mu` represents the scheduling variable.
 
+    Alternatively, a control of the form
+
+    .. math:: u = k_f r - K_p x - K_i \int(C x - r)
+
+    can be created by calling
+
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            sys, K, kf, feedfwd_pattern='refgain')
+
+    In either form, an estimator can also be used to compute the estimated
+    state from the input and output measurements.
+
     Parameters
     ----------
     sys : NonlinearIOSystem
@@ -640,6 +654,15 @@ def create_statefbk_iosystem(
         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.
+
     integral_action : ndarray, optional
         If this keyword is specified, the controller can include integral
         action in addition to state feedback.  The value of the
@@ -841,20 +864,26 @@ def create_statefbk_iosystem(
         raise ControlArgument(f"unknown controller_type '{controller_type}'")
 
     # Figure out the labels to use
-    xd_labels = _process_labels(
-        xd_labels, 'xd', ['xd[{i}]'.format(i=i) for i in range(sys_nstates)])
-    ud_labels = _process_labels(
-        ud_labels, 'ud', ['ud[{i}]'.format(i=i) for i in range(sys_ninputs)])
-
-    # Create the signal and system names
-    if inputs is None:
-        inputs = xd_labels + ud_labels + estimator.output_labels
+    if feedfwd_pattern == 'trajgen':
+        xd_labels = _process_labels(xd_labels, 'xd', [
+            'xd[{i}]'.format(i=i) for i in range(sys_nstates)])
+        ud_labels = _process_labels(ud_labels, 'ud', [
+            'ud[{i}]'.format(i=i) for i in range(sys_ninputs)])
+
+        # Create the signal and system names
+        if inputs is None:
+            inputs = xd_labels + ud_labels + estimator.output_labels
+    elif feedfwd_pattern == 'refgain':
+        raise NotImplementedError("reference gain pattern not yet implemented")
+    else:
+        raise NotImplementedError(f"unknown pattern '{feedfwd_pattern}'")
+
     if outputs is None:
         outputs = [sys.input_labels[i] for i in control_indices]
     if states is None:
         states = nintegrators
 
-    # Process gainscheduling variables, if present
+    # Process gain scheduling variables, if present
     if gainsched:
         # Create a copy of the scheduling variable indices (default = xd)
         gainsched_indices = _process_indices(
@@ -897,7 +926,7 @@ def _compute_gain(mu):
             return K
 
     # Define the controller system
-    if controller_type == 'nonlinear':
+    if controller_type == 'nonlinear' and feedfwd_pattern == 'trajgen':
         # Create an I/O system for the state feedback gains
         def _control_update(t, states, inputs, params):
             # Split input into desired state, nominal input, and current state
@@ -931,7 +960,7 @@ def _control_output(t, states, inputs, params):
             _control_update, _control_output, name=name, inputs=inputs,
             outputs=outputs, states=states, params=params)
 
-    elif controller_type == 'iosystem':
+    elif controller_type == 'iosystem' and feedfwd_pattern == 'trajgen':
         # Use the passed system to compute feedback compensation
         def _control_update(t, states, inputs, params):
             # Split input into desired state, nominal input, and current state
@@ -955,7 +984,7 @@ def _control_output(t, states, inputs, params):
             _control_update, _control_output, name=name, inputs=inputs,
             outputs=outputs, states=fbkctrl.state_labels, dt=fbkctrl.dt)
 
-    elif controller_type == 'linear' or controller_type is None:
+    elif controller_type in 'linear' and feedfwd_pattern == 'trajgen':
         # Create the matrices implementing the controller
         if isctime(sys):
             # Continuous time: integrator
@@ -973,6 +1002,12 @@ def _control_output(t, states, inputs, params):
             A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name,
             inputs=inputs, outputs=outputs, states=states)
 
+    elif feedfwd_pattern == 'refgain':
+        if controller_type not in ['linear', 'iosystem']:
+            raise ControlArgument(
+                "refgain design pattern only supports linear controllers")
+        raise NotImplementedError("reference gain pattern not yet implemented")
+
     else:
         raise ControlArgument(f"unknown controller_type '{controller_type}'")
 
@@ -1020,7 +1055,7 @@ def ctrb(A, B, t=None):
     bmat = _ssmatrix(B)
     n = np.shape(amat)[0]
     m = np.shape(bmat)[1]
-    
+
     if t is None or t > n:
         t = n
 
@@ -1042,7 +1077,7 @@ def obsv(A, C, t=None):
         Dynamics and output matrix of the system
     t : None or integer
         maximum time horizon of the controllability matrix, max = A.shape[0]
-        
+
     Returns
     -------
     O : 2D array (or matrix)
@@ -1062,14 +1097,14 @@ def obsv(A, C, t=None):
     cmat = _ssmatrix(C)
     n = np.shape(amat)[0]
     p = np.shape(cmat)[0]
-    
+
     if t is None or t > n:
         t = n
 
     # Construct the observability matrix
     obsv = np.zeros((t * p, n))
     obsv[:p, :] = cmat
-        
+
     for k in range(1, t):
         obsv[k * p:(k + 1) * p, :] = np.dot(obsv[(k - 1) * p:k * p, :], amat)
 
