def __init__(self, settings):
# add specific private settings
settings.update(input_order=0)
settings.update(ouput_dim=1)
settings.update(input_type='system_state')
pm.Controller.__init__(self, settings)
eq_state = calc_equilibrium(settings)
# pole placement
parameter = [st.m0, st.m1, st.m2, st.l1, st.l2, st.g, self._settings["d0"], st.d1, st.d2]
self.A = symcalc.A_func(list(eq_state), parameter)
self.B = symcalc.B_func(list(eq_state), parameter)
self.C = symcalc.C
# create Q and R matrix
self.Q = np.diag(self._settings["Q"])
self.R = np.diag(self._settings["R"])
# try to solve linear riccati equation
self.P = solve_continuous_are(self.A, self.B, self.Q, self.R)
self.K = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P)
self.V = pm.calc_prefilter(self.A, self.B, self.C, self.K)
eig = np.linalg.eig(self.A - np.dot(self.B, self.K))
self._logger.info("equilibrium = " + str(eq_state))
self._logger.info("Q = " + str(self._settings["Q"]))
self._logger.info("R = " + str(self._settings["R"]))
self._logger.info("P = " + str(self.P))
self._logger.info("K = " + str(self.K))
self._logger.info("eig = " + str(eig))
self._logger.info("V = " + str(self.V[0][0]))
def test_prefilter(self):
"""
check function, which calculates the prefilter matrix
"""
self.assertRaises(ValueError, pm.calc_prefilter, np.array([[1], [1]]),
np.array([[1], [1]]), np.array([[1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]), np.array([[1], [1],
[1]]),
np.array([[1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]),
np.array([[1, 1, 1], [1, 1, 1]]), np.array([[1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]), np.array([[1], [1]]),
np.array([[1, 1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]), np.array([[1], [1]]),
np.array([[1, 1], [1, 1], [1, 1]]))
# enter matrices, where the prefilter's calculation should fail
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[0, 0], [0, 0]]), np.array([[0], [0]]),
np.array([[0, 0]]), np.array([[0, 0]]))
test_V = pm.calc_prefilter(self.A, self.B, self.C, self.K)
self.assertTrue(np.allclose(test_V, self.V))
def test_prefilter(self):
"""
check function, which calculates the prefilter matrix
"""
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1], [1]]),
np.array([[1], [1]]),
np.array([[1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]),
np.array([[1], [1], [1]]),
np.array([[1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]),
np.array([[1, 1, 1], [1, 1, 1]]),
np.array([[1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]),
np.array([[1], [1]]),
np.array([[1, 1, 1]]))
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[1, 1], [1, 1]]),
np.array([[1], [1]]),
np.array([[1, 1], [1, 1], [1, 1]]))
# enter matrices, where the prefilter's calculation should fail
self.assertRaises(ValueError, pm.calc_prefilter,
np.array([[0, 0], [0, 0]]), np.array([[0], [0]]),
np.array([[0, 0]]), np.array([[0, 0]]))
test_V = pm.calc_prefilter(self.A, self.B, self.C, self.K)
self.assertTrue(np.allclose(test_V, self.V))
def __init__(self, settings):
# add specific "private" settings
settings.update(input_order=0)
settings.update(output_dim=1)
settings.update(input_type=settings["source"])
pm.Controller.__init__(self, settings)
self._output = np.zeros((1, ))
# run pole placement
a_mat, b_mat, c_mat = linearise_system(self._settings["steady state"],
self._settings["steady tau"])
self.K = pm.place_siso(a_mat, b_mat, self._settings["poles"])
self.V = pm.calc_prefilter(a_mat, b_mat, c_mat, self.K)
def __init__(self, settings):
# add specific "private" settings
settings.update(input_order=0)
settings.update(output_dim=1)
settings.update(input_type=settings["source"])
pm.Controller.__init__(self, settings)
self._output = np.zeros((1, ))
# run pole placement
a_mat, b_mat, c_mat = linearise_system(self._settings["steady state"],
self._settings["steady tau"])
self.K = pm.place_siso(a_mat, b_mat, self._settings["poles"])
self.V = pm.calc_prefilter(a_mat, b_mat, c_mat, self.K)
def __init__(self, settings):
# add specific private settings
settings.update(input_order=0)
settings.update(ouput_dim=1)
settings.update(input_type='system_state')
pm.Controller.__init__(self, settings)
eq_state = calc_equilibrium(settings)
# pole placement
parameter = [st.m0, st.m1, st.m2, st.l1, st.l2, st.g, st.d0, st.d1, st.d2]
self.A = symcalc.A_func_par_lin(list(eq_state), parameter)
self.B = symcalc.B_func_par_lin(list(eq_state), parameter)
self.C = symcalc.C_par_lin
self.K = pm.place_siso(self.A, self.B, self._settings['poles'])
self.V = pm.calc_prefilter(self.A, self.B, self.C, self.K)
# eig = np.linalg.eig(self.A - np.dot(self.B, self.K))
self._logger.info("Equilibrium: {}".format(eq_state.tolist()))
self._logger.info("Poles: {}".format(self._settings["poles"].tolist()))
self._logger.info("K: {}".format(self.K.tolist()[0]))
self._logger.info("V: {}".format(self.V[0]))