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]))