Skip to content

Commit 213905c

Browse files
murrayrmmurrayrm
committed
add integral_action option to lqr/dlqr
1 parent 2b13747 commit 213905c

File tree

2 files changed

+179
-13
lines changed

2 files changed

+179
-13
lines changed

‎control/statefbk.py‎

Lines changed: 80 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -578,7 +578,7 @@ def acker(A, B, poles):
578578
return _ssmatrix(K)
579579

580580

581-
def lqr(*args, **keywords):
581+
def lqr(*args, **kwargs):
582582
"""lqr(A, B, Q, R[, N])
583583
584584
Linear quadratic regulator design
@@ -646,18 +646,15 @@ def lqr(*args, **keywords):
646646
# Process the arguments and figure out what inputs we received
647647
#
648648

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

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

656-
# If we were passed a discrete time system as the first arg, use dlqr()
657-
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
658-
# Call dlqr
659-
return dlqr(*args, **keywords)
660-
661658
# If we were passed a state space system, use that to get system matrices
662659
if isinstance(args[0], StateSpace):
663660
A = np.array(args[0].A, ndmin=2, dtype=float)
@@ -682,12 +679,47 @@ def lqr(*args, **keywords):
682679
else:
683680
N = None
684681

682+
#
683+
# Process keywords
684+
#
685+
686+
# Get the method to use (if specified as a keyword)
687+
method = kwargs.pop('method', None)
688+
689+
# See if we should augment the controller with integral feedback
690+
integral_action = kwargs.pop('integral_action', None)
691+
if integral_action is not None:
692+
# Figure out the size of the system
693+
nstates = A.shape[0]
694+
ninputs = B.shape[1]
695+
696+
# Make sure that the integral action argument is the right type
697+
if not isinstance(integral_action, np.ndarray):
698+
raise ControlArgument("Integral action must pass an array")
699+
elif integral_action.shape[1] != nstates:
700+
raise ControlArgument(
701+
"Integral gain output size must match system input size")
702+
703+
# Process the states to be integrated
704+
nintegrators = integral_action.shape[0]
705+
C = integral_action
706+
707+
# Augment the system with integrators
708+
A = np.block([
709+
[A, np.zeros((nstates, nintegrators))],
710+
[C, np.zeros((nintegrators, nintegrators))]
711+
])
712+
B = np.vstack([B, np.zeros((nintegrators, ninputs))])
713+
714+
if kwargs:
715+
raise TypeError("unrecognized keywords: ", str(kwargs))
716+
685717
# Compute the result (dimension and symmetry checking done in care())
686718
X, L, G = care(A, B, Q, R, N, None, method=method, S_s="N")
687719
return G, X, L
688720

689721

690-
def dlqr(*args, **keywords):
722+
def dlqr(*args, **kwargs):
691723
"""dlqr(A, B, Q, R[, N])
692724
693725
Discrete-time linear quadratic regulator design
@@ -747,9 +779,6 @@ def dlqr(*args, **keywords):
747779
# Process the arguments and figure out what inputs we received
748780
#
749781

750-
# Get the method to use (if specified as a keyword)
751-
method = keywords.get('method', None)
752-
753782
# Get the system description
754783
if (len(args) < 3):
755784
raise ControlArgument("not enough input arguments")
@@ -782,6 +811,39 @@ def dlqr(*args, **keywords):
782811
else:
783812
N = np.zeros((Q.shape[0], R.shape[1]))
784813

814+
#
815+
# Process keywords
816+
#
817+
818+
# Get the method to use (if specified as a keyword)
819+
method = kwargs.pop('method', None)
820+
821+
# See if we should augment the controller with integral feedback
822+
integral_action = kwargs.pop('integral_action', None)
823+
if integral_action is not None:
824+
# Figure out the size of the system
825+
nstates = A.shape[0]
826+
ninputs = B.shape[1]
827+
828+
if not isinstance(integral_action, np.ndarray):
829+
raise ControlArgument("Integral action must pass an array")
830+
elif integral_action.shape[1] != nstates:
831+
raise ControlArgument(
832+
"Integral gain output size must match system input size")
833+
else:
834+
nintegrators = integral_action.shape[0]
835+
C = integral_action
836+
837+
# Augment the system with integrators
838+
A = np.block([
839+
[A, np.zeros((nstates, nintegrators))],
840+
[C, np.eye(nintegrators)]
841+
])
842+
B = np.vstack([B, np.zeros((nintegrators, ninputs))])
843+
844+
if kwargs:
845+
raise TypeError("unrecognized keywords: ", str(kwargs))
846+
785847
# Compute the result (dimension and symmetry checking done in dare())
786848
S, E, K = dare(A, B, Q, R, N, method=method, S_s="N")
787849
return _ssmatrix(K), _ssmatrix(S), E
@@ -948,7 +1010,12 @@ def _control_output(t, e, z, params):
9481010

9491011
elif type == 'linear' or type is None:
9501012
# Create the matrices implementing the controller
951-
A_lqr = np.zeros((C.shape[0], C.shape[0]))
1013+
if isctime(sys):
1014+
# Continuous time: integrator
1015+
A_lqr = np.zeros((C.shape[0], C.shape[0]))
1016+
else:
1017+
# Discrete time: summer
1018+
A_lqr = np.eye(C.shape[0])
9521019
B_lqr = np.hstack([-C, np.zeros((C.shape[0], sys.ninputs)), C])
9531020
C_lqr = -K[:, sys.nstates:]
9541021
D_lqr = np.hstack([

‎control/tests/statefbk_test.py‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -727,3 +727,102 @@ def test_lqr_iosys(self, nstates, ninputs, noutputs, nintegrators, type):
727727
np.testing.assert_array_almost_equal(clsys.B, Bc)
728728
np.testing.assert_array_almost_equal(clsys.C, Cc)
729729
np.testing.assert_array_almost_equal(clsys.D, Dc)
730+
731+
def test_lqr_integral_continuous(self):
732+
# Generate a continuous time system for testing
733+
sys = ct.rss(4, 4, 2, strictly_proper=True)
734+
sys.C = np.eye(4) # reset output to be full state
735+
C_int = np.eye(2, 4) # integrate outputs for first two states
736+
nintegrators = C_int.shape[0]
737+
738+
# Generate a controller with integral action
739+
K, _, _ = ct.lqr(
740+
sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs),
741+
integral_action=C_int)
742+
Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:]
743+
744+
# Create an I/O system for the controller
745+
ctrl, clsys = ct.create_statefbk_iosystem(
746+
sys, K, integral_action=C_int)
747+
748+
# Construct the state space matrices for the controller
749+
# Controller inputs = xd, ud, x
750+
# Controller state = z (integral of x-xd)
751+
# Controller output = ud - Kp(x - xd) - Ki z
752+
A_ctrl = np.zeros((nintegrators, nintegrators))
753+
B_ctrl = np.block([
754+
[-C_int, np.zeros((nintegrators, sys.ninputs)), C_int]
755+
])
756+
C_ctrl = -K[:, sys.nstates:]
757+
D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]])
758+
759+
# Check to make sure everything matches
760+
np.testing.assert_array_almost_equal(ctrl.A, A_ctrl)
761+
np.testing.assert_array_almost_equal(ctrl.B, B_ctrl)
762+
np.testing.assert_array_almost_equal(ctrl.C, C_ctrl)
763+
np.testing.assert_array_almost_equal(ctrl.D, D_ctrl)
764+
765+
# Construct the state space matrices for the closed loop system
766+
A_clsys = np.block([
767+
[sys.A - sys.B @ Kp, -sys.B @ Ki],
768+
[C_int, np.zeros((nintegrators, nintegrators))]
769+
])
770+
B_clsys = np.block([
771+
[sys.B @ Kp, sys.B],
772+
[-C_int, np.zeros((nintegrators, sys.ninputs))]
773+
])
774+
C_clsys = np.block([
775+
[np.eye(sys.nstates), np.zeros((sys.nstates, nintegrators))],
776+
[-Kp, -Ki]
777+
])
778+
D_clsys = np.block([
779+
[np.zeros((sys.nstates, sys.nstates + sys.ninputs))],
780+
[Kp, np.eye(sys.ninputs)]
781+
])
782+
783+
# Check to make sure closed loop matches
784+
np.testing.assert_array_almost_equal(clsys.A, A_clsys)
785+
np.testing.assert_array_almost_equal(clsys.B, B_clsys)
786+
np.testing.assert_array_almost_equal(clsys.C, C_clsys)
787+
np.testing.assert_array_almost_equal(clsys.D, D_clsys)
788+
789+
# Check the poles of the closed loop system
790+
assert all(np.real(clsys.pole()) < 0)
791+
792+
# Make sure controller infinite zero frequency gain
793+
if slycot_check():
794+
ctrl_tf = tf(ctrl)
795+
assert abs(ctrl_tf(1e-9)[0][0]) > 1e6
796+
assert abs(ctrl_tf(1e-9)[1][1]) > 1e6
797+
798+
def test_lqr_integral_discrete(self):
799+
# Generate a discrete time system for testing
800+
sys = ct.drss(4, 4, 2, strictly_proper=True)
801+
sys.C = np.eye(4) # reset output to be full state
802+
C_int = np.eye(2, 4) # integrate outputs for first two states
803+
nintegrators = C_int.shape[0]
804+
805+
# Generate a controller with integral action
806+
K, _, _ = ct.lqr(
807+
sys, np.eye(sys.nstates + nintegrators), np.eye(sys.ninputs),
808+
integral_action=C_int)
809+
Kp, Ki = K[:, :sys.nstates], K[:, sys.nstates:]
810+
811+
# Create an I/O system for the controller
812+
ctrl, clsys = ct.create_statefbk_iosystem(
813+
sys, K, integral_action=C_int)
814+
815+
# Construct the state space matrices by hand
816+
A_ctrl = np.eye(nintegrators)
817+
B_ctrl = np.block([
818+
[-C_int, np.zeros((nintegrators, sys.ninputs)), C_int]
819+
])
820+
C_ctrl = -K[:, sys.nstates:]
821+
D_ctrl = np.block([[Kp, np.eye(nintegrators), -Kp]])
822+
823+
# Check to make sure everything matches
824+
assert ct.isdtime(clsys)
825+
np.testing.assert_array_almost_equal(ctrl.A, A_ctrl)
826+
np.testing.assert_array_almost_equal(ctrl.B, B_ctrl)
827+
np.testing.assert_array_almost_equal(ctrl.C, C_ctrl)
828+
np.testing.assert_array_almost_equal(ctrl.D, D_ctrl)

0 commit comments

Comments
 (0)