Repository: python-control/python-control
Pull request #827 (2022-12-29 02:07:17)

Add gain scheduling to create_statefbk_iosystem()
This PR adds functionality to the `create_statefbk_iosystem` function, allowing it to be used to set up a simple gain scheduled controller.  From the documentation:

> Gain scheduling on the desired state, desired input, or system state can be implemented by setting the gain to a 2-tuple consisting of a list of gains and a list of points at which the gains
were computed, as well as a description of the scheduling variables
```
  ctrl, clsys = ct.create_statefbk_iosystem(
      sys, ([g1, ..., gN], [p1, ..., pN]), gainsched_indices=[s1, ..., sq])
```
> The list of indices can either be integers indicating the offset into the controller input vector or a list of strings matching the names of the input signals.

Example:
```
    # Create a simple nonlinear system to check (kinematic car)
    def unicycle_update(t, x, u, params):
        return np.array([np.cos(x[2]) * u[0], np.sin(x[2]) * u[0], u[1]])

    def unicycle_output(t, x, u, params):
        return x

    unicycle = ct.NonlinearIOSystem(
        unicycle_update, unicycle_output, inputs = ['v', 'phi'], 
        outputs = ['x', 'y', 'theta'], states = ['x_', 'y_', 'theta_'])

    # Speeds and angles at which to compute the gains
    speeds = [1, 5, 10]
    angles = np.linspace(0, pi/2, 4)
    points = list(itertools.product(speeds, angles))

    # Gains for each speed (using LQR controller)
    Q = np.identity(unicycle.nstates)
    R = np.identity(unicycle.ninputs)
    gains = [np.array(ct.lqr(unicycle.linearize(
        [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]

    #
    # Schedule on desired speed and angle
    #

    # Create gain scheduled controller
    ctrl, clsys = ct.create_statefbk_iosystem(
        unicycle, (gains, points), gainsched_indices=['vd', 'theta'])
```
Includes unit tests and documentation.


Patch:
diff --git a/control/descfcn.py b/control/descfcn.py
index 9adcc5dd..149db1bd 100644
--- a/control/descfcn.py
+++ b/control/descfcn.py
@@ -74,7 +74,7 @@ class DescribingFunctionNonlinearity():
 
 def describing_function(
         F, A, num_points=100, zero_check=True, try_method=True):
-    """Numerically compute the describing function of a nonlinear function
+    """Numerical compute the describing function of a nonlinear function
 
     The describing function of a nonlinearity is given by magnitude and phase
     of the first harmonic of the function when evaluated along a sinusoidal
diff --git a/control/iosys.py b/control/iosys.py
index 23429b30..5a389720 100644
--- a/control/iosys.py
+++ b/control/iosys.py
@@ -536,6 +536,10 @@ class InputOutputSystem(NamedIOSystem):
         # 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)
@@ -1994,14 +1991,6 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
         If `return_result` is True, returns the `result` from the
         :func:`scipy.optimize.root` function.
 
-    Notes
-    -----
-    For continuous time systems, equilibrium points are defined as points for
-    which the right hand side of the differential equation is zero:
-    :math:`f(t, x_e, u_e) = 0`. For discrete time systems, equilibrium points
-    are defined as points for which the right hand side of the difference
-    equation returns the current state: :math:`f(t, x_e, u_e) = x_e`.
-
     """
     from scipy.optimize import root
 
@@ -2018,6 +2007,11 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
     if np.isscalar(y0):
         y0 = np.ones((ninputs,)) * y0
 
+    # Discrete-time not yet supported
+    if isdtime(sys, strict=True):
+        raise NotImplementedError(
+            "Discrete time systems are not yet supported.")
+
     # Make sure the input arguments match the sizes of the system
     if len(x0) != nstates or \
        (u0 is not None and len(u0) != ninputs) or \
@@ -2033,28 +2027,18 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
         # Special cases: either inputs or outputs are constrained
         if y0 is None:
             # Take u0 as fixed and minimize over x
-            if sys.isdtime(strict=True):
-                def state_rhs(z): return sys._rhs(t, z, u0) - z
-            else:
-                def state_rhs(z): return sys._rhs(t, z, u0)
-
-            result = root(state_rhs, x0)
+            # TODO: update to allow discrete time systems
+            def ode_rhs(z): return sys._rhs(t, z, u0)
+            result = root(ode_rhs, x0)
             z = (result.x, u0, sys._out(t, result.x, u0))
-
         else:
             # Take y0 as fixed and minimize over x and u
-            if sys.isdtime(strict=True):
-                def rootfun(z):
-                    x, u = np.split(z, [nstates])
-                    return np.concatenate(
-                        (sys._rhs(t, x, u) - x, sys._out(t, x, u) - y0),
-                        axis=0)
-            else:
-                def rootfun(z):
-                    x, u = np.split(z, [nstates])
-                    return np.concatenate(
-                        (sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0)
-
+            def rootfun(z):
+                # Split z into x and u
+                x, u = np.split(z, [nstates])
+                # TODO: update to allow discrete time systems
+                return np.concatenate(
+                    (sys._rhs(t, x, u), sys._out(t, x, u) - y0), axis=0)
             z0 = np.concatenate((x0, u0), axis=0)   # Put variables together
             result = root(rootfun, z0)              # Find the eq point
             x, u = np.split(result.x, [nstates])    # Split result back in two
@@ -2148,6 +2132,7 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
 
         # Keep track of the number of states in the set of free variables
         nstate_vars = len(state_vars)
+        dtime = isdtime(sys, strict=True)
 
         def rootfun(z):
             # Map the vector of values into the states and inputs
@@ -2156,8 +2141,8 @@ def find_eqpt(sys, x0, u0=None, y0=None, t=0, params=None,
 
             # Compute the update and output maps
             dx = sys._rhs(t, x, u) - dx0
-            if sys.isdtime(strict=True):
-                dx -= x
+            if dtime:
+                dx -= x           # TODO: check
 
             # If no y0 is given, don't evaluate the output function
             if y0 is None:
@@ -2389,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
index 97f314da..fa4ac355 100644
--- 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 @@ except ImportError:
     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 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 create_statefbk_iosystem(
         ])
 
         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 create_statefbk_iosystem(
     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
     )
diff --git a/control/timeresp.py b/control/timeresp.py
index 509107cc..da158e0c 100644
--- a/control/timeresp.py
+++ b/control/timeresp.py
@@ -642,7 +642,7 @@ class TimeResponseData:
     # Convert to pandas
     def to_pandas(self):
         if not pandas_check():
-            raise ImportError("pandas not installed")
+            ImportError('pandas not installed')
         import pandas
 
         # Create a dict for setting up the data frame
diff --git a/doc/iosys.rst b/doc/iosys.rst
index 1da7f588..1f5f21e6 100644
--- a/doc/iosys.rst
+++ b/doc/iosys.rst
@@ -73,18 +73,18 @@ We begin by defining the dynamics of the system
       d = params.get('d', 0.56)
       k = params.get('k', 125)
       r = params.get('r', 1.6)
-      
+
       # Map the states into local variable names
       H = x[0]
       L = x[1]
 
       # Compute the control action (only allow addition of food)
       u_0 = u if u > 0 else 0
-  
+
       # Compute the discrete updates
       dH = (r + u_0) * H * (1 - H/k) - (a * H * L)/(c + H)
       dL = b * (a * H *  L)/(c + H) - d * L
-  
+
       return [dH, dL]
 
 We now create an input/output system using these dynamics:
@@ -105,10 +105,10 @@ of the system:
 
   X0 = [25, 20]                 # Initial H, L
   T = np.linspace(0, 70, 500)   # Simulation 70 years of time
-  
+
   # Simulate the system
   t, y = ct.input_output_response(io_predprey, T, 0, X0)
-  
+
   # Plot the response
   plt.figure(1)
   plt.plot(t, y[0])
@@ -168,14 +168,14 @@ function:
     inplist=['control.Ld'],
     outlist=['predprey.H', 'predprey.L', 'control.y[0]']
   )
-       
+
 Finally, we simulate the closed loop system:
 
 .. code-block:: python
 
   # Simulate the system
   t, y = ct.input_output_response(io_closed, T, 30, [15, 20])
-  
+
   # Plot the response
   plt.figure(2)
   plt.subplot(2, 1, 1)
@@ -198,7 +198,7 @@ Summing junction
 
 The :func:`~control.summing_junction` function can be used to create an
 input/output system that takes the sum of an arbitrary number of inputs.  For
-ezample, to create an input/output system that takes the sum of three inputs,
+example, to create an input/output system that takes the sum of three inputs,
 use the command
 
 .. code-block:: python
@@ -256,6 +256,90 @@ of the interconnected system) is not found, but inputs and outputs of
 individual systems that are not connected to other systems are left
 unconnected (so be careful!).
 
+Automated creation of state feedback systems
+--------------------------------------------
+
+The :func:`~control.create_statefbk_iosystem` function can be used to
+create an I/O system consisting of a state feedback gain (with
+optional integral action and gain scheduling) and an estimator.  A
+basic state feedback controller of the form
+
+.. math::
+
+  u = u_\text{d} - K (x - x_\text{d})
+
+can be created with the command::
+
+  ctrl, clsys = ct.create_statefbk_iosystem(sys, K)
+
+where `sys` is the process dynamics and `K` is the state feedback gain
+(e.g., from LQR).  The function returns the controller `ctrl` and the
+closed loop systems `clsys`, both as I/O systems.  The input to the
+controller is the vector of desired states :math:`x_\text{d}`, desired
+inputs :math:`u_\text{d}`, and system states :math:`x`.
+
+If the full system state is not available, the output of a state
+estimator can be used to construct the controller using the command::
+
+  ctrl, clsys = ct.create_statefbk_iosystem(sys, K, estimator=estim)
+
+where `estim` is the state estimator I/O system.  The controller will
+have the same form as above, but with the system state :math:`x`
+replaced by the estimated state :math:`\hat x` (output of `estim`).
+The closed loop controller will include both the state feedback and
+the estimator.
+
+Integral action can be included using the `integral_action` keyword.
+The value of this keyword can either be an matrix (ndarray) or a
+function.  If a matrix :math:`C` is specified, the difference between
+the desired state and system state will be multiplied by this matrix
+and integrated.  The controller gain should then consist of a set of
+proportional gains :math:`K_\text{p}` and integral gains
+:math:`K_\text{i}` with
+
+.. math::
+
+   K = \begin{bmatrix} K_\text{p} \\ K_\text{i} \end{bmatrix}
+
+and the control action will be given by
+
+.. math::
+
+  u = u_\text{d} - K\text{p} (x - x_\text{d}) -
+      K_\text{i} \int C (x - x_\text{d}) dt.
+
+If `integral_action` is a function `h`, that function will be called
+with the signature `h(t, x, u, params)` to obtain the outputs that
+should be integrated.  The number of outputs that are to be integrated
+must match the number of additional columns in the `K` matrix.  If an
+estimator is specified, :math:`\hat x` will be used in place of
+:math:`x`.
+
+Finally, gain scheduling on the desired state, desired input, or
+system state can be implemented by setting the gain to a 2-tuple
+consisting of a list of gains and a list of points at which the gains
+were computed, as well as a description of the scheduling variables::
+
+  ctrl, clsys = ct.create_statefbk_iosystem(
+      sys, ([g1, ..., gN], [p1, ..., pN]), gainsched_indices=[s1, ..., sq])
+
+The list of indices can either be integers indicating the offset into
+the controller input vector :math:`(x_\text{d}, u_\text{d}, x)` or a
+list of strings matching the names of the input signals.  The
+controller implemented in this case has the form
+
+.. math::
+
+  u = u_\text{d} - K(\mu) (x - x_\text{d})
+
+where :math:`\mu` represents the scheduling variables.  See
+:ref:`steering-gainsched.py` for an example implementation of a gain
+scheduled controller (in the alternative formulation section at the
+bottom of the file).
+
+Integral action and state estimation can also be used with gain
+scheduled controllers.
+
 
 Module classes and functions
 ============================
diff --git a/doc/steering-gainsched.rst b/doc/steering-gainsched.rst
index 511f76b8..a5ec2e0c 100644
--- a/doc/steering-gainsched.rst
+++ b/doc/steering-gainsched.rst
@@ -1,3 +1,5 @@
+.. _steering-gainsched.py:
+
 Gain scheduled control for vehicle steeering (I/O system)
 ---------------------------------------------------------
 
diff --git a/examples/steering-gainsched.py b/examples/steering-gainsched.py
index 7ddc6b5b..88eed9a9 100644
--- a/examples/steering-gainsched.py
+++ b/examples/steering-gainsched.py
@@ -70,7 +70,7 @@ def control_output(t, x, u, params):
     latpole1 = params.get('latpole1', -1/2 + sqrt(-7)/2)
     latpole2 = params.get('latpole2', -1/2 - sqrt(-7)/2)
     l = params.get('wheelbase', 3)
-    
+
     # Extract the system inputs
     ex, ey, etheta, vd, phid = u
 
@@ -85,7 +85,7 @@ def control_output(t, x, u, params):
     else:
         # We aren't moving, so don't turn the steering wheel
         phi = phid
-    
+
     return  np.array([v, phi])
 
 # Define the controller as an input/output system
@@ -135,7 +135,7 @@ trajgen = ct.NonlinearIOSystem(
 #                          +----------- [-1] -----------+
 #
 # We construct the system using the InterconnectedSystem constructor and using
-# signal labels to keep track of everything.  
+# signal labels to keep track of everything.
 
 steering = ct.interconnect(
     # List of subsystems
@@ -186,8 +186,93 @@ for vref in [8, 10, 12]:
     plt.plot([0, 5], [vref, vref], 'k--')
 
     # Plot the system output
-    y_line, = plt.plot(tout, yout[y_index, :], 'r')  # lateral position
-    v_line, = plt.plot(tout, yout[v_index, :], 'b')  # vehicle velocity
+    y_line, = plt.plot(tout, yout[y_index], 'r')  # lateral position
+    v_line, = plt.plot(tout, yout[v_index], 'b')  # vehicle velocity
+
+# Add axis labels
+plt.xlabel('Time (s)')
+plt.ylabel('x vel (m/s), y pos (m)')
+plt.legend((v_line, y_line), ('v', 'y'), loc='center right', frameon=False)
+
+#
+# Alternative formulation, using create_statefbk_iosystem()
+#
+# A different way to implement gain scheduling is to use the gain scheduling
+# functionality built into the create_statefbk_iosystem() function, where we
+# pass a table of gains instead of a single gain.  To generate a more
+# interesting plot, we scale the feedforward input to generate some error.
+#
+
+import itertools
+from math import pi
+
+# Define the points for the scheduling variables
+speeds = [1, 10, 20]
+angles = np.linspace(-pi, pi, 4)
+points = list(itertools.product(speeds, angles))
+
+# Create controllers at each scheduling point
+Q = np.diag([1, 1, 1])
+R = np.diag([0.1, 0.1])
+gains = [np.array(ct.lqr(vehicle.linearize(
+    [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]
+
+# Create the gain scheduled system
+controller, _ = ct.create_statefbk_iosystem(
+    vehicle, (gains, points), name='controller', ud_labels=['vd', 'phid'],
+    gainsched_indices=['vd', 'theta'], gainsched_method='linear')
+
+# Connect everything together (note that controller inputs are different
+steering = ct.interconnect(
+    # List of subsystems
+    (trajgen, controller, vehicle), name='steering',
+
+    # Interconnections between  subsystems
+    connections=(
+        ['controller.xd[0]', 'trajgen.xd'],
+        ['controller.xd[1]', 'trajgen.yd'],
+        ['controller.xd[2]', 'trajgen.thetad'],
+        ['controller.x', 'vehicle.x'],
+        ['controller.y', 'vehicle.y'],
+        ['controller.theta', 'vehicle.theta'],
+        ['controller.vd', ('trajgen', 'vd', 0.2)],      # create error
+        ['controller.phid', 'trajgen.phid'],
+        ['vehicle.v', 'controller.v'],
+        ['vehicle.phi', 'controller.phi']
+    ),
+
+    # System inputs
+    inplist=['trajgen.vref', 'trajgen.yref'],
+    inputs=['yref', 'vref'],
+
+    #  System outputs
+    outlist=['vehicle.x', 'vehicle.y', 'vehicle.theta', 'controller.v',
+             'controller.phi'],
+    outputs=['x', 'y', 'theta', 'v', 'phi']
+)
+
+# Plot the results to compare to the previous case
+plt.figure();
+
+# Plot the reference trajectory for the y position
+plt.plot([0, 5], [yref, yref], 'k--')
+
+# Find the signals we want to plot
+y_index = steering.find_output('y')
+v_index = steering.find_output('v')
+
+# Do an iteration through different speeds
+for vref in [8, 10, 12]:
+    # Simulate the closed loop controller response
+    tout, yout = ct.input_output_response(
+        steering, T, [vref * np.ones(len(T)), yref * np.ones(len(T))])
+
+    # Plot the reference speed
+    plt.plot([0, 5], [vref, vref], 'k--')
+
+    # Plot the system output
+    y_line, = plt.plot(tout, yout[y_index], 'r')  # lateral position
+    v_line, = plt.plot(tout, yout[v_index], 'b')  # vehicle velocity
 
 # Add axis labels
 plt.xlabel('Time (s)')


Test patch:
diff --git a/.github/workflows/os-blas-test-matrix.yml b/.github/workflows/os-blas-test-matrix.yml
index 4470e245..fede1902 100644
--- a/.github/workflows/os-blas-test-matrix.yml
+++ b/.github/workflows/os-blas-test-matrix.yml
@@ -251,9 +251,7 @@ jobs:
           pip install slycot-wheels/${{ matrix.packagekey }}/slycot*.whl
           pip show slycot
       - name: Test with pytest
-        run: JOBNAME="$JOBNAME" pytest control/tests
-        env:
-          JOBNAME: wheel ${{ matrix.packagekey }} ${{ matrix.blas_lib }}
+        run: pytest -v control/tests
 
 
   test-conda:
@@ -320,6 +318,6 @@ jobs:
           mamba install -c ./slycot-conda-pkgs slycot
           conda list
       - name: Test with pytest
-        run: JOBNAME="$JOBNAME" pytest control/tests
+        run: JOBNAME=$JOBNAME pytest control/tests
         env:
-          JOBNAME: conda ${{ matrix.packagekey }} ${{ matrix.blas_lib }}
+          JOBNAME: ${{ matrix.packagekey }} ${{ matrix.blas_lib }}
diff --git a/control/tests/flatsys_test.py b/control/tests/flatsys_test.py
index 196cddbd..4d4c8cf6 100644
--- a/control/tests/flatsys_test.py
+++ b/control/tests/flatsys_test.py
@@ -210,8 +210,7 @@ class TestFlatSys:
                 pytest.xfail("precision loss in some configurations")
 
             elif re.match("Iteration limit.*", traj_ocp.message) and \
-                 re.match(
-                     "conda ubuntu-3.* Generic", os.getenv('JOBNAME', '')) and \
+                 re.match("ubuntu-3.* Generic", os.getenv('JOBNAME')) and \
                  np.__version__ == '1.24.0':
                 pytest.xfail("gh820: iteration limit exceeded")
 
diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py
index 527f597b..71b59e5c 100644
--- a/control/tests/iosys_test.py
+++ b/control/tests/iosys_test.py
@@ -10,10 +10,9 @@ created for that purpose.
 
 import re
 import warnings
-import pytest
 
 import numpy as np
-from math import sqrt
+import pytest
 
 import control as ct
 from control import iosys as ios
@@ -239,7 +238,7 @@ class TestIOSys:
         assert lin_nocopy.find_state('x') is None
 
         # if signal names are provided, they should override those of kincar
-        linearized_newnames = kincar.linearize([0, 0, 0], [0, 0],
+        linearized_newnames = kincar.linearize([0, 0, 0], [0, 0], 
             name='linearized',
             copy_names=True, inputs=['v2', 'phi2'], outputs=['x2','y2'])
         assert linearized_newnames.name == 'linearized'
@@ -767,8 +766,8 @@ class TestIOSys:
         np.testing.assert_allclose(ios_t, lin_t,atol=0.002,rtol=0.)
         np.testing.assert_allclose(ios_y, lin_y,atol=0.002,rtol=0.)
 
-    def test_find_eqpts_dfan(self, tsys):
-        """Test find_eqpt function on dfan example"""
+    def test_find_eqpts(self, tsys):
+        """Test find_eqpt function"""
         # Simple equilibrium point with no inputs
         nlsys = ios.NonlinearIOSystem(predprey)
         xeq, ueq, result = ios.find_eqpt(
@@ -837,7 +836,7 @@ class TestIOSys:
         np.testing.assert_array_almost_equal(
             nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5)
 
-        # Same test as before, but now all constraints are in the state vector
+        # The same test as previous, but now all constraints are in the state vector
         nlsys_full = ios.NonlinearIOSystem(pvtol_full, None)
         xeq, ueq, result = ios.find_eqpt(
             nlsys_full, [0, 0, 0.1, 0.1, 0, 0], [0.01, 4*9.8],
@@ -1188,7 +1187,7 @@ class TestIOSys:
             assert "copy of namedsys.x0" in same_name_series.state_index
 
     def test_named_signals_linearize_inconsistent(self, tsys):
-        """Mare sure that providing inputs or outputs not consistent with
+        """Make sure that providing inputs or outputs not consistent with
            updfcn or outfcn fail
         """
 
@@ -1233,6 +1232,17 @@ class TestIOSys:
             with pytest.raises(ValueError):
                 sys2.linearize(x0, u0)
 
+    def test_linearize_concatenation(self, kincar):
+        # Create a simple nonlinear system to check (kinematic car)
+        iosys = kincar
+        linearized = iosys.linearize([0, np.array([0, 0])], [0, 0])
+        np.testing.assert_array_almost_equal(linearized.A, np.zeros((3,3)))
+        np.testing.assert_array_almost_equal(
+            linearized.B, [[1, 0], [0, 0], [0, 1]])
+        np.testing.assert_array_almost_equal(
+            linearized.C, [[1, 0, 0], [0, 1, 0]])
+        np.testing.assert_array_almost_equal(linearized.D, np.zeros((2,2)))
+
     def test_lineariosys_statespace(self, tsys):
         """Make sure that a LinearIOSystem is also a StateSpace object"""
         iosys_siso = ct.LinearIOSystem(tsys.siso_linsys, name='siso')
@@ -1483,7 +1493,7 @@ def test_linear_interconnection():
     tf_siso = ct.tf(1, [0.1, 1])
     ss_siso = ct.ss(1, 2, 1, 1)
     nl_siso = ios.NonlinearIOSystem(
-        lambda t, x, u, params: x*x,
+        lambda t, x, u, params: x*x, 
         lambda t, x, u, params: u*x, states=1, inputs=1, outputs=1)
 
     # Create a "regular" InterconnectedSystem
@@ -1531,7 +1541,7 @@ def test_linear_interconnection():
     np.testing.assert_array_almost_equal(io_connect.C, ss_connect.C)
     np.testing.assert_array_almost_equal(io_connect.D, ss_connect.D)
 
-    # make sure interconnections of linear systems are linear and
+    # make sure interconnections of linear systems are linear and 
     # if a nonlinear system is included then system is nonlinear
     assert isinstance(ss_siso*ss_siso, ios.LinearIOSystem)
     assert isinstance(tf_siso*ss_siso, ios.LinearIOSystem)
@@ -1542,7 +1552,7 @@ def test_linear_interconnection():
     assert ~isinstance(tf_siso*nl_siso, ios.LinearIOSystem)
     assert ~isinstance(nl_siso*tf_siso, ios.LinearIOSystem)
     assert ~isinstance(nl_siso*nl_siso, ios.LinearIOSystem)
-
+    
 
 def predprey(t, x, u, params={}):
     """Predator prey dynamics"""
@@ -1899,73 +1909,3 @@ def test_rss():
     with pytest.warns(UserWarning, match="may be interpreted as continuous"):
         sys = ct.drss(2, 1, 1, dt=None)
         assert np.all(np.abs(sys.poles()) < 1)
-
-
-def eqpt_rhs(t, x, u, params):
-    return np.array([x[0]/2 + u[0], x[0] - x[1]**2 + u[1], x[1] - x[2]])
-
-def eqpt_out(t, x, u, params):
-    return np.array([x[0], x[1] + u[1]])
-
-@pytest.mark.parametrize(
-    "x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect", [
-        # Equilibrium points with input given
-        (0, None, 0, None, None, None, None, None, 0, [0, 0, 0], [0, 0]),
-        (0, None, 0, None, None, None, None, None, None, [0, 0, 0], [0, 0]),
-        ([0.9, 0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0,
-         [2, sqrt(2), sqrt(2)], [-1, 0]),
-        ([0.9, -0.9, 0.9], None, [-1, 0], None, None, None, None, None, 0,
-         [2, -sqrt(2), -sqrt(2)], [-1, 0]),     # same input, different eqpt
-        (0, None, 0, None, None, None, None, None, 1, [0, 0, 0], [0, 0]), #DT
-        (0, None, [-1, 0], None, None, None, None, None, 1, None, None),  #DT
-        ([0, -0.1, 0], None, [0, -0.25], None, None, None, None, None, 1, #DT
-         [0, -0.5, -0.25], [0, -0.25]),
-
-        # Equilibrium points with output given
-        ([0.9, 0.9, 0.9], None, [-0.9, 0], None, [2, sqrt(2)], None, None,
-         None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]),
-        (0, None, [0, -0.25], None, [0, -0.75], None, None, None, 1,      #DT
-         [0, -0.5, -0.25], [0, -0.25]),
-
-        # Equilibrium points with mixture of inputs and outputs given
-        ([0.9, 0.9, 0.9], None, [-1, 0], [0], [2, sqrt(2)], [1], None,
-         None, 0, [2, sqrt(2), sqrt(2)], [-1, 0]),
-        (0, None, [0, -0.22], [0], [0, -0.75], [1], None, None, 1,        #DT
-         [0, -0.5, -0.25], [0, -0.25]),
-    ])
-
-def test_find_eqpt(x0, ix, u0, iu, y0, iy, dx0, idx, dt, x_expect, u_expect):
-    sys = ct.NonlinearIOSystem(
-        eqpt_rhs, eqpt_out, dt=dt, states=3, inputs=2, outputs=2)
-
-    xeq, ueq = ct.find_eqpt(
-        sys, x0, u0, y0, ix=ix, iu=iu, iy=iy, dx0=dx0, idx=idx)
-
-    # If no equilibrium points, skip remaining tests
-    if x_expect is None:
-        assert xeq is None
-        assert ueq is None
-        return
-
-    # Make sure we are at an appropriate equilibrium point
-    if dt is None or dt == 0:
-        # Continuous time system
-        np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), 0, atol=1e-6)
-        if y0 is not None:
-            y0 = np.array(y0)
-            iy = np.s_[:] if iy is None else np.array(iy)
-            np.testing.assert_allclose(
-                eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6)
-
-    else:
-        # Discrete time system
-        np.testing.assert_allclose(eqpt_rhs(0, xeq, ueq, {}), xeq, atol=1e-6)
-        if y0 is not None:
-            y0 = np.array(y0)
-            iy = np.s_[:] if iy is None else np.array(iy)
-            np.testing.assert_allclose(
-                eqpt_out(0, xeq, ueq, {})[iy], y0[iy], atol=1e-6)
-
-    # Check that we got the expected result as well
-    np.testing.assert_allclose(np.array(xeq), x_expect, atol=1e-6)
-    np.testing.assert_allclose(np.array(ueq), u_expect, atol=1e-6)
diff --git a/control/tests/optimal_test.py b/control/tests/optimal_test.py
index 53f2c29a..c76859df 100644
--- a/control/tests/optimal_test.py
+++ b/control/tests/optimal_test.py
@@ -626,7 +626,7 @@ def test_equality_constraints():
         ('collocation', 5, 'u0', 'endpoint'),
         ('collocation', 5, 'input', 'openloop'),# open loop sim fails
         ('collocation', 10, 'input', None),
-        ('collocation', 10, 'u0', None),        # from documentation
+        ('collocation', 10, 'u0', None),        # from documenentation
         ('collocation', 10, 'state', None),
         ('collocation', 20, 'state', None),
     ])
@@ -716,11 +716,9 @@ def test_optimal_doc(method, npts, initial_guess, fail):
 
         # Make sure we started and stopped at the right spot
         if fail == 'endpoint':
-            assert not np.allclose(result.states[:, -1], xf, rtol=1e-4)
             pytest.xfail("optimization does not converge to endpoint")
         else:
             np.testing.assert_almost_equal(result.states[:, 0], x0, decimal=4)
-            np.testing.assert_almost_equal(result.states[:, -1], xf, decimal=2)
 
             # Simulate the trajectory to make sure it looks OK
             resp = ct.input_output_response(
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
index 13f164e1..9e8feb4c 100644
--- a/control/tests/statefbk_test.py
+++ b/control/tests/statefbk_test.py
@@ -5,6 +5,8 @@ RMM, 30 Mar 2011 (based on TestStatefbk from v0.4a)
 
 import numpy as np
 import pytest
+import itertools
+from math import pi, atan
 
 import control as ct
 from control import lqe, dlqe, poles, rss, ss, tf
@@ -777,3 +779,201 @@ class TestStatefbk:
         with pytest.raises(ControlArgument, match="must be an array of size"):
             ctrl, clsys = ct.create_statefbk_iosystem(
                 sys, K, integral_action=C_int)
+
+
+# Kinematic car example for testing gain scheduling
+@pytest.fixture
+def unicycle():
+    # Create a simple nonlinear system to check (kinematic car)
+    def unicycle_update(t, x, u, params):
+        return np.array([np.cos(x[2]) * u[0], np.sin(x[2]) * u[0], u[1]])
+
+    def unicycle_output(t, x, u, params):
+        return x
+
+    return ct.NonlinearIOSystem(
+        unicycle_update, unicycle_output,
+        inputs = ['v', 'phi'],
+        outputs = ['x', 'y', 'theta'],
+        states = ['x_', 'y_', 'theta_'])
+
+from math import pi
+
+@pytest.mark.parametrize("method", ['nearest', 'linear', 'cubic'])
+def test_gainsched_unicycle(unicycle, method):
+    # Speeds and angles at which to compute the gains
+    speeds = [1, 5, 10]
+    angles = np.linspace(0, pi/2, 4)
+    points = list(itertools.product(speeds, angles))
+
+    # Gains for each speed (using LQR controller)
+    Q = np.identity(unicycle.nstates)
+    R = np.identity(unicycle.ninputs)
+    gains = [np.array(ct.lqr(unicycle.linearize(
+        [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]
+
+    #
+    # Schedule on desired speed and angle
+    #
+
+    # Create gain scheduled controller
+    ctrl, clsys = ct.create_statefbk_iosystem(
+        unicycle, (gains, points),
+        gainsched_indices=[3, 2], gainsched_method=method)
+
+    # Check the gain at the selected points
+    for speed, angle in points:
+        # Figure out the desired state and input
+        xe, ue = np.array([0, 0, angle]), np.array([speed, 0])
+        xd, ud = np.array([0, 0, angle]), np.array([speed, 0])
+
+        # Check the control system at the scheduling points
+        ctrl_lin = ctrl.linearize([], [xd, ud, xe*0])
+        K, S, E = ct.lqr(unicycle.linearize(xd, ud), Q, R)
+        np.testing.assert_allclose(
+            ctrl_lin.D[-xe.size:, -xe.size:], -K, rtol=1e-2)
+
+        # Check the closed loop system at the scheduling points
+        clsys_lin = clsys.linearize(xe, [xd, ud])
+        np.testing.assert_allclose(
+            np.sort(clsys_lin.poles()), np.sort(E), rtol=1e-2)
+
+    # Check the gain at an intermediate point and confirm stability
+    speed, angle = 2, pi/3
+    xe, ue = np.array([0, 0, angle]), np.array([speed, 0])
+    xd, ud = np.array([0, 0, angle]), np.array([speed, 0])
+    clsys_lin = clsys.linearize(xe, [xd, ud])
+    assert np.all(np.real(clsys_lin.poles()) < 0)
+
+    # Make sure that gains are different from 'nearest'
+    if method is not None and method != 'nearest':
+        ctrl_nearest, clsys_nearest = ct.create_statefbk_iosystem(
+            unicycle, (gains, points),
+            gainsched_indices=['ud[0]', 2], gainsched_method='nearest')
+        nearest_lin = clsys_nearest.linearize(xe, [xd, ud])
+        assert not np.allclose(
+            np.sort(clsys_lin.poles()), np.sort(nearest_lin.poles()), rtol=1e-2)
+
+    # Run a simulation following a curved path
+    T = 10                      # length of the trajectory [sec]
+    r = 10                      # radius of the circle [m]
+    timepts = np.linspace(0, T, 50)
+    Xd = np.vstack([
+        r * np.cos(timepts/T * pi/2 + 3*pi/2),
+        r * np.sin(timepts/T * pi/2 + 3*pi/2) + r,
+        timepts/T * pi/2
+    ])
+    Ud = np.vstack([
+        np.ones_like(timepts) * (r * pi/2) / T,
+        np.ones_like(timepts) * (pi / 2) / T
+    ])
+    X0 = Xd[:, 0] + np.array([-0.1, -0.1, -0.1])
+
+    resp = ct.input_output_response(clsys, timepts, [Xd, Ud], X0)
+    # plt.plot(resp.states[0], resp.states[1])
+    np.testing.assert_allclose(
+        resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2)
+
+    #
+    # Schedule on actual speed
+    #
+
+    # Create gain scheduled controller
+    ctrl, clsys = ct.create_statefbk_iosystem(
+        unicycle, (gains, points),
+        ud_labels=['vd', 'phid'], gainsched_indices=['vd', 'theta'])
+
+    # Check the gain at the selected points
+    for speed, angle in points:
+        # Figure out the desired state and input
+        xe, ue = np.array([0, 0, angle]), np.array([speed, 0])
+        xd, ud = np.array([0, 0, angle]), np.array([speed, 0])
+
+        # Check the control system at the scheduling points
+        ctrl_lin = ctrl.linearize([], [xd*0, ud, xe])
+        K, S, E = ct.lqr(unicycle.linearize(xe, ue), Q, R)
+        np.testing.assert_allclose(
+            ctrl_lin.D[-xe.size:, -xe.size:], -K, rtol=1e-2)
+
+        # Check the closed loop system at the scheduling points
+        clsys_lin = clsys.linearize(xe, [xd, ud])
+        np.testing.assert_allclose(np.sort(
+            clsys_lin.poles()), np.sort(E), rtol=1e-2)
+
+    # Run a simulation following a curved path
+    resp = ct.input_output_response(clsys, timepts, [Xd, Ud], X0)
+    np.testing.assert_allclose(
+        resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2)
+
+
+def test_gainsched_default_indices():
+    # Define a linear system to test
+    sys = ct.ss([[-1, 0.1], [0, -2]], [[0], [1]], np.eye(2), 0)
+
+    # Define gains at origin + corners of unit cube
+    points = [[0, 0]] + list(itertools.product([-1, 1], [-1, 1]))
+
+    # Define gain to be constant
+    K, _, _ = ct.lqr(sys, np.eye(sys.nstates), np.eye(sys.ninputs))
+    gains = [K for p in points]
+
+    # Define the paramters for the simulations
+    timepts = np.linspace(0, 10, 100)
+    X0 = np.ones(sys.nstates) * 0.9
+
+    # Create a controller and simulate the initial response
+    gs_ctrl, gs_clsys = ct.create_statefbk_iosystem(sys, (gains, points))
+    gs_resp = ct.input_output_response(gs_clsys, timepts, 0, X0)
+
+    # Verify that we get the same result as a constant gain
+    ck_clsys = ct.ss(sys.A - sys.B @ K, sys.B, sys.C, 0)
+    ck_resp = ct.input_output_response(ck_clsys, timepts, 0, X0)
+
+    np.testing.assert_allclose(gs_resp.states, ck_resp.states)
+
+
+def test_gainsched_errors(unicycle):
+    # Set up gain schedule (same as previous test)
+    speeds = [1, 5, 10]
+    angles = np.linspace(0, pi/2, 4)
+    points = list(itertools.product(speeds, angles))
+
+    Q = np.identity(unicycle.nstates)
+    R = np.identity(unicycle.ninputs)
+    gains = [np.array(ct.lqr(unicycle.linearize(
+        [0, 0, angle], [speed, 0]), Q, R)[0]) for speed, angle in points]
+
+    # Make sure the generic case works OK
+    ctrl, clsys = ct.create_statefbk_iosystem(
+        unicycle, (gains, points), gainsched_indices=[3, 2])
+    xd, ud = np.array([0, 0, angles[0]]), np.array([speeds[0], 0])
+    ctrl_lin = ctrl.linearize([], [xd, ud, xd*0])
+    K, S, E = ct.lqr(unicycle.linearize(xd, ud), Q, R)
+    np.testing.assert_allclose(
+        ctrl_lin.D[-xd.size:, -xd.size:], -K, rtol=1e-2)
+
+    # Wrong type of gain schedule argument
+    with pytest.raises(ControlArgument, match="gain must be an array"):
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            unicycle, [gains, points], gainsched_indices=[3, 2])
+
+    # Wrong number of gain schedule argument
+    with pytest.raises(ControlArgument, match="gain must be a 2-tuple"):
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            unicycle, (gains, speeds, angles), gainsched_indices=[3, 2])
+
+    # Mismatched dimensions for gains and points
+    with pytest.raises(ControlArgument, match="length of gainsched_indices"):
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            unicycle, (gains, [speeds]), gainsched_indices=[3, 2])
+
+    # Unknown gain scheduling variable label
+    with pytest.raises(ValueError, match=".* not in list"):
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            unicycle, (gains, points), gainsched_indices=['stuff', 2])
+
+    # Unknown gain scheduling method
+    with pytest.raises(ControlArgument, match="unknown gain scheduling method"):
+        ctrl, clsys = ct.create_statefbk_iosystem(
+            unicycle, (gains, points),
+            gainsched_indices=[3, 2], gainsched_method='unknown')