python-control · murrayrm · Dec 30, 2022 · Dec 28, 2022 · Dec 28, 2022 · Dec 28, 2022
diff --git a/control/iosys.py b/control/iosys.py
@@ -536,6 +536,10 @@ def linearize(self, x0, u0, t=0, params=None, eps=1e-6,
         # functions.
         #
 
+        # If x0 and u0 are specified as lists, concatenate the elements
+        x0 = _concatenate_list_elements(x0, 'x0')
+        u0 = _concatenate_list_elements(u0, 'u0')
+
         # Figure out dimensions if they were not specified.
         nstates = _find_size(self.nstates, x0)
         ninputs = _find_size(self.ninputs, u0)
@@ -1758,14 +1762,7 @@ def input_output_response(
     ninputs = U.shape[0]
 
     # If we were passed a list of initial states, concatenate them
-    if isinstance(X0, (tuple, list)):
-        X0_list = []
-        for i, x0 in enumerate(X0):
-            x0 = np.array(x0).reshape(-1)       # convert everyting to 1D array
-            X0_list += x0.tolist()              # add elements to initial state
-
-        # Save the newly created input vector
-        X0 = np.array(X0_list)
+    X0 = _concatenate_list_elements(X0, 'X0')
 
     # If the initial state is too short, make it longer (NB: sys.nstates
     # could be None if nstates comes from size of initial condition)
@@ -2377,6 +2374,19 @@ def ss(*args, **kwargs):
     return sys
 
 
+# Utility function to allow lists states, inputs
+def _concatenate_list_elements(X, name='X'):
+    # If we were passed a list, concatenate the elements together
+    if isinstance(X, (tuple, list)):
+        X_list = []
+        for i, x in enumerate(X):
+            x = np.array(x).reshape(-1)         # convert everyting to 1D array
+            X_list += x.tolist()                # add elements to initial state
+        return np.array(X_list)
+
+    # Otherwise, do nothing
+    return X
+
 def rss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs):
     """Create a stable random state space object.
 

diff --git a/control/statefbk.py b/control/statefbk.py
@@ -41,6 +41,7 @@
 
 # External packages and modules
 import numpy as np
+import scipy as sp
 
 from . import statesp
 from .mateqn import care, dare, _check_shape
@@ -71,7 +72,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None):
     sb03od = None
 
 
-__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 
+__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr',
            'dlqr', 'acker', 'create_statefbk_iosystem']
 
 
@@ -600,8 +601,10 @@ def dlqr(*args, **kwargs):
 
 # Function to create an I/O sytems representing a state feedback controller
 def create_statefbk_iosystem(
-        sys, K, integral_action=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]',
-        estimator=None, type='linear'):
+        sys, gain, integral_action=None, estimator=None, type=None,
+        xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None,
+        gainsched_method='linear', name=None, inputs=None, outputs=None,
+        states=None):
     """Create an I/O system using a (full) state feedback controller
 
     This function creates an input/output system that implements a
@@ -617,31 +620,47 @@ def create_statefbk_iosystem(
     feedback gain (eg, from LQR).  The function returns the controller
     ``ctrl`` and the closed loop systems ``clsys``, both as I/O systems.
 
+    A gain scheduled controller can also be created, by passing a list of
+    gains and a corresponding list of values of a set of scheduling
+    variables.  In this case, the controller has the form
+
+        u = ud - K_p(mu) (x - xd) - K_i(mu) integral(C x - C x_d)
+
+    where mu represents the scheduling variable.
+
     Parameters
     ----------
     sys : InputOutputSystem
         The I/O system that represents the process dynamics.  If no estimator
         is given, the output of this system should represent the full state.
 
-    K : ndarray
-        The state feedback gain.  This matrix defines the gains to be
-        applied to the system.  If ``integral_action`` is None, then the
-        dimensions of this array should be (sys.ninputs, sys.nstates).  If
-        `integral action` is set to a matrix or a function, then additional
-        columns represent the gains of the integral states of the
-        controller.
+    gain : ndarray or tuple
+        If a 
8000
array is give, it represents the state feedback gain (K).
+        This matrix defines the gains to be applied to the system.  If
+        ``integral_action`` is None, then the dimensions of this array
+        should be (sys.ninputs, sys.nstates).  If `integral action` is
+        set to a matrix or a function, then additional columns
+        represent the gains of the integral states of the controller.
+
+        If a tuple is given, then it specifies a gain schedule.  The tuple
+        should be of the form ``(gains, points)`` where gains is a list of
+        gains :math:`K_j` and points is a list of values :math:`\\mu_j` at
+        which the gains are computed.  The `gainsched_indices` parameter
+        should be used to specify the scheduling variables.
 
     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 xd and ud, respectively, should be used.  Default is
-        ``'xd[{i}]'`` for xd_labels and ``'xd[{i}]'`` for ud_labels.
+        the variable ``i`` as an index.  Otherwise, a list of strings
+        matching the size of xd and ud, respectively, should be used.
+        Default is ``'xd[{i}]'`` for xd_labels and ``'ud[{i}]'`` for
+        ud_labels.  These settings can also be overriden using the `inputs`
+        keyword.
 
     integral_action : None, ndarray, or func, optional
         If this keyword is specified, the controller can include integral
-        action in addition to state feedback.  If ``integral_action`` is an
-        ndarray, it will be multiplied by the current and desired state to
+        action in addition to state feedback.  If ``integral_action`` is a
+        matrix, it will be multiplied by the current and desired state to
         generate the error for the internal integrator states of the control
         law.  If ``integral_action`` is a function ``h``, that function will
         be called with the signature h(t, x, u, params) to obtain the
@@ -650,30 +669,63 @@ def create_statefbk_iosystem(
         ``K`` matrix.
 
     estimator : InputOutputSystem, optional
-        If an estimator is provided, using the states of the estimator as
+        If an estimator is provided, use the states of the estimator as
         the system inputs for the controller.
 
-    type : 'nonlinear' or 'linear', optional
-        Set the type of controller to create. The default is a linear
-        controller implementing the LQR regulator. If the type is 'nonlinear',
-        a :class:NonlinearIOSystem is created instead, with the gain ``K`` as
-        a parameter (allowing modifications of the gain at runtime).
+    gainsched_indices : list of int or str, optional
+        If a gain scheduled controller is specified, specify the indices of
+        the controller input to use for scheduling the gain. The input to
+        the controller is the desired state xd, the desired input ud, and
+        the system state x (or state estimate xhat, if an estimator is
+        given). The indices can either be specified as integer offsets into
+        the input vector or as strings matching the signal names of the
+        input vector. The default is to use the desire state xd.
+
+    gainsched_method : str, optional
+        The method to use for gain scheduling.  Possible values are 'linear'
+        (default), 'nearest', and 'cubic'.  More information is available in
+        :func:`scipy.interpolate.griddata`. For points outside of the convex
+        hull of the scheduling points, the gain at the nearest point is
+        used.
+
+    type : 'linear' or 'nonlinear', optional
+        Set the type of controller to create. The default for a linear gain
+        is a linear controller implementing the LQR regulator. If the type
+        is 'nonlinear', a :class:NonlinearIOSystem is created instead, with
+        the gain ``K`` as a parameter (allowing modifications of the gain at
+        runtime). If the gain parameter is a tuple, then a nonlinear,
+        gain-scheduled controller is created.
 
     Returns
     -------
     ctrl : InputOutputSystem
         Input/output system representing the controller.  This system takes
-        as inputs the desired state xd, the desired input ud, and the system
-        state x.  It outputs the controller action u according to the
-        formula u = ud - K(x - xd).  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).
+        as inputs the desired state ``xd``, the desired input ``ud``, and
+        either the system state ``x`` or the estimated state ``xhat``.  It
+        outputs the controller action u according to the formula :math:`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``.
 
     clsys : InputOutputSystem
         Input/output system representing the closed loop system.  This
-        systems takes as inputs the desired trajectory (xd, ud) and outputs
-        the system state x and the applied input u (vertically stacked).
+        systems takes as inputs the desired trajectory ``(xd, ud)`` and
+        outputs the system state ``x`` and the applied input ``u``
+        (vertically stacked).
+
+    Other Parameters
+    ----------------
+    inputs, outputs : str, or list of str, optional
+        List of strings that name the individual signals of the transformed
+        system.  If not given, the inputs and outputs are the same as the
+        original system.
+
+    name : string, optional
+        System name. If unspecified, a generic name <sys[id]> is generated
+        with a unique integer id.
 
     """
     # Make sure that we were passed an I/O system as an input
@@ -709,53 +761,127 @@ def create_statefbk_iosystem(
         C = np.zeros((0, sys.nstates))
 
     # Check to make sure that state feedback has the right shape
-    if not isinstance(K, np.ndarray) or \
-       K.shape != (sys.ninputs, estimator.noutputs + nintegrators):
+    if isinstance(gain, np.ndarray):
+        K = gain
+        if K.shape != (sys.ninputs, estimator.noutputs + nintegrators):
+            raise ControlArgument(
+                f'Control gain must be an array of size {sys.ninputs}'
+                f'x {sys.nstates}' +
+                (f'+{nintegrators}' if nintegrators > 0 else ''))
+        gainsched = False
+
+    elif isinstance(gain, tuple):
+        # Check for gain scheduled controller
+        if len(gain) != 2:
+            raise ControlArgument("gain must be a 2-tuple for gain scheduling")
+        gains, points = gain[0:2]
+
+        # Stack gains and points if past as a list
+        gains = np.stack(gains)
+        points = np.stack(points)
+        gainsched=True
+
+    else:
+        raise ControlArgument("gain must be an array or a tuple")
+
+    # Decide on the type of system to create
+    if gainsched and type == 'linear':
         raise ControlArgument(
-            f'Control gain must be an array of size {sys.ninputs}'
-            f'x {sys.nstates}' +
-            (f'+{nintegrators}' if nintegrators > 0 else ''))
+            "type 'linear' not allowed for gain scheduled controller")
+    elif type is None:
+        type = 'nonlinear' if gainsched else 'linear'
+    elif type not in {'linear', 'nonlinear'}:
+        raise ControlArgument(f"unknown type '{type}'")
 
     # Figure out the labels to use
     if isinstance(xd_labels, str):
-        # Gnerate the list of labels using the argument as a format string
+        # Generate the list of labels using the argument as a format string
         xd_labels = [xd_labels.format(i=i) for i in range(sys.nstates)]
 
     if isinstance(ud_labels, str):
-        # Gnerate the list of labels using the argument as a format string
+        # Generate the list of labels using the argument as a format string
         ud_labels = [ud_labels.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 outputs is None:
+        outputs = list(sys.input_index.keys())
+    if states is None:
+        states = nintegrators
+
+    # Process gainscheduling variables, if present
+    if gainsched:
+        # Create a copy of the scheduling variable indices (default = xd)
+        gainsched_indices = range(sys.nstates) if gainsched_indices is None \
+            else list(gainsched_indices)
+
+        # Make sure the scheduling variable indices are the right length
+        if len(gainsched_indices) != points.shape[1]:
+            raise ControlArgument(
+                "length of gainsched_indices must match dimension of"
+                " scheduling variables")
+
+        # Process scheduling variables
+        for i, idx in enumerate(gainsched_indices):
+            if isinstance(idx, str):
+                gainsched_indices[i] = inputs.index(gainsched_indices[i])
+
+        # Create interpolating function
+        if gainsched_method == 'nearest':
+            _interp = sp.interpolate.NearestNDInterpolator(points, gains)
+            def _nearest(mu):
+                raise SystemError(f"could not find nearest gain at mu = {mu}")
+        elif gainsched_method == 'linear':
+            _interp = sp.interpolate.LinearNDInterpolator(points, gains)
+            _nearest = sp.interpolate.NearestNDInterpolator(points, gains)
+        elif gainsched_method == 'cubic':
+            _interp = sp.interpolate.CloughTocher2DInterpolator(points, gains)
+            _nearest = sp.interpolate.NearestNDInterpolator(points, gains)
+        else:
+            raise ControlArgument(
+                f"unknown gain scheduling method '{gainsched_method}'")
+
+        def _compute_gain(mu):
+            K = _interp(mu)
+            if np.isnan(K).any():
+                K = _nearest(mu)
+            return K
+
     # Define the controller system
     if type == 'nonlinear':
         # Create an I/O system for the state feedback gains
-        def _control_update(t, x, inputs, params):
+        def _control_update(t, states, inputs, params):
             # Split input into desired state, nominal input, and current state
             xd_vec = inputs[0:sys.nstates]
             x_vec = inputs[-estimator.nstates:]
 
             # Compute the integral error in the xy coordinates
-            return C @ x_vec - C @ xd_vec
+            return C @ (x_vec - xd_vec)
 
-        def _control_output(t, e, z, params):
-            K = params.get('K')
+        def _control_output(t, states, inputs, params):
+            if gainsched:
+                mu = inputs[gainsched_indices]
+                K_ = _compute_gain(mu)
+            else:
+                K_ = params.get('K')
 
             # Split input into desired state, nominal input, and current state
-            xd_vec = z[0:sys.nstates]
-            ud_vec = z[sys.nstates:sys.nstates + sys.ninputs]
-            x_vec = z[-sys.nstates:]
+            xd_vec = inputs[0:sys.nstates]
+            ud_vec = inputs[sys.nstates:sys.nstates + sys.ninputs]
+            x_vec = inputs[-sys.nstates:]
 
             # Compute the control law
-            u = ud_vec - K[:, 0:sys.nstates] @ (x_vec - xd_vec)
+            u = ud_vec - K_[:, 0:sys.nstates] @ (x_vec - xd_vec)
             if nintegrators > 0:
-                u -= K[:, sys.nstates:] @ e
+                u -= K_[:, sys.nstates:] @ states
 
             return u
 
+        params = {} if gainsched else {'K': K}
         ctrl = NonlinearIOSystem(
-            _control_update, _control_output, name='control',
-            inputs=xd_labels + ud_labels + estimator.output_labels,
-            outputs=list(sys.input_index.keys()), params={'K': K},
-            states=nintegrators)
+            _control_update, _control_output, name=name, inputs=inputs,
+            outputs=outputs, states=states, params=params)
 
     elif type == 'linear' or type is None:
         # Create the matrices implementing the controller
@@ -772,9 +898,8 @@ def _control_output(t, e, z, params):
         ])
 
         ctrl = ss(
-            A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name='control',
-            inputs=xd_labels + ud_labels + estimator.output_labels,
-            outputs=list(sys.input_index.keys()), states=nintegrators)
+            A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name,
+            inputs=inputs, outputs=outputs, states=states)
 
     else:
         raise ControlArgument(f"unknown type '{type}'")
@@ -783,7 +908,7 @@ def _control_output(t, e, z, params):
     closed = interconnect(
         [sys, ctrl] if estimator == sys else [sys, ctrl, estimator],
         name=sys.name + "_" + ctrl.name,
-        inplist=xd_labels + ud_labels, inputs=xd_labels + ud_labels,
+        inplist=inputs[:-sys.nstates], inputs=inputs[:-sys.nstates],
         outlist=sys.output_labels + sys.input_labels,
         outputs=sys.output_labels + sys.input_labels
     )