set default for gainsched_indices to xd + add unit test

murrayrm · murrayrm · commit 51f3b6b3f9b3 · 2022-12-30T08:29:03.000-08:00
diff --git 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
@@ -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]
