def test_place_siso(self):
"""
test for pole placement
"""
# ValueError must occur if it is a MIMO system
self.assertRaises(ValueError, pm.place_siso, np.array([[1, 1], [1,
1]]),
np.array([[1, 1], [1, 1]]), self.poles)
self.assertRaises(ValueError, pm.place_siso, np.array([[1, 1], [1,
1]]),
np.array([[1], [1]]), np.array([1, 1, 1]))
# calculate a state feedback and an observer gain
test_K = pm.place_siso(self.A, self.B, self.poles)
test_L = pm.place_siso(self.A.transpose(), self.C.transpose(),
self.poles).transpose()
self.assertTrue(np.allclose(test_K, self.K))
self.assertTrue(np.allclose(test_L, self.L))
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
self.a_mat, self.b_mat, self.c_mat = linearise_system(
self._settings["steady state"],
self._settings["steady tau"])
self.L = pm.place_siso(self.a_mat.T,
self.c_mat.T,
self._settings["poles"]).T
self.output = np.array(self._settings["initial state"], dtype=float)
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
self.a_mat, self.b_mat, self.c_mat = linearise_system(
self._settings["steady state"],
self._settings["steady tau"])
self.L = pm.place_siso(self.a_mat.T,
self.c_mat.T,
self._settings["poles"]).T
self.output = np.array(self._settings["initial state"], dtype=float)
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 test_place_siso(self):
"""
test for pole placement
"""
# ValueError must occur if it is a MIMO system
self.assertRaises(ValueError, pm.place_siso,
np.array([[1, 1], [1, 1]]),
np.array([[1, 1], [1, 1]]),
self.poles)
self.assertRaises(ValueError, pm.place_siso,
np.array([[1, 1], [1, 1]]),
np.array([[1], [1]]),
np.array([1, 1, 1]))
# calculate a state feedback and an observer gain
test_K = pm.place_siso(self.A, self.B, self.poles)
test_L = pm.place_siso(self.A.transpose(),
self.C.transpose(),
self.poles).transpose()
self.assertTrue(np.allclose(test_K, self.K))
self.assertTrue(np.allclose(test_L, self.L))
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
self.a_mat, self.b_mat, self.c_mat = linearise_system(
self._settings["steady state"],
self._settings["steady tau"])
self.L = pm.place_siso(self.a_mat.T,
self.c_mat.T,
self._settings["poles"]).T
self.step_width = self._settings["tick divider"] * self._settings["step width"]
self.output = np.array(self._settings["initial state"], dtype=float)
self.solver = ode(self.linear_state_function)
self.solver.set_integrator("dopri5",
method=self._settings["Method"],
rtol=self._settings["rTol"],
atol=self._settings["aTol"])
self.solver.set_initial_value(self._settings["initial state"])
self.nextObserver_output = self.output
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
self.a_mat, self.b_mat, self.c_mat = linearise_system(
self._settings["steady state"],
self._settings["steady tau"])
self.L = pm.place_siso(self.a_mat.T,
self.c_mat.T,
self._settings["poles"]).T
self.step_width = self._settings["tick divider"] * self._settings["step width"]
self.output = np.array(self._settings["initial state"], dtype=float)
self.solver = ode(self.linear_state_function)
self.solver.set_integrator("dopri5",
method=self._settings["Method"],
rtol=self._settings["rTol"],
atol=self._settings["aTol"])
self.solver.set_initial_value(self._settings["initial state"])
self.nextObserver_output = self.output
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
self.a_mat, self.b_mat, self.c_mat = linearise_system(
self._settings["steady state"],
self._settings["steady tau"])
self.n = self.a_mat.shape[0]
self.r = self.c_mat.shape[0]
if self.c_mat.shape != (1, 4):
raise Exception("LuenbergerObserverReduced only usable for SISO")
# which cols and rows must be sorted
self.switch = 0
for i in range(self.c_mat.shape[1]):
if self.c_mat[0, i] == 1:
self.switch = i
break
# sort A, B, C for measured and unmeasured states
self.a_mat = swap_cols(self.a_mat, 0, self.switch)
self.a_mat = swap_rows(self.a_mat, 0, self.switch)
self.C = swap_cols(self.c_mat, 0, self.switch)
self.B = swap_rows(self.b_mat, 0, self.switch)
# get reduced system
self.a_mat_11 = self.a_mat[0:self.r, 0:self.r]
self.a_mat_12 = self.a_mat[0:self.r, self.r:self.n]
self.a_mat_21 = self.a_mat[self.r:self.n, 0:self.r]
self.a_mat_22 = self.a_mat[self.r:self.n, self.r:self.n]
self.b_1 = self.b_mat[0:self.r, :]
self.b_2 = self.b_mat[self.r:self.n, :]
self.L = pm.place_siso(self.a_mat_22.T,
self.a_mat_12.T,
self._settings["poles"]).T
self.output = np.array(self._settings["initial state"], dtype=float)
def __init__(self, settings):
settings.update(output_dim=4)
super().__init__(settings)
self.a_mat, self.b_mat, self.c_mat = linearise_system(
self._settings["steady state"],
self._settings["steady tau"])
self.n = self.a_mat.shape[0]
self.r = self.c_mat.shape[0]
if self.c_mat.shape != (1, 4):
raise Exception("LuenbergerObserverReduced only usable for SISO")
# which cols and rows must be sorted
self.switch = 0
for i in range(self.c_mat.shape[1]):
if self.c_mat[0, i] == 1:
self.switch = i
break
# sort A, B, C for measured and unmeasured states
self.a_mat = swap_cols(self.a_mat, 0, self.switch)
self.a_mat = swap_rows(self.a_mat, 0, self.switch)
self.C = swap_cols(self.c_mat, 0, self.switch)
self.B = swap_rows(self.b_mat, 0, self.switch)
# get reduced system
self.a_mat_11 = self.a_mat[0:self.r, 0:self.r]
self.a_mat_12 = self.a_mat[0:self.r, self.r:self.n]
self.a_mat_21 = self.a_mat[self.r:self.n, 0:self.r]
self.a_mat_22 = self.a_mat[self.r:self.n, self.r:self.n]
self.b_1 = self.b_mat[0:self.r, :]
self.b_2 = self.b_mat[self.r:self.n, :]
self.L = pm.place_siso(self.a_mat_22.T,
self.a_mat_12.T,
self._settings["poles"]).T
self.output = np.array(self._settings["initial state"], dtype=float)
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]))