add support for 1D gain scheduling

murrayrm · bnavigator · commit 91c0260b7b69 · 2023-02-02T13:18:55.000+01:00
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -826,6 +826,10 @@ def create_statefbk_iosystem(
         gainsched_indices = range(sys.nstates) if gainsched_indices is None \
             else list(gainsched_indices)
 
+        # If points is a 1D list, convert to 2D
+        if points.ndim == 1:
+            points = points.reshape(-1, 1)
+
         # Make sure the scheduling variable indices are the right length
         if len(gainsched_indices) != points.shape[1]:
             raise ControlArgument(
@@ -838,7 +842,12 @@ def create_statefbk_iosystem(
                 gainsched_indices[i] = inputs.index(gainsched_indices[i])
 
         # Create interpolating function
-        if gainsched_method == 'nearest':
+        if points.shape[1] < 2:
+            _interp = sp.interpolate.interp1d(
+                points[:, 0], gains, axis=0, kind=gainsched_method)
+            _nearest = sp.interpolate.interp1d(
+                points[:, 0], gains, axis=0, kind='nearest')
+        elif gainsched_method == 'nearest':
             _interp = sp.interpolate.NearestNDInterpolator(points, gains)
             def _nearest(mu):
                 raise SystemError(f"could not find nearest gain at mu = {mu}")
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -924,6 +924,34 @@ def test_gainsched_unicycle(unicycle, method):
         resp.states[:, -1], Xd[:, -1], atol=1e-2, rtol=1e-2)
 
 
+@pytest.mark.parametrize("method", ['nearest', 'linear', 'cubic'])
+def test_gainsched_1d(method):
+    # Define a linear system to test
+    sys = ct.ss([[-1, 0.1], [0, -2]], [[0], [1]], np.eye(2), 0)
+
+    # Define gains for the first state only
+    points = [-1, 0, 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) * 1.1     # Start outside defined range
+
+    # Create a controller and simulate the initial response
+    gs_ctrl, gs_clsys = ct.create_statefbk_iosystem(
+        sys, (gains, points), gainsched_indices=[0])
+    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_default_indices():
     # Define a linear system to test
     sys = ct.ss([[-1, 0.1], [0, -2]], [[0], [1]], np.eye(2), 0)
@@ -937,7 +965,7 @@ def test_gainsched_default_indices():
 
     # Define the paramters for the simulations
     timepts = np.linspace(0, 10, 100)
-    X0 = np.ones(sys.nstates) * 0.9
+    X0 = np.ones(sys.nstates) * 1.1     # Start outside defined range
 
     # Create a controller and simulate the initial response
     gs_ctrl, gs_clsys = ct.create_statefbk_iosystem(sys, (gains, points))
