From 8710ee2df653a003f5412a97d8b344086fc29b9d Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Wed, 28 Dec 2022 08:39:43 -0800
Subject: [PATCH 1/6] initial implementation of gain scheduling

---
 control/iosys.py               |  26 +++--
 control/statefbk.py            | 179 +++++++++++++++++++++++++--------
 control/tests/iosys_test.py    |  13 ++-
 control/tests/statefbk_test.py | 125 +++++++++++++++++++++++
 4 files changed, 290 insertions(+), 53 deletions(-)

diff --git a/control/iosys.py b/control/iosys.py
index 2152092fc..5a3897200 100644
--- 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
index 97f314da5..0901684a3 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 @@ 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,8 @@ 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):
     """Create an I/O system using a (full) state feedback controller
 
     This function creates an input/output system that implements a
@@ -617,26 +618,46 @@ 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[, method])
+
+        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.  If `method`
+        is specified, it is passed to :func:`scipy.interpolate.griddata` to
+        specify the method of interpolation.  Possible values include
+        `linear`, `nearest`, and `cubic`.
 
     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.
 
     integral_action : None, ndarray, or func, optional
         If this keyword is specified, the controller can include integral
@@ -650,30 +671,48 @@ 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 integers, 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
+        either the system state x or the system output y (if an estimator is
+        given).
+
+    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, the 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 system output y (if an estimator is given).
+        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).  If a gain scheduled controller is specified, the gain
+        (proportional and integral) are evaluated using the input mu.
 
     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) along with
+        any unassigned gain scheduling values mu and outputs the system
+        state x and the applied input u (vertically stacked).
+
+    Notes
+    -----
+    1. If the gain scheduling variable labes are set to the names of system
+       states, inputs, or outputs or desired states or inputs, then the
+       scheduling variables are internally connected to those variables.
 
     """
     # Make sure that we were passed an I/O system as an input
@@ -709,52 +748,104 @@ 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
+        gains, points = gain[0:2]
+        method = 'nearest' if len(gain) < 3 else gain[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)]
 
+    # Process gainscheduling variables, if present
+    if gainsched:
+        # Create a copy of the scheduling variable indices (default = empty)
+        gainsched_indices = [] 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")
+
+        # TODO: Process scheduling variables
+
     # 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 _compute_gain(mu, gains_, points_):
+            K = np.array([
+                [sp.interpolate.griddata(
+                    points_, gains_[:, i, j], mu, method=method).item()
+                 for j in range(gains_.shape[2])]
+                for i in range(gains_.shape[1])
+            ])
+            return K
 
-        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, gains, points)
+            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},
+            outputs=list(sys.input_index.keys()), params=params,
             states=nintegrators)
 
     elif type == 'linear' or type is None:
diff --git a/control/tests/iosys_test.py b/control/tests/iosys_test.py
index c6b8d9d15..71b59e5ce 100644
--- a/control/tests/iosys_test.py
+++ b/control/tests/iosys_test.py
@@ -1187,7 +1187,7 @@ def test_signals_naming_convention_0_8_4(self, tsys):
             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
         """
 
@@ -1232,6 +1232,17 @@ def outfcn(t, x, u, params):
             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')
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
index 13f164e1f..80ad37b08 100644
--- a/control/tests/statefbk_test.py
+++ b/control/tests/statefbk_test.py
@@ -5,6 +5,8 @@
 
 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,126 @@ def test_statefbk_errors(self):
         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", [None, 'nearest'])
+def test_gainsched_unicycle(unicycle, method):
+    # Speeds and angles at which to compute the gains
+    speeds = [1, 5, 10]
+    angles = -pi + np.linspace(0, 2*pi, 10)
+    points = list(itertools.product(speeds, angles))
+
+    # Gains for each speed (using LQR controller)
+    Q = np.identity(unicycle.nstates)
+    R = np.identity(unicycle.ninputs)
+    gains = [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) if method is None else (gains, points, method),
+        gainsched_indices=[3, 2])
+
+    # 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)
+
+    # 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, 100)
+    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), gainsched_indices=[3, 7])
+
+    # 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
+    T = 10                      # length of the trajectory [sec]
+    r = 10                      # radius of the circle [m]
+    timepts = np.linspace(0, T, 100)
+    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)
+    np.testing.assert_allclose(
+        resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2)

From 5fcf2b57dc3eec173ff91b215c99393b02e23eaa Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Wed, 28 Dec 2022 11:05:12 -0800
Subject: [PATCH 2/6] add processing for linear, cubic interpolation

---
 control/statefbk.py            | 31 ++++++++++++++++--------
 control/tests/statefbk_test.py | 43 +++++++++++++++++-----------------
 2 files changed, 43 insertions(+), 31 deletions(-)

diff --git a/control/statefbk.py b/control/statefbk.py
index 0901684a3..199c63bb9 100644
--- a/control/statefbk.py
+++ b/control/statefbk.py
@@ -802,6 +802,26 @@ def create_statefbk_iosystem(
 
         # TODO: Process scheduling variables
 
+        # Create interpolating function
+        if method == 'nearest':
+            _interp = sp.interpolate.NearestNDInterpolator(points, gains)
+            _nearest = _interp
+        elif method == 'linear':
+            _interp = sp.interpolate.LinearNDInterpolator(points, gains)
+            _nearest = sp.interpolate.NearestNDInterpolator(points, gains)
+        elif method == 'cubic':
+            _interp = sp.interpolate.CloughTocher2DInterpolator(points, gains)
+            _nearest = sp.interpolate.NearestNDInterpolator(points, gains)
+        else:
+            raise ControlArgument(
+                f"unknown gain scheduling method '{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
@@ -813,19 +833,10 @@ def _control_update(t, states, inputs, params):
             # Compute the integral error in the xy coordinates
             return C @ (x_vec - xd_vec)
 
-        def _compute_gain(mu, gains_, points_):
-            K = np.array([
-                [sp.interpolate.griddata(
-                    points_, gains_[:, i, j], mu, method=method).item()
-                 for j in range(gains_.shape[2])]
-                for i in range(gains_.shape[1])
-            ])
-            return K
-
         def _control_output(t, states, inputs, params):
             if gainsched:
                 mu = inputs[gainsched_indices]
-                K_ = _compute_gain(mu, gains, points)
+                K_ = _compute_gain(mu)
             else:
                 K_ = params.get('K')
 
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
index 80ad37b08..998e94c85 100644
--- a/control/tests/statefbk_test.py
+++ b/control/tests/statefbk_test.py
@@ -799,18 +799,18 @@ def unicycle_output(t, x, u, params):
 
 from math import pi
 
-@pytest.mark.parametrize("method", [None, 'nearest'])
+@pytest.mark.parametrize("method", [None, 'nearest', 'linear', 'cubic'])
 def test_gainsched_unicycle(unicycle, method):
     # Speeds and angles at which to compute the gains
     speeds = [1, 5, 10]
-    angles = -pi + np.linspace(0, 2*pi, 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 = [ct.lqr(unicycle.linearize(
-        [0, 0, angle], [speed, 0]), Q, R)[0] for speed, angle in points]
+    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
@@ -836,13 +836,28 @@ def test_gainsched_unicycle(unicycle, method):
 
         # 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)
+        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, 'nearest'), gainsched_indices=[3, 2])
+        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, 100)
+    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,
@@ -885,20 +900,6 @@ def test_gainsched_unicycle(unicycle, method):
             clsys_lin.poles()), np.sort(E), 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, 100)
-    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)
     np.testing.assert_allclose(
         resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2)

From 6e56b733830cb339f82abd4ef7d07d90911e878e Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Wed, 28 Dec 2022 11:33:44 -0800
Subject: [PATCH 3/6] add gainsched label processing + unit tests for errors

---
 control/statefbk.py            | 15 +++++++----
 control/tests/statefbk_test.py | 46 ++++++++++++++++++++++++++++++++--
 2 files changed, 54 insertions(+), 7 deletions(-)

diff --git a/control/statefbk.py b/control/statefbk.py
index 199c63bb9..5ec989cf7 100644
--- a/control/statefbk.py
+++ b/control/statefbk.py
@@ -788,6 +788,9 @@ def create_statefbk_iosystem(
         # 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 string of labels for the control system
+    input_labels = xd_labels + ud_labels + estimator.output_labels
+
     # Process gainscheduling variables, if present
     if gainsched:
         # Create a copy of the scheduling variable indices (default = empty)
@@ -797,10 +800,13 @@ def create_statefbk_iosystem(
         # 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"
+                "length of gainsched_indices must match dimension of"
                 " scheduling variables")
 
-        # TODO: Process scheduling variables
+        # Process scheduling variables
+        for i, idx in enumerate(gainsched_indices):
+            if isinstance(idx, str):
+                gainsched_indices[i] = input_labels.index(gainsched_indices[i])
 
         # Create interpolating function
         if method == 'nearest':
@@ -855,9 +861,8 @@ def _control_output(t, states, inputs, params):
         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=params,
-            states=nintegrators)
+            inputs=input_labels, outputs=list(sys.input_index.keys()),
+            params=params, states=nintegrators)
 
     elif type == 'linear' or type is None:
         # Create the matrices implementing the controller
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
index 998e94c85..792fc59b7 100644
--- a/control/tests/statefbk_test.py
+++ b/control/tests/statefbk_test.py
@@ -849,7 +849,8 @@ def test_gainsched_unicycle(unicycle, method):
     # 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, 'nearest'), gainsched_indices=[3, 2])
+            unicycle, (gains, points, 'nearest'),
+            gainsched_indices=['ud[0]', 2])
         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)
@@ -880,7 +881,8 @@ def test_gainsched_unicycle(unicycle, method):
 
     # Create gain scheduled controller
     ctrl, clsys = ct.create_statefbk_iosystem(
-        unicycle, (gains, points), gainsched_indices=[3, 7])
+        unicycle, (gains, points),
+        ud_labels=['vd', 'phid'], gainsched_indices=['vd', 'theta'])
 
     # Check the gain at the selected points
     for speed, angle in points:
@@ -903,3 +905,43 @@ def test_gainsched_unicycle(unicycle, method):
     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_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])
+
+    # 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, 'stuff'), gainsched_indices=[3, 2])

From 21aa0a6fd563e360b23d8957a7aac6f27df4b937 Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Wed, 28 Dec 2022 17:50:30 -0800
Subject: [PATCH 4/6] add examples, update documentation, small refactoring of
 code

---
 control/statefbk.py            | 117 +++++++++++++++++++--------------
 control/tests/statefbk_test.py |  19 ++++--
 doc/iosys.rst                  |  96 ++++++++++++++++++++++++---
 doc/steering-gainsched.rst     |   2 +
 examples/steering-gainsched.py |  95 ++++++++++++++++++++++++--
 5 files changed, 259 insertions(+), 70 deletions(-)

diff --git a/control/statefbk.py b/control/statefbk.py
index 5ec989cf7..9052d159d 100644
--- a/control/statefbk.py
+++ b/control/statefbk.py
@@ -602,7 +602,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, type=None,
-        xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=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
@@ -640,16 +642,11 @@ def create_statefbk_iosystem(
         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[, method])
-
-        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.  If `method`
-        is specified, it is passed to :func:`scipy.interpolate.griddata` to
-        specify the method of interpolation.  Possible values include
-        `linear`, `nearest`, and `cubic`.
+        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.
@@ -657,12 +654,13 @@ def create_statefbk_iosystem(
         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.
+        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
@@ -674,19 +672,28 @@ def create_statefbk_iosystem(
         If an estimator is provided, use the states of the estimator as
         the system inputs for the controller.
 
-    gainsched_indices : list of integers, optional
+    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 input to use for scheduling the gain. The input to
         the controller is the desired state xd, the desired input ud, and
         either the system state x or the system output y (if an estimator is
-        given).
+        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.
+
+    gainsched_method : str, optional
+        The method to use for gain scheduling.  Possible values include
+        `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, the a nonlinear,
+        runtime). If the gain parameter is a tuple, then a nonlinear,
         gain-scheduled controller is created.
 
     Returns
@@ -694,25 +701,30 @@ def create_statefbk_iosystem(
     ctrl : InputOutputSystem
         Input/output system representing the controller.  This system takes
         as inputs the desired state xd, the desired input ud, and either the
-        system state x or the system output y (if an estimator is given).
-        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).  If a gain scheduled controller is specified, the gain
-        (proportional and integral) are evaluated using the input mu.
+        system state x or the estimated state xhat.  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).
+        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) along with
-        any unassigned gain scheduling values mu and outputs the system
-        state x and the applied input u (vertically stacked).
-
-    Notes
-    -----
-    1. If the gain scheduling variable labes are set to the names of system
-       states, inputs, or outputs or desired states or inputs, then the
-       scheduling variables are internally connected to those variables.
+        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
@@ -759,8 +771,9 @@ def create_statefbk_iosystem(
 
     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]
-        method = 'nearest' if len(gain) < 3 else gain[2]
 
         # Stack gains and points if past as a list
         gains = np.stack(gains)
@@ -788,8 +801,13 @@ def create_statefbk_iosystem(
         # 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 string of labels for the control system
-    input_labels = xd_labels + ud_labels + estimator.output_labels
+    # 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:
@@ -806,21 +824,22 @@ def create_statefbk_iosystem(
         # Process scheduling variables
         for i, idx in enumerate(gainsched_indices):
             if isinstance(idx, str):
-                gainsched_indices[i] = input_labels.index(gainsched_indices[i])
+                gainsched_indices[i] = inputs.index(gainsched_indices[i])
 
         # Create interpolating function
-        if method == 'nearest':
+        if gainsched_method == 'nearest':
             _interp = sp.interpolate.NearestNDInterpolator(points, gains)
-            _nearest = _interp
-        elif method == 'linear':
+            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 method == 'cubic':
+        elif gainsched_method == 'cubic':
             _interp = sp.interpolate.CloughTocher2DInterpolator(points, gains)
             _nearest = sp.interpolate.NearestNDInterpolator(points, gains)
         else:
             raise ControlArgument(
-                f"unknown gain scheduling method '{method}'")
+                f"unknown gain scheduling method '{gainsched_method}'")
 
         def _compute_gain(mu):
             K = _interp(mu)
@@ -860,9 +879,8 @@ def _control_output(t, states, inputs, params):
 
         params = {} if gainsched else {'K': K}
         ctrl = NonlinearIOSystem(
-            _control_update, _control_output, name='control',
-            inputs=input_labels, outputs=list(sys.input_index.keys()),
-            params=params, 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
@@ -879,9 +897,8 @@ def _control_output(t, states, inputs, 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}'")
@@ -890,7 +907,7 @@ def _control_output(t, states, inputs, 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
     )
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
index 792fc59b7..343c8790b 100644
--- a/control/tests/statefbk_test.py
+++ b/control/tests/statefbk_test.py
@@ -799,7 +799,7 @@ def unicycle_output(t, x, u, params):
 
 from math import pi
 
-@pytest.mark.parametrize("method", [None, 'nearest', 'linear', 'cubic'])
+@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]
@@ -818,9 +818,8 @@ def test_gainsched_unicycle(unicycle, method):
 
     # Create gain scheduled controller
     ctrl, clsys = ct.create_statefbk_iosystem(
-        unicycle,
-        (gains, points) if method is None else (gains, points, method),
-        gainsched_indices=[3, 2])
+        unicycle, (gains, points),
+        gainsched_indices=[3, 2], gainsched_method=method)
 
     # Check the gain at the selected points
     for speed, angle in points:
@@ -849,8 +848,8 @@ def test_gainsched_unicycle(unicycle, method):
     # 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, 'nearest'),
-            gainsched_indices=['ud[0]', 2])
+            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)
@@ -931,6 +930,11 @@ def test_gainsched_errors(unicycle):
         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(
@@ -944,4 +948,5 @@ def test_gainsched_errors(unicycle):
     # Unknown gain scheduling method
     with pytest.raises(ControlArgument, match="unknown gain scheduling method"):
         ctrl, clsys = ct.create_statefbk_iosystem(
-            unicycle, (gains, points, 'stuff'), gainsched_indices=[3, 2])
+            unicycle, (gains, points),
+            gainsched_indices=[3, 2], gainsched_method='unknown')
diff --git a/doc/iosys.rst b/doc/iosys.rst
index 1da7f5884..994d4900e 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,86 @@ 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 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 511f76b8e..a5ec2e0c8 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 7ddc6b5b8..88eed9a95 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 @@ def trajgen_output(t, x, u, params):
 #                          +----------- [-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 @@ def trajgen_output(t, x, u, params):
     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)')

From 0bfe92142367f4580cb173402c286b846e024bdb Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Wed, 28 Dec 2022 20:39:25 -0800
Subject: [PATCH 5/6] small documentation updates

---
 control/statefbk.py | 35 ++++++++++++++++++-----------------
 doc/iosys.rst       |  8 ++++++--
 2 files changed, 24 insertions(+), 19 deletions(-)

diff --git a/control/statefbk.py b/control/statefbk.py
index 9052d159d..87f191ff0 100644
--- a/control/statefbk.py
+++ b/control/statefbk.py
@@ -676,17 +676,17 @@ def create_statefbk_iosystem(
         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
-        either the system state x or the system output y (if an estimator is
+        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.
 
     gainsched_method : str, optional
-        The method to use for gain scheduling.  Possible values include
-        `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.
+        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
@@ -700,20 +700,21 @@ def create_statefbk_iosystem(
     -------
     ctrl : InputOutputSystem
         Input/output system representing the controller.  This system takes
-        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 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).
-        If a gain scheduled controller is specified, the gain (proportional
-        and integral) are evaluated using the scheduling variables specified
-        by ``gainsched_indices``.
+        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
     ----------------
diff --git a/doc/iosys.rst b/doc/iosys.rst
index 994d4900e..1f5f21e69 100644
--- a/doc/iosys.rst
+++ b/doc/iosys.rst
@@ -308,8 +308,12 @@ and the control action will be given by
   u = u_\text{d} - K\text{p} (x - x_\text{d}) -
       K_\text{i} \int C (x - x_\text{d}) dt.
 
-(If an estimator is specified, :math:`\hat x` will be used in place of
-:math:`x`.)
+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

From 51f3b6b3f9b3860813c5003dcb0e442a055be9e0 Mon Sep 17 00:00:00 2001
From: Richard Murray <murray@cds.caltech.edu>
Date: Fri, 30 Dec 2022 08:29:03 -0800
Subject: [PATCH 6/6] set default for gainsched_indices to xd + add unit test

---
 control/statefbk.py            |  6 +++---
 control/tests/statefbk_test.py | 27 +++++++++++++++++++++++++++
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/control/statefbk.py b/control/statefbk.py
index 87f191ff0..fa4ac3551 100644
--- a/control/statefbk.py
+++ b/control/statefbk.py
@@ -679,7 +679,7 @@ def create_statefbk_iosystem(
         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.
+        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'
@@ -812,8 +812,8 @@ def create_statefbk_iosystem(
 
     # Process gainscheduling variables, if present
     if gainsched:
-        # Create a copy of the scheduling variable indices (default = empty)
-        gainsched_indices = [] if gainsched_indices is None \
+        # 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
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
index 343c8790b..9e8feb4c9 100644
--- a/control/tests/statefbk_test.py
+++ b/control/tests/statefbk_test.py
@@ -905,6 +905,33 @@ def test_gainsched_unicycle(unicycle, method):
     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]