Skip to content

I/O system enhancements #710

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
murrayrm merged 7 commits into from
Mar 23, 2022
Merged

I/O system enhancements #710

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
f712096
ss, rss, drss return LinearIOSystem
murrayrm Mar 13, 2022
4b0584d
create _NamedIOSystem, _NamedIOStateSystem parent classes
murrayrm Mar 14, 2022
2b13747
add create_statefbk_iosystem + unit tests
murrayrm Mar 15, 2022
213905c
add integral_action option to lqr/dlqr
murrayrm Mar 16, 2022
ca77cca
update create_statefbk_iosys/lqr/dlqr errors + unit tests
murrayrm Mar 16, 2022
effd6b6
add documentation on integral action to lqr/dqlr
murrayrm Mar 16, 2022
316945d
use super() for LTI functions
murrayrm Mar 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
add integral_action option to lqr/dlqr
  • Loading branch information
@murrayrm
murrayrm committed Mar 16, 2022
commit 213905ca1bf134ef0147da6d5d4cb87c728e66ca
93 changes: 80 additions & 13 deletions control/statefbk.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -578,7 +578,7 @@ def acker(A, B, poles):
return _ssmatrix(K)


def lqr(*args, **keywords):
def lqr(*args, **kwargs):
"""lqr(A, B, Q, R[, N])

Linear quadratic regulator design
Expand Down Expand Up @@ -646,18 +646,15 @@ def lqr(*args, **keywords):
# Process the arguments and figure out what inputs we received
#

# Get the method to use (if specified as a keyword)
method = keywords.get('method', None)
# If we were passed a discrete time system as the first arg, use dlqr()
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
# Call dlqr
return dlqr(*args, **kwargs)

# Get the system description
if (len(args) < 3):
raise ControlArgument("not enough input arguments")

# If we were passed a discrete time system as the first arg, use dlqr()
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
# Call dlqr
return dlqr(*args, **keywords)

# If we were passed a state space system, use that to get system matrices
if isinstance(args[0], StateSpace):
A = np.array(args[0].A, ndmin=2, dtype=float)
Expand All @@ -682,12 +679,47 @@ def lqr(*args, **keywords):
else:
N = None

#
# Process keywords
#

# Get the method to use (if specified as a keyword)
method = kwargs.pop('method', None)

# See if we should augment the controller with integral feedback
integral_action = kwargs.pop('integral_action', None)
if integral_action is not None:
# Figure out the size of the system
nstates = A.shape[0]
ninputs = B.shape[1]

# Make sure that the integral action argument is the right type
if not isinstance(integral_action, np.ndarray):
raise ControlArgument("Integral action must pass an array")
elif integral_action.shape[1] != nstates:
raise ControlArgument(
"Integral gain output size must match system input size")

# Process the states to be integrated
nintegrators = integral_action.shape[0]
C = integral_action

# Augment the system with integrators
A = np.block([
[A, np.zeros((nstates, nintegrators))],
[C, np.zeros((nintegrators, nintegrators))]
])
B = np.vstack([B, np.zeros((nintegrators, ninputs))])

if kwargs:
raise TypeError("unrecognized keywords: ", str(kwargs))

# Compute the result (dimension and symmetry checking done in care())
X, L, G = care(A, B, Q, R, N, None, method=method, S_s="N")
return G, X, L


def dlqr(*args, **keywords):
def dlqr(*args, **kwargs):
"""dlqr(A, B, Q, R[, N])

Discrete-time linear quadratic regulator design
Expand Down Expand Up @@ -747,9 +779,6 @@ def dlqr(*args, **keywords):
# Process the arguments and figure out what inputs we received
#

# Get the method to use (if specified as a keyword)
method = keywords.get('method', None)

# Get the system description
if (len(args) < 3):
raise ControlArgument("not enough input arguments")
Expand Down Expand Up @@ -782,6 +811,39 @@ def dlqr(*args, **keywords):
else:
N = np.zeros((Q.shape[0], R.shape[1]))

#
# Process keywords
#

# Get the method to use (if specified as a keyword)
method = kwargs.pop('method', None)

# See if we should augment the controller with integral feedback
integral_action = kwargs.pop('integral_action', None)
if integral_action is not None:
# Figure out the size of the system
nstates = A.shape[0]
ninputs = B.shape[1]

if not isinstance(integral_action, np.ndarray):
raise ControlArgument("Integral action must pass an array")
elif integral_action.shape[1] != nstates:
raise ControlArgument(
"Integral gain output size must match system input size")
else:
nintegrators = integral_action.shape[0]
C = integral_action

# Augment the system with integrators
A = np.block([
[A, np.zeros((nstates, nintegrators))],
[C, np.eye(nintegrators)]
])
B = np.vstack([B, np.zeros((nintegrators, ninputs))])

if kwargs:
raise TypeError("unrecognized keywords: ", str(kwargs))

# Compute the result (dimension and symmetry checking done in dare())
S, E, K = dare(A, B, Q, R, N, method=method, S_s="N")
return _ssmatrix(K), _ssmatrix(S), E
Expand Down Expand Up @@ -948,7 +1010,12 @@ def _control_output(t, e, z, params):

elif type == 'linear' or type is None:
# Create the matrices implementing the controller
A_lqr = np.zeros((C.shape[0], C.shape[0]))
if isctime(sys):
# Continuous time: integrator
A_lqr = np.zeros((C.shape[0], C.shape[0]))
else:
# Discrete time: summer
A_lqr = np.eye(C.shape[0])
B_lqr = np.hstack([-C, np.zeros((C.shape[0], sys.ninputs)), C])
C_lqr = -K[:, sys.nstates:]
D_lqr = np.hstack([
Expand Down
99 changes: 99 additions & 0 deletions control/tests/statefbk_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -727,3 +727,102 @@ def test_lqr_iosys(self, nstates, ninputs, noutputs, nintegrators, type):
np.testing.assert_array_almost_equal(clsys.B, Bc)
np.testing.assert_array_almost_equal(clsys.C, Cc)
np.testing.assert_array_almost_equal(clsys.D, Dc)

def test_lqr_integral_continuous(self):
# Generate a continuous time system for testing
sys = ct.rss(4, 4, 2, strictly_proper=True)
sys.C = np.eye(4) # reset output to be full state
C_int = np.eye(2, 4) # integrate outputs for first two states
nintegrators = C_int.shape[0]

# Generate a controller with integral action
K, _, _ = ct.lqr(
sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs),
integral_action=C_int)
Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:]

# Create an I/O system for the controller
ctrl, clsys = ct.create_statefbk_iosystem(
sys, K, integral_action=C_int)

# Construct the state space matrices for the controller
# Controller inputs = xd, ud, x
# Controller state = z (integral of x-xd)
# Controller output = ud - Kp(x - xd) - Ki z
A_ctrl = np.zeros((nintegrators, nintegrators))
B_ctrl = np.block([
[-C_int, np.zeros((nintegrators, sys.ninputs)), C_int]
])
C_ctrl = -K[:, sys.nstates:]
D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]])

# Check to make sure everything matches
np.testing.assert_array_almost_equal(ctrl.A, A_ctrl)
np.testing.assert_array_almost_equal(ctrl.B, B_ctrl)
np.testing.assert_array_almost_equal(ctrl.C, C_ctrl)
np.testing.assert_array_almost_equal(ctrl.D, D_ctrl)

# Construct the state space matrices for the closed loop system
A_clsys = np.block([
[sys.A - sys.B @ Kp, -sys.B @ Ki],
[C_int, np.zeros((nintegrators, nintegrators))]
])
B_clsys = np.block([
[sys.B @ Kp, sys.B],
[-C_int, np.zeros((nintegrators, sys.ninputs))]
])
C_clsys = np.block([
[np.eye(sys.nstates), np.zeros((sys.nstates, nintegrators))],
[-Kp, -Ki]
])
D_clsys = np.block([
[np.zeros((sys.nstates, sys.nstates + sys.ninputs))],
[Kp, np.eye(sys.ninputs)]
])

# Check to make sure closed loop matches
np.testing.assert_array_almost_equal(clsys.A, A_clsys)
np.testing.assert_array_almost_equal(clsys.B, B_clsys)
np.testing.assert_array_almost_equal(clsys.C, C_clsys)
np.testing.assert_array_almost_equal(clsys.D, D_clsys)

# Check the poles of the closed loop system
assert all(np.real(clsys.pole()) < 0)

# Make sure controller infinite zero frequency gain
if slycot_check():
ctrl_tf = tf(ctrl)
assert abs(ctrl_tf(1e-9)[0][0]) > 1e6
assert abs(ctrl_tf(1e-9)[1][1]) > 1e6

def test_lqr_integral_discrete(self):
# Generate a discrete time system for testing
sys = ct.drss(4, 4, 2, strictly_proper=True)
sys.C = np.eye(4) # reset output to be full state
C_int = np.eye(2, 4) # integrate outputs for first two states
nintegrators = C_int.shape[0]

# Generate a controller with integral action
K, _, _ = ct.lqr(
sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs),
integral_action=C_int)
Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:]

# Create an I/O system for the controller
ctrl, clsys = ct.create_statefbk_iosystem(
sys, K, integral_action=C_int)

# Construct the state space matrices by hand
A_ctrl = np.eye(nintegrators)
B_ctrl = np.block([
[-C_int, np.zeros((nintegrators, sys.ninputs)), C_int]
])
C_ctrl = -K[:, sys.nstates:]
D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]])

# Check to make sure everything matches
assert ct.isdtime(clsys)
np.testing.assert_array_almost_equal(ctrl.A, A_ctrl)
np.testing.assert_array_almost_equal(ctrl.B, B_ctrl)
np.testing.assert_array_almost_equal(ctrl.C, C_ctrl)
np.testing.assert_array_almost_equal(ctrl.D, D_ctrl)