Skip to content

Commit c3488cd

Browse files
murrayrmmurrayrm
authored
Merge pull request #827 from murrayrm/gainsched-23Dec2022
Add gain scheduling to create_statefbk_iosystem()
2 parents 508dc7f + 51f3b6b commit c3488cd

File tree

7 files changed

+592
-75
lines changed

7 files changed

+592
-75
lines changed

‎control/iosys.py‎

Lines changed: 18 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -536,6 +536,10 @@ def linearize(self, x0, u0, t=0, params=None, eps=1e-6,
536536
# functions.
537537
#
538538

539+
# If x0 and u0 are specified as lists, concatenate the elements
540+
x0 = _concatenate_list_elements(x0, 'x0')
541+
u0 = _concatenate_list_elements(u0, 'u0')
542+
539543
# Figure out dimensions if they were not specified.
540544
nstates = _find_size(self.nstates, x0)
541545
ninputs = _find_size(self.ninputs, u0)
@@ -1762,14 +1766,7 @@ def input_output_response(
17621766
ninputs = U.shape[0]
17631767

17641768
# If we were passed a list of initial states, concatenate them
1765-
if isinstance(X0, (tuple, list)):
1766-
X0_list = []
1767-
for i, x0 in enumerate(X0):
1768-
x0 = np.array(x0).reshape(-1) # convert everyting to 1D array
1769-
X0_list += x0.tolist() # add elements to initial state
1770-
1771-
# Save the newly created input vector
1772-
X0 = np.array(X0_list)
1769+
X0 = _concatenate_list_elements(X0, 'X0')
17731770

17741771
# If the initial state is too short, make it longer (NB: sys.nstates
17751772
# could be None if nstates comes from size of initial condition)
@@ -2393,6 +2390,19 @@ def ss(*args, **kwargs):
23932390
return sys
23942391

23952392

2393+
# Utility function to allow lists states, inputs
2394+
def _concatenate_list_elements(X, name='X'):
2395+
# If we were passed a list, concatenate the elements together
2396+
if isinstance(X, (tuple, list)):
2397+
X_list = []
2398+
for i, x in enumerate(X):
2399+
x = np.array(x).reshape(-1) # convert everyting to 1D array
2400+
X_list += x.tolist() # add elements to initial state
2401+
return np.array(X_list)
2402+
2403+
# Otherwise, do nothing
2404+
return X
2405+
23962406
def rss(states=1, outputs=1, inputs=1, strictly_proper=False, **kwargs):
23972407
"""Create a stable random state space object.
23982408

‎control/statefbk.py‎

Lines changed: 178 additions & 53 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,7 @@
4141

4242
# External packages and modules
4343
import numpy as np
44+
import scipy as sp
4445

4546
from . import statesp
4647
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):
7172
sb03od = None
7273

7374

74-
__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr',
75+
__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr',
7576
'dlqr', 'acker', 'create_statefbk_iosystem']
7677

7778

@@ -596,8 +597,10 @@ def dlqr(*args, **kwargs):
596597

597598
# Function to create an I/O sytems representing a state feedback controller
598599
def create_statefbk_iosystem(
599-
sys, K, integral_action=None, xd_labels='xd[{i}]', ud_labels='ud[{i}]',
600-
estimator=None, type='linear'):
600+
sys, gain, integral_action=None, estimator=None, type=None,
601+
xd_labels='xd[{i}]', ud_labels='ud[{i}]', gainsched_indices=None,
602+
gainsched_method='linear', name=None, inputs=None, outputs=None,
603+
states=None):
601604
"""Create an I/O system using a (full) state feedback controller
602605
603606
This function creates an input/output system that implements a
@@ -613,31 +616,47 @@ def create_statefbk_iosystem(
613616
feedback gain (eg, from LQR). The function returns the controller
614617
``ctrl`` and the closed loop systems ``clsys``, both as I/O systems.
615618
619+
A gain scheduled controller can also be created, by passing a list of
620+
gains and a corresponding list of values of a set of scheduling
621+
variables. In this case, the controller has the form
622+
623+
u = ud - K_p(mu) (x - xd) - K_i(mu) integral(C x - C x_d)
624+
625+
where mu represents the scheduling variable.
626+
616627
Parameters
617628
----------
618629
sys : InputOutputSystem
619630
The I/O system that represents the process dynamics. If no estimator
620631
is given, the output of this system should represent the full state.
621632
622-
K : ndarray
623-
The state feedback gain. This matrix defines the gains to be
624-
applied to the system. If ``integral_action`` is None, then the
625-
dimensions of this array should be (sys.ninputs, sys.nstates). If
626-
`integral action` is set to a matrix or a function, then additional
627-
columns represent the gains of the integral states of the
628-
controller.
633+
gain : ndarray or tuple
634+
If a array is give, it represents the state feedback gain (K).
635+
This matrix defines the gains to be applied to the system. If
636+
``integral_action`` is None, then the dimensions of this array
637+
should be (sys.ninputs, sys.nstates). If `integral action` is
638+
set to a matrix or a function, then additional columns
639+
represent the gains of the integral states of the controller.
640+
641+
If a tuple is given, then it specifies a gain schedule. The tuple
642+
should be of the form ``(gains, points)`` where gains is a list of
643+
gains :math:`K_j` and points is a list of values :math:`\\mu_j` at
644+
which the gains are computed. The `gainsched_indices` parameter
645+
should be used to specify the scheduling variables.
629646
630647
xd_labels, ud_labels : str or list of str, optional
631648
Set the name of the signals to use for the desired state and inputs.
632649
If a single string is specified, it should be a format string using
633-
the variable ``i`` as an index. Otherwise, a list of strings matching
634-
the size of xd and ud, respectively, should be used. Default is
635-
``'xd[{i}]'`` for xd_labels and ``'xd[{i}]'`` for ud_labels.
650+
the variable ``i`` as an index. Otherwise, a list of strings
651+
matching the size of xd and ud, respectively, should be used.
652+
Default is ``'xd[{i}]'`` for xd_labels and ``'ud[{i}]'`` for
653+
ud_labels. These settings can also be overriden using the `inputs`
654+
keyword.
636655
637656
integral_action : None, ndarray, or func, optional
638657
If this keyword is specified, the controller can include integral
639-
action in addition to state feedback. If ``integral_action`` is an
640-
ndarray, it will be multiplied by the current and desired state to
658+
action in addition to state feedback. If ``integral_action`` is a
659+
matrix, it will be multiplied by the current and desired state to
641660
generate the error for the internal integrator states of the control
642661
law. If ``integral_action`` is a function ``h``, that function will
643662
be called with the signature h(t, x, u, params) to obtain the
@@ -646,30 +665,63 @@ def create_statefbk_iosystem(
646665
``K`` matrix.
647666
648667
estimator : InputOutputSystem, optional
649-
If an estimator is provided, using the states of the estimator as
668+
If an estimator is provided, use the states of the estimator as
650669
the system inputs for the controller.
651670
652-
type : 'nonlinear' or 'linear', optional
653-
Set the type of controller to create. The default is a linear
654-
controller implementing the LQR regulator. If the type is 'nonlinear',
655-
a :class:NonlinearIOSystem is created instead, with the gain ``K`` as
656-
a parameter (allowing modifications of the gain at runtime).
671+
gainsched_indices : list of int or str, optional
672+
If a gain scheduled controller is specified, specify the indices of
673+
the controller input to use for scheduling the gain. The input to
674+
the controller is the desired state xd, the desired input ud, and
675+
the system state x (or state estimate xhat, if an estimator is
676+
given). The indices can either be specified as integer offsets into
677+
the input vector or as strings matching the signal names of the
678+
input vector. The default is to use the desire state xd.
679+
680+
gainsched_method : str, optional
681+
The method to use for gain scheduling. Possible values are 'linear'
682+
(default), 'nearest', and 'cubic'. More information is available in
683+
:func:`scipy.interpolate.griddata`. For points outside of the convex
684+
hull of the scheduling points, the gain at the nearest point is
685+
used.
686+
687+
type : 'linear' or 'nonlinear', optional
688+
Set the type of controller to create. The default for a linear gain
689+
is a linear controller implementing the LQR regulator. If the type
690+
is 'nonlinear', a :class:NonlinearIOSystem is created instead, with
691+
the gain ``K`` as a parameter (allowing modifications of the gain at
692+
runtime). If the gain parameter is a tuple, then a nonlinear,
693+
gain-scheduled controller is created.
657694
658695
Returns
659696
-------
660697
ctrl : InputOutputSystem
661698
Input/output system representing the controller. This system takes
662-
as inputs the desired state xd, the desired input ud, and the system
663-
state x. It outputs the controller action u according to the
664-
formula u = ud - K(x - xd). If the keyword `integral_action` is
665-
specified, then an additional set of integrators is included in the
666-
control system (with the gain matrix K having the integral gains
667-
appended after the state gains).
699+
as inputs the desired state ``xd``, the desired input ``ud``, and
700+
either the system state ``x`` or the estimated state ``xhat``. It
701+
outputs the controller action u according to the formula :math:`u =
702+
u_d - K(x - x_d)`. If the keyword ``integral_action`` is specified,
703+
then an additional set of integrators is included in the control
704+
system (with the gain matrix ``K`` having the integral gains
705+
appended after the state gains). If a gain scheduled controller is
706+
specified, the gain (proportional and integral) are evaluated using
707+
the scheduling variables specified by ``gainsched_indices``.
668708
669709
clsys : InputOutputSystem
670710
Input/output system representing the closed loop system. This
671-
systems takes as inputs the desired trajectory (xd, ud) and outputs
672-
the system state x and the applied input u (vertically stacked).
711+
systems takes as inputs the desired trajectory ``(xd, ud)`` and
712+
outputs the system state ``x`` and the applied input ``u``
713+
(vertically stacked).
714+
715+
Other Parameters
716+
----------------
717+
inputs, outputs : str, or list of str, optional
718+
List of strings that name the individual signals of the transformed
719+
system. If not given, the inputs and outputs are the same as the
720+
original system.
721+
722+
name : string, optional
723+
System name. If unspecified, a generic name <sys[id]> is generated
724+
with a unique integer id.
673725
674726
"""
675727
# Make sure that we were passed an I/O system as an input
@@ -705,53 +757,127 @@ def create_statefbk_iosystem(
705757
C = np.zeros((0, sys.nstates))
706758

707759
# Check to make sure that state feedback has the right shape
708-
if not isinstance(K, np.ndarray) or \
709-
K.shape != (sys.ninputs, estimator.noutputs + nintegrators):
760+
if isinstance(gain, np.ndarray):
761+
K = gain
762+
if K.shape != (sys.ninputs, estimator.noutputs + nintegrators):
763+
raise ControlArgument(
764+
f'Control gain must be an array of size {sys.ninputs}'
765+
f'x {sys.nstates}' +
766+
(f'+{nintegrators}' if nintegrators > 0 else ''))
767+
gainsched = False
768+
769+
elif isinstance(gain, tuple):
770+
# Check for gain scheduled controller
771+
if len(gain) != 2:
772+
raise ControlArgument("gain must be a 2-tuple for gain scheduling")
773+
gains, points = gain[0:2]
774+
775+
# Stack gains and points if past as a list
776+
gains = np.stack(gains)
777+
points = np.stack(points)
778+
gainsched=True
779+
780+
else:
781+
raise ControlArgument("gain must be an array or a tuple")
782+
783+
# Decide on the type of system to create
784+
if gainsched and type == 'linear':
710785
raise ControlArgument(
711-
f'Control gain must be an array of size {sys.ninputs}'
712-
f'x {sys.nstates}' +
713-
(f'+{nintegrators}' if nintegrators > 0 else ''))
786+
"type 'linear' not allowed for gain scheduled controller")
787+
elif type is None:
788+
type = 'nonlinear' if gainsched else 'linear'
789+
elif type not in {'linear', 'nonlinear'}:
790+
raise ControlArgument(f"unknown type '{type}'")
714791

715792
# Figure out the labels to use
716793
if isinstance(xd_labels, str):
717-
# Gnerate the list of labels using the argument as a format string
794+
# Generate the list of labels using the argument as a format string
718795
xd_labels = [xd_labels.format(i=i) for i in range(sys.nstates)]
719796

720797
if isinstance(ud_labels, str):
721-
# Gnerate the list of labels using the argument as a format string
798+
# Generate the list of labels using the argument as a format string
722799
ud_labels = [ud_labels.format(i=i) for i in range(sys.ninputs)]
723800

801+
# Create the signal and system names
802+
if inputs is None:
803+
inputs = xd_labels + ud_labels + estimator.output_labels
804+
if outputs is None:
805+
outputs = list(sys.input_index.keys())
806+
if states is None:
807+
states = nintegrators
808+
809+
# Process gainscheduling variables, if present
810+
if gainsched:
811+
# Create a copy of the scheduling variable indices (default = xd)
812+
gainsched_indices = range(sys.nstates) if gainsched_indices is None \
813+
else list(gainsched_indices)
814+
815+
# Make sure the scheduling variable indices are the right length
816+
if len(gainsched_indices) != points.shape[1]:
817+
raise ControlArgument(
818+
"length of gainsched_indices must match dimension of"
819+
" scheduling variables")
820+
821+
# Process scheduling variables
822+
for i, idx in enumerate(gainsched_indices):
823+
if isinstance(idx, str):
824+
gainsched_indices[i] = inputs.index(gainsched_indices[i])
825+
826+
# Create interpolating function
827+
if gainsched_method == 'nearest':
828+
_interp = sp.interpolate.NearestNDInterpolator(points, gains)
829+
def _nearest(mu):
830+
raise SystemError(f"could not find nearest gain at mu = {mu}")
831+
elif gainsched_method == 'linear':
832+
_interp = sp.interpolate.LinearNDInterpolator(points, gains)
833+
_nearest = sp.interpolate.NearestNDInterpolator(points, gains)
834+
elif gainsched_method == 'cubic':
835+
_interp = sp.interpolate.CloughTocher2DInterpolator(points, gains)
836+
_nearest = sp.interpolate.NearestNDInterpolator(points, gains)
837+
else:
838+
raise ControlArgument(
839+
f"unknown gain scheduling method '{gainsched_method}'")
840+
841+
def _compute_gain(mu):
842+
K = _interp(mu)
843+
if np.isnan(K).any():
844+
K = _nearest(mu)
845+
return K
846+
724847
# Define the controller system
725848
if type == 'nonlinear':
726849
# Create an I/O system for the state feedback gains
727-
def _control_update(t, x, inputs, params):
850+
def _control_update(t, states, inputs, params):
728851
# Split input into desired state, nominal input, and current state
729852
xd_vec = inputs[0:sys.nstates]
730853
x_vec = inputs[-estimator.nstates:]
731854

732855
# Compute the integral error in the xy coordinates
733-
return C @ x_vec - C @ xd_vec
856+
return C @ (x_vec - xd_vec)
734857

735-
def _control_output(t, e, z, params):
736-
K = params.get('K')
858+
def _control_output(t, states, inputs, params):
859+
if gainsched:
860+
mu = inputs[gainsched_indices]
861+
K_ = _compute_gain(mu)
862+
else:
863+
K_ = params.get('K')
737864

738865
# Split input into desired state, nominal input, and current state
739-
xd_vec = z[0:sys.nstates]
740-
ud_vec = z[sys.nstates:sys.nstates + sys.ninputs]
741-
x_vec = z[-sys.nstates:]
866+
xd_vec = inputs[0:sys.nstates]
867+
ud_vec = inputs[sys.nstates:sys.nstates + sys.ninputs]
868+
x_vec = inputs[-sys.nstates:]
742869

743870
# Compute the control law
744-
u = ud_vec - K[:, 0:sys.nstates] @ (x_vec - xd_vec)
871+
u = ud_vec - K_[:, 0:sys.nstates] @ (x_vec - xd_vec)
745872
if nintegrators > 0:
746-
u -= K[:, sys.nstates:] @ e
873+
u -= K_[:, sys.nstates:] @ states
747874

748875
return u
749876

877+
params = {} if gainsched else {'K': K}
750878
ctrl = NonlinearIOSystem(
751-
_control_update, _control_output, name='control',
752-
inputs=xd_labels + ud_labels + estimator.output_labels,
753-
outputs=list(sys.input_index.keys()), params={'K': K},
754-
states=nintegrators)
879+
_control_update, _control_output, name=name, inputs=inputs,
880+
outputs=outputs, states=states, params=params)
755881

756882
elif type == 'linear' or type is None:
757883
# Create the matrices implementing the controller
@@ -768,9 +894,8 @@ def _control_output(t, e, z, params):
768894
])
769895

770896
ctrl = ss(
771-
A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name='control',
772-
inputs=xd_labels + ud_labels + estimator.output_labels,
773-
outputs=list(sys.input_index.keys()), states=nintegrators)
897+
A_lqr, B_lqr, C_lqr, D_lqr, dt=sys.dt, name=name,
898+
inputs=inputs, outputs=outputs, states=states)
774899

775900
else:
776901
raise ControlArgument(f"unknown type '{type}'")
@@ -779,7 +904,7 @@ def _control_output(t, e, z, params):
779904
closed = interconnect(
780905
[sys, ctrl] if estimator == sys else [sys, ctrl, estimator],
781906
name=sys.name + "_" + ctrl.name,
782-
inplist=xd_labels + ud_labels, inputs=xd_labels + ud_labels,
907+
inplist=inputs[:-sys.nstates], inputs=inputs[:-sys.nstates],
783908
outlist=sys.output_labels + sys.input_labels,
784909
outputs=sys.output_labels + sys.input_labels
785910
)

0 commit comments

Comments
 (0)