def test_lqe_discrete():
"""Test overloading of lqe operator for discrete time systems"""
csys = ct.rss(2, 1, 1)
dsys = ct.drss(2, 1, 1)
Q = np.eye(1)
R = np.eye(1)
# Calling with a system versus explicit A, B should be the sam
K_csys, S_csys, E_csys = ct.lqe(csys, Q, R)
K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R)
np.testing.assert_almost_equal(K_csys, K_expl)
np.testing.assert_almost_equal(S_csys, S_expl)
np.testing.assert_almost_equal(E_csys, E_expl)
# Calling lqe() with a discrete time system should call dlqe()
K_lqe, S_lqe, E_lqe = ct.lqe(dsys, Q, R)
K_dlqe, S_dlqe, E_dlqe = ct.dlqe(dsys, Q, R)
np.testing.assert_almost_equal(K_lqe, K_dlqe)
np.testing.assert_almost_equal(S_lqe, S_dlqe)
np.testing.assert_almost_equal(E_lqe, E_dlqe)
# Calling lqe() with no timebase should call lqe()
asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None)
K_asys, S_asys, E_asys = ct.lqe(asys, Q, R)
K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R)
np.testing.assert_almost_equal(K_asys, K_expl)
np.testing.assert_almost_equal(S_asys, S_expl)
np.testing.assert_almost_equal(E_asys, E_expl)
# Calling dlqe() with a continuous time system should raise an error
with pytest.raises(ControlArgument, match="called with a continuous"):
K, S, E = ct.dlqe(csys, Q, R)
def test_LQE(matarrayin, method):
if method == 'slycot' and not slycot_check():
return
A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.])
L, P, poles = lqe(A, G, C, QN, RN, method=method)
check_LQE(L, P, poles, G, QN, RN)
def __init__(self, Ts):
self.A = CTRL.A
self.B = CTRL.B
self.C = CTRL.C
self.L, _, _ = control.lqe(CTRL.A, np.eye(9), CTRL.C, SENSOR.Q_N,
SENSOR.R_N)
self.Ts = Ts
g = -10
d = 1
# System representation
A = np.array([[0, 1, 0, 0], [0, -d / M, -m * g / M, 0], [0, 0, 0, 1],
[0, d / (M * L), (m + M) * g / (M * L), 0]])
B = np.array([[0], [1 / M], [0], [-1 / (M * L)]])
C = np.array([[1, 0, 0, 0]])
D = np.zeros(
(np.size(C, 0),
np.size(B, 1))) # D has the same numbers of rows as C, and columns as B
# Disturbance and noise covariances
Vd = 0.1 * np.eye(4)
Vn = 1
# B matrix considering the new inputs (noise and disturbance)
BF = np.column_stack([B, Vd, 0 * B])
sys_single_state = control.ss(A, BF, C, [0, 0, 0, 0, 0, Vn])
sys_full_state = control.ss(A, BF, np.eye(4), np.zeros((4, np.size(BF, 1))))
[Kf, P, E] = control.lqe(A, Vd, C, Vd, Vn)
sys_kf = control.ss(A - Kf * C, np.column_stack([B, Kf]), np.eye(4),
0 * (np.column_stack([B, Kf])))
def test_LQE(self, matarrayin):
A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.])
L, P, poles = lqe(A, G, C, QN, RN)
self.check_LQE(L, P, poles, G, QN, RN)
def test_lqr_iosys(self, nstates, ninputs, noutputs, nintegrators, type):
# Create the system to be controlled (and estimator)
# TODO: make sure it is controllable?
if noutputs == 0:
# Create a system with full state output
sys = ct.rss(nstates, nstates, ninputs, strictly_proper=True)
sys.C = np.eye(nstates)
est = None
else:
# Create a system with of the desired size
sys = ct.rss(nstates, noutputs, ninputs, strictly_proper=True)
# Create an estimator with different signal names
L, _, _ = ct.lqe(sys.A, sys.B, sys.C, np.eye(ninputs),
np.eye(noutputs))
est = ss(sys.A - L @ sys.C,
np.hstack([L, sys.B]),
np.eye(nstates),
0,
inputs=sys.output_labels + sys.input_labels,
outputs=[f'xhat[{i}]' for i in range(nstates)])
# Decide whether to include integral action
if nintegrators:
# Choose the first 'n' outputs as integral terms
C_int = np.eye(nintegrators, nstates)
# Set up an augmented system for LQR computation
# TODO: move this computation into LQR
A_aug = np.block([[sys.A,
np.zeros((sys.nstates, nintegrators))],
[C_int,
np.zeros((nintegrators, nintegrators))]])
B_aug = np.vstack([sys.B, np.zeros((nintegrators, ninputs))])
C_aug = np.hstack(
[sys.C, np.zeros((sys.C.shape[0], nintegrators))])
aug = ss(A_aug, B_aug, C_aug, 0)
else:
C_int = np.zeros((0, nstates))
aug = sys
# Design an LQR controller
K, _, _ = ct.lqr(aug, np.eye(nstates + nintegrators), np.eye(ninputs))
Kp, Ki = K[:, :nstates], K[:, nstates:]
# Create an I/O system for the controller
ctrl, clsys = ct.create_statefbk_iosystem(sys,
K,
integral_action=C_int,
estimator=est,
type=type)
# If we used a nonlinear controller, linearize it for testing
if type == 'nonlinear':
clsys = clsys.linearize(0, 0)
# Make sure the linear system elements are correct
if noutputs == 0:
# No estimator
Ac = np.block([[sys.A - sys.B @ Kp, -sys.B @ Ki],
[C_int,
np.zeros((nintegrators, nintegrators))]])
Bc = np.block([[sys.B @ Kp, sys.B],
[-C_int, np.zeros((nintegrators, ninputs))]])
Cc = np.block(
[[np.eye(nstates),
np.zeros((nstates, nintegrators))], [-Kp, -Ki]])
Dc = np.block([[np.zeros((nstates, nstates + ninputs))],
[Kp, np.eye(ninputs)]])
else:
# Estimator
Be1, Be2 = est.B[:, :noutputs], est.B[:, noutputs:]
Ac = np.block(
[[sys.A, -sys.B @ Ki, -sys.B @ Kp],
[np.zeros((nintegrators, nstates + nintegrators)), C_int],
[Be1 @ sys.C, -Be2 @ Ki, est.A - Be2 @ Kp]])
Bc = np.block([[sys.B @ Kp, sys.B],
[-C_int, np.zeros((nintegrators, ninputs))],
[Be2 @ Kp, Be2]])
Cc = np.block(
[[sys.C, np.zeros((noutputs, nintegrators + nstates))],
[np.zeros_like(Kp), -Ki, -Kp]])
Dc = np.block([[np.zeros((noutputs, nstates + ninputs))],
[Kp, np.eye(ninputs)]])
# Check to make sure everything matches
np.testing.assert_array_almost_equal(clsys.A, Ac)
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)
