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)