def check_DLQE(L, P, poles, G, QN, RN):
P_expected = asmatarrayout(G.dot(QN).dot(G))
L_expected = asmatarrayout(0)
poles_expected = -np.squeeze(np.asarray(L_expected))
np.testing.assert_almost_equal(P, P_expected)
np.testing.assert_almost_equal(L, L_expected)
np.testing.assert_almost_equal(poles, poles_expected)
def check_DLQR(self, K, S, poles, Q, R):
S_expected = asmatarrayout(Q)
K_expected = asmatarrayout(0)
poles_expected = -np.squeeze(np.asarray(K_expected))
np.testing.assert_array_almost_equal(S, S_expected)
np.testing.assert_array_almost_equal(K, K_expected)
np.testing.assert_array_almost_equal(poles, poles_expected)
def check_LQE(L, P, poles, G, QN, RN):
P_expected = asmatarrayout(np.sqrt(G @ QN @ G @ RN))
L_expected = asmatarrayout(P_expected / RN)
poles_expected = -np.squeeze(np.asarray(L_expected))
np.testing.assert_almost_equal(P, P_expected)
np.testing.assert_almost_equal(L, L_expected)
np.testing.assert_almost_equal(poles, poles_expected)
def check_LQE(self, L, P, poles, G, QN, RN):
P_expected = asmatarrayout(np.sqrt(G.dot(QN.dot(G).dot(RN))))
L_expected = asmatarrayout(P_expected / RN)
poles_expected = -np.squeeze(np.asarray(L_expected))
np.testing.assert_array_almost_equal(P, P_expected)
np.testing.assert_array_almost_equal(L, L_expected)
np.testing.assert_array_almost_equal(poles, poles_expected)