def test_minimal_realization_State():
M = array([[-6.5, 0.5, 6.5, -6.5, 0., 1., 0.],
[-0.5, -5.5, -5.5, 5.5, 2., 1., 2.],
[-0.5, 0.5, 0.5, -6.5, 3., 4., 3.],
[-0.5, 0.5, -5.5, -0.5, 3., 2., 3.],
[1., 1., 0., 0., 0., 0., 0.]])
G = State(*matrix_slice(M, (1, 4), corner='sw'))
H = minimal_realization(G)
assert H.a.shape == (2, 2)
#
G = State(
array([[0., 1., 0., 0., 0.], [-0.1, -0.5, 1., -1., 0.],
[0., 0., 0., 1., 0.], [0., 0., 0., 0., 1.],
[0., 3.5, 1., -2., 2.]]), array([[0.], [1.], [0.], [0.], [1.]]),
array([[0., 3.5, 1., -1., 0.]]), array([[1.]]))
H = minimal_realization(G)
assert H.a.shape == (4, 4)
#
G = State(
array([[-2., 0., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.],
[0., -12., 4., 3.]]),
array([[1., 0.], [0., 0.], [0., 0.], [0., 1.]]),
array([[1., -9., 0., 0.], [0., -20., 0., 5.]]),
array([[0., 0.], [0., 1.]]))
H = minimal_realization(G)
assert H.a.shape == (3, 3)
def test_minimal_realization_State():
M = array([[-6.5, 0.5, 6.5, -6.5, 0., 1., 0.],
[-0.5, -5.5, -5.5, 5.5, 2., 1., 2.],
[-0.5, 0.5, 0.5, -6.5, 3., 4., 3.],
[-0.5, 0.5, -5.5, -0.5, 3., 2., 3.],
[1., 1., 0., 0., 0., 0., 0.]])
G = State(*matrix_slice(M, (1, 4), corner='sw'))
H = minimal_realization(G)
assert H.a.shape == (2, 2)
#
G = State(array([[0., 1., 0., 0., 0.],
[-0.1, -0.5, 1., -1., 0.],
[0., 0., 0., 1., 0.],
[0., 0., 0., 0., 1.],
[0., 3.5, 1., -2., 2.]]),
array([[0.], [1.], [0.], [0.], [1.]]),
array([[0., 3.5, 1., -1., 0.]]),
array([[1.]]))
H = minimal_realization(G)
assert H.a.shape == (4, 4)
#
G = State(array([[-2., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.],
[0., -12., 4., 3.]]),
array([[1., 0.], [0., 0.], [0., 0.], [0., 1.]]),
array([[1., -9., 0., 0.], [0., -20., 0., 5.]]),
array([[0., 0.], [0., 1.]]))
H = minimal_realization(G)
assert H.a.shape == (3, 3)
def test_minimal_realization_State():
M = array([[-6.5, 0.5, 6.5, -6.5, 0., 1., 0.],
[-0.5, -5.5, -5.5, 5.5, 2., 1., 2.],
[-0.5, 0.5, 0.5, -6.5, 3., 4., 3.],
[-0.5, 0.5, -5.5, -0.5, 3., 2., 3.],
[1., 1., 0., 0., 0., 0., 0.]])
G = State(*matrix_slice(M, (1, 4), corner='sw'))
H = minimal_realization(G)
assert_(H.a.shape, (2, 2))
def test_matrix_slice():
A = array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
a, b, c, d = matrix_slice(A, (1, 1))
assert_almost_equal(a, array([[1]]))
assert_almost_equal(b, array([[2, 3]]))
assert_almost_equal(c, array([[4], [7]]))
assert_almost_equal(d, array([[5, 6], [8, 9]]))
a, b, c, d = matrix_slice(A, (2, 2), 'sw')
assert_almost_equal(a, array([[1, 2]]))
assert_almost_equal(b, array([[3]]))
assert_almost_equal(c, array([[4, 5], [7, 8]]))
assert_almost_equal(d, array([[6], [9]]))
a, b, c, d = matrix_slice(A, (0, 0))
assert_almost_equal(d, array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]))
for x in (a, b, c):
assert_equal(x.size, 0)
def test_matrix_slice():
A = array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
a, b, c, d = matrix_slice(A, (1, 1))
assert_almost_equal(a, array([[1]]))
assert_almost_equal(b, array([[2, 3]]))
assert_almost_equal(c, array([[4], [7]]))
assert_almost_equal(d, array([[5, 6], [8, 9]]))
a, b, c, d = matrix_slice(A, (2, 2), 'sw')
assert_almost_equal(a, array([[1, 2]]))
assert_almost_equal(b, array([[3]]))
assert_almost_equal(c, array([[4, 5], [7, 8]]))
assert_almost_equal(d, array([[6], [9]]))
a, b, c, d = matrix_slice(A, (0, 0))
assert_almost_equal(d, array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]))
for x in (a, b, c):
assert_equal(x.size, 0)
def test_staircase():
M = array([[-6.5, 0.5, 6.5, -6.5, 0., 1., 0.],
[-0.5, -5.5, -5.5, 5.5, 2., 1., 2.],
[-0.5, 0.5, 0.5, -6.5, 3., 4., 3.],
[-0.5, 0.5, -5.5, -0.5, 3., 2., 3.],
[1., 1., 0., 0., 0., 0., 0.]])
A, B, C, D = matrix_slice(M, (1, 4), corner='sw')
a, b, c, k = staircase(A, B, C, form='o', invert=True, block_indices=True)
assert_almost_equal(a[2:, :2], zeros((2, 2)))
assert_almost_equal(k, array([1, 1]))
def test_feedback_dynamic_dynamic():
M = array([[2, 2, 1, -1],
[3, 2, -2, 3],
[1, -1, 1, -3],
[0, -1, 2, 0]])
N = array([[2, 0, 2, 1],
[-2, 1, -2, -2],
[0, 1, 1, -3],
[0, 3, -1, 2]])
a, b, c, d = matrix_slice(M, [3, 3])
k, l, m, n = matrix_slice(N, [3, 3])
G = State(a, b, c, d)
H = State(k, l, m, n)
CL = feedback(G, H)
acl = array([[2., 0., 5., 0., 3., -1.],
[3., 8., -14., 0., -9., 3.],
[1., -7., 13., 0., 9., -3.],
[0., -1., 2., 2., 0., 2.],
[0., 2., -4., -2., 1., -2.],
[0., 3., -6., 0., 1., 1.]])
bcl = array([[-1, 3, -3, 0, 0, 0]]).T
ccl = array([[0, -1, 2, 0, 0, 0]])
dcl = array([[0.]])
assert_equal(CL.a, acl)
assert_equal(CL.b, bcl)
assert_equal(CL.c, ccl)
assert_equal(CL.d, dcl)
G = State(a, b, c, 1)
CL = feedback(G, H)
acl = array([[2., 4., -3., 0., -3., 1.],
[3., -4., 10., 0., 9., -3.],
[1., 5., -11., 0., -9., 3.],
[0., -3., 6., 2., 3., 1.],
[0., 6., -12., -2., -5., 0.],
[0., 9., -18., 0., -8., 4.]])
bcl = array([[-3.], [9.], [-9.], [3.], [-6.], [-9.]])
ccl = array([[0., -3., 6., 0., 3., -1.]])
dcl = array([[3.]])
assert_equal(CL.a, acl)
assert_equal(CL.b, bcl)
assert_equal(CL.c, ccl)
assert_equal(CL.d, dcl)
def test_staircase():
M = array([[-6.5, 0.5, 6.5, -6.5, 0., 1., 0.],
[-0.5, -5.5, -5.5, 5.5, 2., 1., 2.],
[-0.5, 0.5, 0.5, -6.5, 3., 4., 3.],
[-0.5, 0.5, -5.5, -0.5, 3., 2., 3.],
[1., 1., 0., 0., 0., 0., 0.]])
A, B, C, D = matrix_slice(M, (1, 4), corner='sw')
a, b, c, T = staircase(A, B, C, form='o', invert=True)
assert_raises(ValueError, staircase, A, B, C, form='zzz')
assert_almost_equal(a[2:, :2], zeros((2, 2)))
assert_almost_equal(T.T @ A @ T, a)
a, b, c, T = staircase(A, zeros_like(B), C, form='o', invert=True)
assert_almost_equal(b, zeros_like(B))
def test_staircase():
M = array([[-6.5, 0.5, 6.5, -6.5, 0., 1., 0.],
[-0.5, -5.5, -5.5, 5.5, 2., 1., 2.],
[-0.5, 0.5, 0.5, -6.5, 3., 4., 3.],
[-0.5, 0.5, -5.5, -0.5, 3., 2., 3.],
[1., 1., 0., 0., 0., 0., 0.]])
A, B, C, D = matrix_slice(M, (1, 4), corner='sw')
a, b, c, T = staircase(A, B, C, form='o', invert=True)
assert_raises(ValueError, staircase, A, B, C, form='zzz')
assert_almost_equal(a[2:, :2], zeros((2, 2)))
assert_almost_equal(T.T @ A @ T, a)
a, b, c, T = staircase(A, zeros_like(B), C, form='o', invert=True)
assert_almost_equal(b, zeros_like(B))
def test_feedback_dynamic_static():
M = array([[2, 2, 1, -1],
[3, 2, -2, 3],
[1, -1, 1, -3],
[0, -1, 2, 0]])
a, b, c, d = matrix_slice(M, [3, 3])
G = State(a, b, eye(3))
H, _, _ = lqr(G, eye(3))
CL = feedback(G, H)
assert_almost_equal(CL.poles,
array([-5.42998305, -3.75504185, -1.55400925]))
G = State(a, b, c, d)
CL = feedback(G, 5+0j)
assert_almost_equal(concatenate_state_matrices(CL),
array([[2., -3., 11., -1.],
[3., 17., -32., 3.],
[1., -16., 31., -3.],
[0., -1., 2., 0.]]))
CL = feedback(5, G)
assert_almost_equal(concatenate_state_matrices(CL),
array([[2., -3., 11., -5.],
[3., 17., -32., 15.],
[1., -16., 31., -15.],
[0., 5., -10., 5.]]))