def test_State_algebra_add_radd():
sta_siso = State(5)
sta_mimo = State(2.0 * np.eye(3))
dyn_siso = State(haroldcompanion([1, 3, 3, 1]), e_i(3, -1), e_i(3, 1).T)
dyn_mimo = State(haroldcompanion([1, 3, 3, 1]), e_i(3, [1, 2]), np.eye(3))
dyn_mimo_sq = State(haroldcompanion([1, 3, 3, 1]), np.eye(3), np.eye(3))
G = dyn_mimo + sta_siso
assert_array_almost_equal(G.d,
sta_siso.to_array() * np.ones(dyn_mimo.shape))
assert_raises(ValueError, dyn_mimo.__add__, sta_mimo)
G = dyn_mimo_sq + sta_mimo
assert_array_almost_equal(G.d, 2. * np.eye(3))
G = dyn_mimo + dyn_siso
J = np.array([[0., 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 1., 0.],
[-1., -3., -3., 0., 0., 0., 0., 1.],
[0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 1., 0., 0.],
[0., 0., 0., -1., -3., -3., 1., 1.],
[1., 0., 0., 0., 1., 0., 0., 0.],
[0., 1., 0., 0., 1., 0., 0., 0.],
[0., 0., 1., 0., 1., 0., 0., 0.]])
assert_array_almost_equal(concatenate_state_matrices(G), J)
assert_raises(ValueError, dyn_mimo.__add__, dyn_mimo_sq)
assert_raises(ValueError, dyn_mimo.__sub__, dyn_mimo_sq)
assert_raises(ValueError, dyn_mimo.__radd__, dyn_mimo_sq)
assert_raises(ValueError, dyn_mimo.__rsub__, dyn_mimo_sq)
def test_State_algebra_mimo_siso():
static_siso_state = State(5)
static_mimo_state = State(2.0*np.eye(3))
dynamic_siso_state = State(haroldcompanion([1, 3, 3, 1]),
e_i(3, -1),
e_i(3, 1).T,
0)
dynamic_mimo_state = State(haroldcompanion([1, 3, 3, 1]),
e_i(3, [1, 2]),
np.eye(3),
np.zeros((3, 2)))
dynamic_square_state = State(haroldcompanion([1, 3, 3, 1]),
np.eye(3),
np.eye(3),
np.zeros((3, 3))
)
assert_raises(IndexError, dynamic_siso_state.__mul__, static_mimo_state)
assert_raises(IndexError, dynamic_siso_state.__add__, static_mimo_state)
assert_raises(IndexError, static_mimo_state.__add__, dynamic_mimo_state)
assert_raises(IndexError, static_mimo_state.__add__, static_siso_state)
assert_raises(IndexError, static_mimo_state.__mul__, static_siso_state)
F = static_mimo_state @ dynamic_mimo_state
assert_almost_equal(F.c, np.eye(3)*2.0)
assert_almost_equal((dynamic_square_state + static_mimo_state).d,
2*np.eye(3))
def test_State_algebra_add_radd():
sta_siso = State(5)
sta_mimo = State(2.0*np.eye(3))
dyn_siso = State(haroldcompanion([1, 3, 3, 1]), e_i(3, -1), e_i(3, 1).T)
dyn_mimo = State(haroldcompanion([1, 3, 3, 1]), e_i(3, [1, 2]), np.eye(3))
dyn_mimo_sq = State(haroldcompanion([1, 3, 3, 1]), np.eye(3), np.eye(3))
G = dyn_mimo + sta_siso
assert_array_almost_equal(G.d, sta_siso.to_array()*np.ones(dyn_mimo.shape))
assert_raises(ValueError, dyn_mimo.__add__, sta_mimo)
G = dyn_mimo_sq + sta_mimo
assert_array_almost_equal(G.d, 2.*np.eye(3))
G = dyn_mimo + dyn_siso
J = np.array([[0., 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 1., 0.],
[-1., -3., -3., 0., 0., 0., 0., 1.],
[0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 1., 0., 0.],
[0., 0., 0., -1., -3., -3., 1., 1.],
[1., 0., 0., 0., 1., 0., 0., 0.],
[0., 1., 0., 0., 1., 0., 0., 0.],
[0., 0., 1., 0., 1., 0., 0., 0.]])
assert_array_almost_equal(concatenate_state_matrices(G), J)
assert_raises(ValueError, dyn_mimo.__add__, dyn_mimo_sq)
assert_raises(ValueError, dyn_mimo.__sub__, dyn_mimo_sq)
assert_raises(ValueError, dyn_mimo.__radd__, dyn_mimo_sq)
assert_raises(ValueError, dyn_mimo.__rsub__, dyn_mimo_sq)
def test_State_algebra_mul_rmul_mimo_siso():
sta_siso = State(5)
sta_mimo = State(2.0 * np.eye(3))
dyn_siso = State(haroldcompanion([1, 3, 3, 1]), e_i(3, -1), e_i(3, 1).T)
dyn_mimo = State(haroldcompanion([1, 3, 3, 1]), e_i(3, [1, 2]), np.eye(3))
dyn_mimo_sq = State(haroldcompanion([1, 3, 3, 1]), np.eye(3), np.eye(3))
G = dyn_siso * dyn_mimo
J = np.array([[0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 1., 0., 0., 0., 0., 0., 0.],
[-1., -3., -3., 0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.],
[0., 0., 0., -1., -3., -3., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 0., 0., -1., -3., -3., 0., 1.],
[1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0.]])
assert_array_almost_equal(concatenate_state_matrices(G), J)
G = dyn_mimo * dyn_siso
assert_array_almost_equal(concatenate_state_matrices(G), J)
G = dyn_mimo * sta_siso
assert_array_almost_equal(G.b, 5 * dyn_mimo.b)
assert_array_almost_equal(G.d, 5 * dyn_mimo.d)
assert_raises(ValueError, sta_mimo.__add__, dyn_mimo)
F = sta_mimo @ dyn_mimo
J = np.array([[0., 1., 0., 0., 0.], [0., 0., 1., 1., 0.],
[-1., -3., -3., 0., 1.], [2., 0., 0., 0., 0.],
[0., 2., 0., 0., 0.], [0., 0., 2., 0., 0.]])
assert_array_almost_equal(concatenate_state_matrices(F), J)
assert_almost_equal((dyn_mimo_sq + sta_mimo).d, 2 * np.eye(3))
def test_ackermann_controllable():
#
A = haroldcompanion([1, 6, 5, 1])
B = eye(3)[:, [-1]]
p = [-10, -9, -8]
K = ackermann((A, B), p)
pa = eigvals(A - B @ K)
assert_array_almost_equal(array(p, dtype=complex), sort(pa))
def test_ackermann_controllable():
#
A = haroldcompanion([1, 6, 5, 1])
B = eye(3)[:, [-1]]
p = [-10, -9, -8]
K = ackermann((A, B), p)
pa = eigvals(A - B@K)
assert_array_almost_equal(array(p, dtype=complex), sort(pa))
def test_State_algebra_mul_rmul_mimo_siso():
sta_siso = State(5)
sta_mimo = State(2.0*np.eye(3))
dyn_siso = State(haroldcompanion([1, 3, 3, 1]), e_i(3, -1), e_i(3, 1).T)
dyn_mimo = State(haroldcompanion([1, 3, 3, 1]), e_i(3, [1, 2]), np.eye(3))
dyn_mimo_sq = State(haroldcompanion([1, 3, 3, 1]), np.eye(3), np.eye(3))
G = dyn_siso * dyn_mimo
J = np.array([[0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 1., 0., 0., 0., 0., 0., 0.],
[-1., -3., -3., 0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.],
[0., 0., 0., -1., -3., -3., 0., 0., 0., 1., 0.],
[0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0.],
[0., 0., 0., 0., 0., 0., -1., -3., -3., 0., 1.],
[1., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 1., 0., 0., 0., 0., 0., 0., 0., 0., 0.],
[0., 0., 1., 0., 0., 0., 0., 0., 0., 0., 0.]])
assert_array_almost_equal(concatenate_state_matrices(G), J)
G = dyn_mimo * dyn_siso
assert_array_almost_equal(concatenate_state_matrices(G), J)
G = dyn_mimo * sta_siso
assert_array_almost_equal(G.b, 5*dyn_mimo.b)
assert_array_almost_equal(G.d, 5*dyn_mimo.d)
assert_raises(ValueError, sta_mimo.__add__, dyn_mimo)
F = sta_mimo @ dyn_mimo
J = np.array([[0., 1., 0., 0., 0.],
[0., 0., 1., 1., 0.],
[-1., -3., -3., 0., 1.],
[2., 0., 0., 0., 0.],
[0., 2., 0., 0., 0.],
[0., 0., 2., 0., 0.]])
assert_array_almost_equal(concatenate_state_matrices(F), J)
assert_almost_equal((dyn_mimo_sq + sta_mimo).d, 2*np.eye(3))
def test_ackermann_uncontrollable():
A = block_diag(haroldcompanion([1, 6, 5, 1]), 1)
B = eye(4)[:, [-2]]
p = [-10, -9, -8, -7]
assert_raises(ValueError, ackermann, (A, B), p)
def test_ackermann_uncontrollable():
A = block_diag(haroldcompanion([1, 6, 5, 1]), 1)
B = eye(4)[:, [-2]]
p = [-10, -9, -8, -7]
assert_raises(ValueError, ackermann, (A, B), p)