From ef49e5f1b289ce2802ddf29db4fea1d6a248307a Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Tue, 25 Dec 2018 02:13:34 +0900 Subject: [PATCH] add mpc --- mpc/simulator_func.py | 194 ------------------------------------------ 1 file changed, 194 deletions(-) diff --git a/mpc/simulator_func.py b/mpc/simulator_func.py index b4460ff..bf38e16 100644 --- a/mpc/simulator_func.py +++ b/mpc/simulator_func.py @@ -68,197 +68,3 @@ class FirstOrderSystem(): return y_dot -class LyapunovMRAC(): - """LyapunovMRAC - - Attributes - ----------- - input : float - system state, this system should have one input - one output - a : float - parameter of reference model - alpha_1 : float - parameter of the controller - alpha_2 : float - parameter of the controller - theta_1 : float - state of the controller - theta_2 : float - state of the controller - history_input : list - time history of input - """ - def __init__(self, g_1, g_2, init_theta_1=0.0, init_theta_2=0.0, init_input=0.0): - """ - Parameters - ----------- - g_1 : float - parameter of the controller - g_2 : float - parameter of the controller - theta_1 : float, optional - state of the controller default is 0.0 - theta_2 : float, optional - state of the controller default is 0.0 - init_input : float, optional - initial input of controller default is 0.0 - """ - self.input = init_input - - # parameters - self.g_1 = g_1 - self.g_2 = g_2 - - # states - self.theta_1 = init_theta_1 - self.theta_2 = init_theta_2 - - self.history_input = [init_input] - - def update_input(self, e, r, y, dt=0.01): - """ - Parameters - ------------ - e : float - error value of system - r : float - reference value - y : float - output the model value - dt : float in seconds, optional - sampling time of simulation, default is 0.01 [s] - """ - # for theta 1, theta 1 dot, theta 2, theta 2 dot - k0 = [0.0 for _ in range(4)] - k1 = [0.0 for _ in range(4)] - k2 = [0.0 for _ in range(4)] - k3 = [0.0 for _ in range(4)] - - functions = [self._func_theta_1, self._func_theta_2] - - # solve Runge-Kutta - for i, func in enumerate(functions): - k0[i] = dt * func(self.theta_1, self.theta_2, e, r, y) - - for i, func in enumerate(functions): - k1[i] = dt * func(self.theta_1 + k0[0]/2.0, self.theta_2 + k0[1]/2.0, e, r, y) - - for i, func in enumerate(functions): - k2[i] = dt * func(self.theta_1 + k1[0]/2.0, self.theta_2 + k1[1]/2.0, e, r, y) - - for i, func in enumerate(functions): - k3[i] = dt * func(self.theta_1 + k2[0], self.theta_2 + k2[1], e, r, y) - - self.theta_1 += (k0[0] + 2 * k1[0] + 2 * k2[0] + k3[0]) / 6.0 - self.theta_2 += (k0[1] + 2 * k1[1] + 2 * k2[1] + k3[1]) / 6.0 - - # for oylar - """ - self.theta_1 += k0[0] - self.u_1 += k0[1] - self.theta_2 += k0[2] - self.u_2 += k0[3] - """ - # calc input - self.input = self.theta_1 * r + self.theta_2 * y - - # save - self.history_input.append(self.input) - - def _func_theta_1(self, theta_1, theta_2, e, r, y): - """ - Parameters - ------------ - theta_1 : float - state of the controller - theta_2 : float - state of the controller - e : float - error - r : float - reference - y : float - output of system - """ - y_dot = self.g_1 * r * e - - return y_dot - - def _func_theta_2(self, theta_1, theta_2, e, r, y): - """ - Parameters - ------------ - Parameters - ------------ - theta_1 : float - state of the controller - theta_2 : float - state of the controller - e : float - error - r : float - reference - y : float - output of system - """ - y_dot = self.g_2 * y * e - - return y_dot - - -def main(): - # control plant - a = -0.5 - b = 0.5 - plant = FirstOrderSystem(a, b) - - # reference model - a = 1. - b = 1. - reference_model = FirstOrderSystem(a, b) - - # controller - g_1 = 5. - g_2 = 5. - controller = LyapunovMRAC(g_1, g_2) - - simulation_time = 50 # in second - dt = 0.01 - simulation_iterations = int(simulation_time / dt) # dt is 0.01 - - history_error = [0.0] - history_r = [0.0] - - for i in range(1, simulation_iterations): # skip the first - # reference input - r = math.sin(dt * i) - # update reference - reference_model.update_state(r, dt=dt) - # update plant - plant.update_state(controller.input, dt=dt) - - # calc error - e = reference_model.state - plant.state - y = plant.state - history_error.append(e) - history_r.append(r) - - # make the gradient - controller.update_input(e, r, y, dt=dt) - - # fig - plt.plot(np.arange(simulation_iterations)*dt, plant.history_state, label="plant y", linestyle="dashed") - plt.plot(np.arange(simulation_iterations)*dt, reference_model.history_state, label="model reference") - plt.plot(np.arange(simulation_iterations)*dt, history_error, label="error", linestyle="dashdot") - # plt.plot(range(simulation_iterations), history_r, label="error") - plt.xlabel("time [s]") - plt.ylabel("y") - plt.legend() - plt.show() - - # input - # plt.plot(np.arange(simulation_iterations)*dt, controller.history_input) - # plt.show() - -if __name__ == "__main__": - main() \ No newline at end of file