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.]]))