diff --git a/mpc/__pycache__/mpc_func.cpython-36.pyc b/mpc/__pycache__/mpc_func.cpython-36.pyc index cb3f01c..73049d5 100644 Binary files a/mpc/__pycache__/mpc_func.cpython-36.pyc and b/mpc/__pycache__/mpc_func.cpython-36.pyc differ diff --git a/mpc/main_example.py b/mpc/main_example.py index 145d870..fcf66f7 100644 --- a/mpc/main_example.py +++ b/mpc/main_example.py @@ -1,6 +1,7 @@ import numpy as np import matplotlib.pyplot as plt import math +import copy from mpc_func import MpcController from control import matlab @@ -39,9 +40,6 @@ class FirstOrderSystem(): time history of system states """ - if init_states is not None: - self.states = init_states - self.A = A self.B = B self.C = C @@ -51,9 +49,12 @@ class FirstOrderSystem(): self.xs = np.zeros(self.A.shape[0]) + if init_states is not None: + self.xs = copy.deepcopy(init_states) + self.history_xs = [init_states] - def update_state(self, us, dt=0.01): + def update_state(self, u, dt=0.01): """calculating input Parameters ------------ @@ -62,34 +63,39 @@ class FirstOrderSystem(): dt : float in seconds, optional sampling time of simulation, default is 0.01 [s] """ - temp = self.xs.reshape(-1, 1) + temp_x = self.xs.reshape(-1, 1) + temp_u = u.reshape(-1, 1) # solve Runge-Kutta - k0 = dt * (np.dot(self.A, temp) + np.dot(self.B, us)) - k1 = dt * (np.dot(self.A, temp + k0/2.) + np.dot(self.B, us)) - k2 = dt * (np.dot(self.A, temp + k1/2.) + np.dot(self.B, us)) - k3 = dt * (np.dot(self.A, temp + k2) + np.dot(self.B, us)) + k0 = dt * (np.dot(self.A, temp_x) + np.dot(self.B, temp_u)) + k1 = dt * (np.dot(self.A, temp_x + k0/2.) + np.dot(self.B, temp_u)) + k2 = dt * (np.dot(self.A, temp_x + k1/2.) + np.dot(self.B, temp_u)) + k3 = dt * (np.dot(self.A, temp_x + k2) + np.dot(self.B, temp_u)) - self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() + # self.xs += ((k0 + 2 * k1 + 2 * k2 + k3) / 6.).flatten() # for oylar - # self.state += k0 + self.xs += k0.flatten() + # print("xs = {0}".format(self.xs)) + # a = input() # save - self.history_xs.append(self.xs) + save_states = copy.deepcopy(self.xs) + self.history_xs.append(save_states) + # print(self.history_xs) def main(): dt = 0.01 - simulation_time = 100 # in seconds + simulation_time = 300 # in seconds iteration_num = int(simulation_time / dt) # you must be care about this matrix # these A and B are for continuos system if you want to use discret system matrix please skip this step - tau = 0.53 - A = np.array([[1./tau, 0., 0., 0.], - [0., 1./tau, 0., 0.], + tau = 0.63 + A = np.array([[-1./tau, 0., 0., 0.], + [0., -1./tau, 0., 0.], [1., 0., 0., 0.], - [1., 0., 0., 0.]]) + [0., 1., 0., 0.]]) B = np.array([[1./tau, 0.], [0., 1./tau], [0., 0.], @@ -99,7 +105,8 @@ def main(): D = np.zeros((4, 2)) # make simulator with coninuous matrix - plant = FirstOrderSystem(A, B, C) + init_xs = np.array([0., 0., -3000., 50.]) + plant = FirstOrderSystem(A, B, C, init_states=init_xs) # create system sysc = matlab.ss(A, B, C, D) @@ -111,20 +118,31 @@ def main(): # evaluation function weight Q = np.diag([1., 1., 1., 1.]) - R = np.diag([1., 1.]) + R = np.diag([100., 100.]) pre_step = 3 # make controller with discreted matrix controller = MpcController(Ad, Bd, Q, R, pre_step) controller.initialize_controller() - xs = np.array([0., 0., 0., 0.]) - for i in range(iteration_num): - controller.calc_input(xs) + print("simulation time = {0}".format(i)) + reference = np.array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]) + controller.calc_input(plant.xs, reference) + + states = plant.xs + opt_u = controller.calc_input(states, reference) + plant.update_state(opt_u) - # states = plant.states - # controller.calc_input + history_states = np.array(plant.history_xs) + + print(history_states[:, 2]) + + plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0]) + plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1]) + plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2], linestyle="dashed") + plt.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3]) + plt.show() if __name__ == "__main__": main() diff --git a/mpc/mpc_func.py b/mpc/mpc_func.py index 443da34..3b4ddc8 100644 --- a/mpc/mpc_func.py +++ b/mpc/mpc_func.py @@ -12,7 +12,7 @@ class MpcController(): """ - def __init__(self, A, B, Q, R, pre_step, input_upper=None, input_lower=None): + def __init__(self, A, B, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None): """ """ self.A = np.array(A) @@ -27,8 +27,25 @@ class MpcController(): self.state_size = self.A.shape[0] self.input_size = self.B.shape[1] - self.history_us = [] + self.history_us = [np.zeros(self.input_size)] + # initial state + if initial_input is not None: + self.history_us = [initial_input] + + # constraints + if dt_input_lower in not None: + self.dt_input_lower = dt_input_lower + + if dt_input_upper in not None: + self.dt_input_upper = dt_input_upper + + if input_upper in not None: + self.input_upper = input_upper + + if input_lower in not None: + self.input_lower = input_lower + def initialize_controller(self): """ make matrix to calculate optimal controller @@ -66,6 +83,7 @@ class MpcController(): print("theta_mat = \n{0}".format(self.theta_mat)) + # evaluation function weight diag_Qs = np.array([np.diag(self.Q) for _ in range(self.pre_step)]) diag_Rs = np.array([np.diag(self.R) for _ in range(self.pre_step)]) @@ -75,6 +93,16 @@ class MpcController(): print("Qs = {0}".format(self.Qs)) print("Rs = {0}".format(self.Rs)) + # constraints + # dt U + F = np.array([[], [], []]) + + # u + + + # state + + def calc_input(self, states, references): """ Parameters @@ -89,10 +117,10 @@ class MpcController(): """ - temp_1 = np.dot(self.phi_mat, states) - temp_2 = np.dot(self.gamma_mat, self.history_us[-1]) + temp_1 = np.dot(self.phi_mat, states.reshape(-1, 1)) + temp_2 = np.dot(self.gamma_mat, self.history_us[-1].reshape(-1, 1)) - error = references - temp_1 - temp_2 + error = references.reshape(-1, 1) - temp_1 - temp_2 G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) ) @@ -101,35 +129,22 @@ class MpcController(): def optimized_func(dt_us): """ """ - return np.dot(dt_us.T, np.dot(H, dt_us)) - np.dot(G.T, dt_us) + return np.dot(dt_us.flatten(), np.dot(H, dt_us)) - np.dot(G.T, dt_us) def constraint_func(): """ """ - return + return None - init_dt_us = np.zeros(self.pre_step) + init_dt_us = np.zeros((self.input_size * self.pre_step, 1)) opt_result = minimize(optimized_func, init_dt_us) - opt_dt_us = opt_result + opt_dt_us = opt_result.x - opt_us = opt_dt_us[0] + self.history_us[-1] + opt_u = opt_dt_us[:self.input_size] + self.history_us[-1] # save - self.history_us.append(opt_us) - return opt_us - - - - - - - - - - - - - + self.history_us.append(opt_u) + return opt_u \ No newline at end of file