From 0094d340b32cb90212581c18dfdd153c1ce9e68f Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Wed, 19 Dec 2018 12:14:30 +0900 Subject: [PATCH] complete cgmres/main_example.py --- .../{main_cgmres.py => main_example.py} | 325 +++++++-- nmpc/cgmres/main_two_wheeled.py | 645 ++++++++++++++++++ 2 files changed, 910 insertions(+), 60 deletions(-) rename nmpc/cgmres/{main_cgmres.py => main_example.py} (57%) create mode 100644 nmpc/cgmres/main_two_wheeled.py diff --git a/nmpc/cgmres/main_cgmres.py b/nmpc/cgmres/main_example.py similarity index 57% rename from nmpc/cgmres/main_cgmres.py rename to nmpc/cgmres/main_example.py index d98028f..c5a13ab 100644 --- a/nmpc/cgmres/main_cgmres.py +++ b/nmpc/cgmres/main_example.py @@ -3,17 +3,26 @@ import matplotlib.pyplot as plt import math class SampleSystem(): - """SampleSystem - + """SampleSystem, this is the simulator Attributes ----------- - + x_1 : float + system state 1 + x_2 : float + system state 2 + history_x_1 : list + time history of system state 1 (x_1) + history_x_2 : list + time history of system state 2 (x_2) """ def __init__(self, init_x_1=0., init_x_2=0.): """ Palameters ----------- - + init_x_1 : float, optional + initial value of x_1, default is 0. + init_x_2 : float, optional + initial value of x_2, default is 0. """ self.x_1 = init_x_1 self.x_2 = init_x_2 @@ -61,34 +70,40 @@ class SampleSystem(): """ Parameters ------------ - + y_1 : float + y_2 : float + u : float + system input """ y_dot = y_2 - return y_dot def _func_x_2(self, y_1, y_2, u): """ Parameters ------------ - + y_1 : float + y_2 : float + u : float + system input """ y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u - return y_dot class NMPCSimulatorSystem(): - """SimulatorSystem for nmpc - + """SimulatorSystem for nmpc, this is the simulator of nmpc + the reason why I seperate the real simulator and nmpc's simulator is sometimes the modeling error, disturbance can include in real simulator Attributes ----------- - + None + """ def __init__(self): """ Parameters ----------- + None """ pass @@ -96,29 +111,55 @@ class NMPCSimulatorSystem(): """main Parameters ------------ - + x_1 : float + current state + x_2 : float + current state + us : list of float + estimated optimal input Us for N steps + N : int + predict step + dt : float + sampling time Returns -------- - x_1s : - x_2s : - lam_1s : - lam_2s : + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + lam_1s : list of float + adjoint state of x_1s, lam_1s for N steps + lam_2s : list of float + adjoint state of x_2s, lam_2s for N steps """ - x_1s, x_2s = self._calc_predict_states(x_1, x_2, us, N, dt) - lam_1s, lam_2s = self._calc_adjoint_states(x_1s, x_2s, us, N, dt) + x_1s, x_2s = self._calc_predict_states(x_1, x_2, us, N, dt) # by usin state equation + lam_1s, lam_2s = self._calc_adjoint_states(x_1s, x_2s, us, N, dt) # by using adjoint equation return x_1s, x_2s, lam_1s, lam_2s def _calc_predict_states(self, x_1, x_2, us, N, dt): - """ + """ Parameters ------------ - predict_t : float - predict time + x_1 : float + current state + x_2 : float + current state + us : list of float + estimated optimal input Us for N steps + N : int + predict step dt : float sampling time + + Returns + -------- + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps """ # initial state x_1s = [x_1] @@ -135,17 +176,30 @@ class NMPCSimulatorSystem(): """ Parameters ------------ - predict_t : float - predict time + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + us : list of float + estimated optimal input Us for N steps + N : int + predict step dt : float sampling time + + Returns + -------- + lam_1s : list of float + adjoint state of x_1s, lam_1s for N steps + lam_2s : list of float + adjoint state of x_2s, lam_2s for N steps """ # final state # final_state_func lam_1s = [x_1s[-1]] lam_2s = [x_2s[-1]] - for i in range(N-1, 0, -1): + for i in range(N-1, 0, -1): temp_lam_1, temp_lam_2 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], lam_1s[0] ,lam_2s[0], us[i], dt) lam_1s.insert(0, temp_lam_1) lam_2s.insert(0, temp_lam_2) @@ -158,20 +212,28 @@ class NMPCSimulatorSystem(): pass def _predict_state_with_oylar(self, x_1, x_2, u, dt): - """in this case this function is the same as simulatoe + """in this case this function is the same as simulator Parameters ------------ + x_1 : float + system state + x_2 : float + system state u : float - input of system in some cases this means the reference + system input dt : float in seconds - sampling time of simulation, default is 0.01 [s] + sampling time + Returns + -------- + next_x_1 : float + next state, x_1 calculated by using state equation + next_x_2 : float + next state, x_2 calculated by using state equation """ - # for theta 1, theta 1 dot, theta 2, theta 2 dot k0 = [0. for _ in range(2)] functions = [self.func_x_1, self.func_x_2] - # solve Runge-Kutta for i, func in enumerate(functions): k0[i] = dt * func(x_1, x_2, u) @@ -181,66 +243,161 @@ class NMPCSimulatorSystem(): return next_x_1, next_x_2 def func_x_1(self, y_1, y_2, u): - """ + """calculating \dot{x_1} Parameters ------------ - + y_1 : float + means x_1 + y_2 : float + means x_2 + u : float + means system input + Returns + --------- + y_dot : float + means \dot{x_1} """ y_dot = y_2 - return y_dot def func_x_2(self, y_1, y_2, u): - """ + """calculating \dot{x_2} Parameters ------------ - + y_1 : float + means x_1 + y_2 : float + means x_2 + u : float + means system input + Returns + --------- + y_dot : float + means \dot{x_2} """ y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u - return y_dot def _adjoint_state_with_oylar(self, x_1, x_2, lam_1, lam_2, u, dt): """ + Parameters + ------------ + x_1 : float + system state + x_2 : float + system state + lam_1 : float + adjoint state + lam_2 : float + adjoint state + u : float + system input + dt : float in seconds + sampling time + Returns + -------- + pre_lam_1 : float + pre, 1 step before lam_1 calculated by using adjoint equation + pre_lam_2 : float + pre, 1 step before lam_2 calculated by using adjoint equation """ - # for theta 1, theta 1 dot, theta 2, theta 2 dot k0 = [0. for _ in range(2)] functions = [self._func_lam_1, self._func_lam_2] - # solve Runge-Kutta for i, func in enumerate(functions): k0[i] = dt * func(x_1, x_2, lam_1, lam_2, u) - next_lam_1 = lam_1 + k0[0] - next_lam_2 = lam_2 + k0[1] + pre_lam_1 = lam_1 + k0[0] + pre_lam_2 = lam_2 + k0[1] - return next_lam_1, next_lam_2 + return pre_lam_1, pre_lam_2 def _func_lam_1(self, y_1, y_2, y_3, y_4, u): - """ + """calculating -\dot{lam_1} + Parameters + ------------ + y_1 : float + means x_1 + y_2 : float + means x_2 + y_3 : float + means lam_1 + y_4 : float + means lam_2 + u : float + means system input + Returns + --------- + y_dot : float + means -\dot{lam_1} """ y_dot = y_1 - (2. * y_1 * y_2 + 1.) * y_4 - return y_dot def _func_lam_2(self, y_1, y_2, y_3, y_4, u): - """ + """calculating -\dot{lam_2} + Parameters + ------------ + y_1 : float + means x_1 + y_2 : float + means x_2 + y_3 : float + means lam_1 + y_4 : float + means lam_2 + u : float + means system input + Returns + --------- + y_dot : float + means -\dot{lam_2} """ y_dot = y_2 + y_3 + (-3. * (y_2**2) - y_1**2 + 1. ) * y_4 - return y_dot class NMPCController_with_CGMRES(): """ Attributes ------------ - + zeta : float + gain of optimal answer stability + ht : float + update value of NMPC this should be decided by zeta + tf : float + predict time + alpha : float + gain of predict time + N : int + predicte step, discritize value + threshold : float + cgmres's threshold value + input_num : int + system input length, this should include dummy u and constraint variables + max_iteration : int + decide by the solved matrix size + simulator : NMPCSimulatorSystem class + us : list of float + estimated optimal system input + dummy_us : list of float + estimated dummy input + raws : list of float + estimated constraint variable + history_u : list of float + time history of actual system input + history_dummy_u : list of float + time history of actual dummy u + history_raw : list of float + time history of actual raw + history_f : list of float + time history of error of optimal """ def __init__(self): """ Parameters ----------- + None """ # parameters self.zeta = 100. # 安定化ゲイン @@ -269,7 +426,20 @@ class NMPCController_with_CGMRES(): def calc_input(self, x_1, x_2, time): """ + Parameters + ------------ + x_1 : float + current state + x_2 : float + current state + time : float in seconds + now time + Returns + -------- + us : list of float + estimated optimal system input """ + # calculating sampling time dt = self.tf * (1. - np.exp(-self.alpha * time)) / float(self.N) # x_dot @@ -282,25 +452,24 @@ class NMPCController_with_CGMRES(): x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us, self.N, dt) # Fxt - Fxt = self.calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, + Fxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, self.raws, self.N, dt) # F x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) - F = self.calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, + F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, self.raws, self.N, dt) right = -self.zeta * F - ((Fxt - F) / self.ht) - # dus du = self.us * self.ht ddummy_u = self.dummy_us * self.ht draw = self.raws * self.ht x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us + du, self.N, dt) - Fuxt = self.calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, + Fuxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, self.raws + draw, self.N, dt) left = ((Fuxt - Fxt) / self.ht) @@ -311,7 +480,7 @@ class NMPCController_with_CGMRES(): vs = np.zeros((self.max_iteration, self.max_iteration + 1)) # 数×iterarion回数 - vs[:, 0] = r0 / r0_norm + vs[:, 0] = r0 / r0_norm # 最初の基底を算出 hs = np.zeros((self.max_iteration + 1, self.max_iteration + 1)) @@ -325,14 +494,14 @@ class NMPCController_with_CGMRES(): x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us + du, self.N, dt) - Fuxt = self.calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, + Fuxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, self.raws + draw, self.N, dt) Av = (( Fuxt - Fxt) / self.ht) sum_Av = np.zeros(self.max_iteration) - for j in range(i + 1): + for j in range(i + 1): # グラムシュミットの直交化法です、和を取って差分を取って算出します hs[j, i] = np.dot(Av, vs[:, j]) sum_Av = sum_Av + hs[j, i] * vs[:, j] @@ -342,13 +511,9 @@ class NMPCController_with_CGMRES(): vs[:, i+1] = v_est / hs[i+1, i] - # print("v_est = {0}".format(v_est)) - - inv_hs = np.linalg.pinv(hs[:i+1, :i]) + inv_hs = np.linalg.pinv(hs[:i+1, :i]) # この辺は教科書（実時間の方）にのっています ys = np.dot(inv_hs, r0_norm * e[:i+1]) - # print("ys = {0}".format(ys)) - judge_value = r0_norm * e[:i+1] - np.dot(hs[:i+1, :i], ys[:i]) if np.linalg.norm(judge_value) < self.threshold or i == self.max_iteration-1: @@ -367,10 +532,11 @@ class NMPCController_with_CGMRES(): x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) - F = self.calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, + F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, self.raws, self.N, dt) print("check F = {0}".format(np.linalg.norm(F))) + # for save self.history_f.append(np.linalg.norm(F)) self.history_u.append(self.us[0]) @@ -379,8 +545,28 @@ class NMPCController_with_CGMRES(): return self.us - def calc_f(self, x_1s, x_2s, lam_1s, lam_2s, us, dummy_us, raws, N, dt): - """ここはケースによって変えるめっちゃ使う + def _calc_f(self, x_1s, x_2s, lam_1s, lam_2s, us, dummy_us, raws, N, dt): + """ + Parameters + ------------ + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + lam_1s : list of float + adjoint state of x_1s, lam_1s for N steps + lam_2s : list of float + adjoint state of x_2s, lam_2s for N steps + us : list of float + estimated optimal system input + dummy_us : list of float + estimated dummy input + raws : list of float + estimated constraint variable + N : int + predict time step + dt : float + sampling time of system """ F = [] @@ -424,11 +610,30 @@ def main(): f_fig = fig.add_subplot(326) x_1_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_1) + x_1_fig.set_xlabel("time [s]") + x_1_fig.set_ylabel("x_1") + x_2_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_2) + x_2_fig.set_xlabel("time [s]") + x_2_fig.set_ylabel("x_2") + u_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u) + u_fig.set_xlabel("time [s]") + u_fig.set_ylabel("u") + dummy_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u) + dummy_fig.set_xlabel("time [s]") + dummy_fig.set_ylabel("dummy u") + raw_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw) + raw_fig.set_xlabel("time [s]") + raw_fig.set_ylabel("raw") + f_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_f) + f_fig.set_xlabel("time [s]") + f_fig.set_ylabel("optimal error") + + fig.tight_layout() plt.show() diff --git a/nmpc/cgmres/main_two_wheeled.py b/nmpc/cgmres/main_two_wheeled.py new file mode 100644 index 0000000..c5a13ab --- /dev/null +++ b/nmpc/cgmres/main_two_wheeled.py @@ -0,0 +1,645 @@ +import numpy as np +import matplotlib.pyplot as plt +import math + +class SampleSystem(): + """SampleSystem, this is the simulator + Attributes + ----------- + x_1 : float + system state 1 + x_2 : float + system state 2 + history_x_1 : list + time history of system state 1 (x_1) + history_x_2 : list + time history of system state 2 (x_2) + """ + def __init__(self, init_x_1=0., init_x_2=0.): + """ + Palameters + ----------- + init_x_1 : float, optional + initial value of x_1, default is 0. + init_x_2 : float, optional + initial value of x_2, default is 0. + """ + self.x_1 = init_x_1 + self.x_2 = init_x_2 + self.history_x_1 = [init_x_1] + self.history_x_2 = [init_x_2] + + def update_state(self, u, dt=0.01): + """ + Palameters + ------------ + u : float + input of system in some cases this means the reference + 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(2)] + k1 = [0.0 for _ in range(2)] + k2 = [0.0 for _ in range(2)] + k3 = [0.0 for _ in range(2)] + + functions = [self._func_x_1, self._func_x_2] + + # solve Runge-Kutta + for i, func in enumerate(functions): + k0[i] = dt * func(self.x_1, self.x_2, u) + + for i, func in enumerate(functions): + k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., u) + + for i, func in enumerate(functions): + k2[i] = dt * func(self.x_1 + k1[0]/2., self.x_2 + k1[1]/2., u) + + for i, func in enumerate(functions): + k3[i] = dt * func(self.x_1 + k2[0], self.x_2 + k2[1], u) + + self.x_1 += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6. + self.x_2 += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6. + + # save + self.history_x_1.append(self.x_1) + self.history_x_2.append(self.x_2) + + def _func_x_1(self, y_1, y_2, u): + """ + Parameters + ------------ + y_1 : float + y_2 : float + u : float + system input + """ + y_dot = y_2 + return y_dot + + def _func_x_2(self, y_1, y_2, u): + """ + Parameters + ------------ + y_1 : float + y_2 : float + u : float + system input + """ + y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u + return y_dot + + +class NMPCSimulatorSystem(): + """SimulatorSystem for nmpc, this is the simulator of nmpc + the reason why I seperate the real simulator and nmpc's simulator is sometimes the modeling error, disturbance can include in real simulator + Attributes + ----------- + None + + """ + def __init__(self): + """ + Parameters + ----------- + None + """ + pass + + def calc_predict_and_adjoint_state(self, x_1, x_2, us, N, dt): + """main + Parameters + ------------ + x_1 : float + current state + x_2 : float + current state + us : list of float + estimated optimal input Us for N steps + N : int + predict step + dt : float + sampling time + + Returns + -------- + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + lam_1s : list of float + adjoint state of x_1s, lam_1s for N steps + lam_2s : list of float + adjoint state of x_2s, lam_2s for N steps + """ + + x_1s, x_2s = self._calc_predict_states(x_1, x_2, us, N, dt) # by usin state equation + lam_1s, lam_2s = self._calc_adjoint_states(x_1s, x_2s, us, N, dt) # by using adjoint equation + + return x_1s, x_2s, lam_1s, lam_2s + + def _calc_predict_states(self, x_1, x_2, us, N, dt): + """ + Parameters + ------------ + x_1 : float + current state + x_2 : float + current state + us : list of float + estimated optimal input Us for N steps + N : int + predict step + dt : float + sampling time + + Returns + -------- + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + """ + # initial state + x_1s = [x_1] + x_2s = [x_2] + + for i in range(N): + temp_x_1, temp_x_2 = self._predict_state_with_oylar(x_1s[i], x_2s[i], us[i], dt) + x_1s.append(temp_x_1) + x_2s.append(temp_x_2) + + return x_1s, x_2s + + def _calc_adjoint_states(self, x_1s, x_2s, us, N, dt): + """ + Parameters + ------------ + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + us : list of float + estimated optimal input Us for N steps + N : int + predict step + dt : float + sampling time + + Returns + -------- + lam_1s : list of float + adjoint state of x_1s, lam_1s for N steps + lam_2s : list of float + adjoint state of x_2s, lam_2s for N steps + """ + # final state + # final_state_func + lam_1s = [x_1s[-1]] + lam_2s = [x_2s[-1]] + + for i in range(N-1, 0, -1): + temp_lam_1, temp_lam_2 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], lam_1s[0] ,lam_2s[0], us[i], dt) + lam_1s.insert(0, temp_lam_1) + lam_2s.insert(0, temp_lam_2) + + return lam_1s, lam_2s + + def final_state_func(self): + """this func usually need + """ + pass + + def _predict_state_with_oylar(self, x_1, x_2, u, dt): + """in this case this function is the same as simulator + Parameters + ------------ + x_1 : float + system state + x_2 : float + system state + u : float + system input + dt : float in seconds + sampling time + Returns + -------- + next_x_1 : float + next state, x_1 calculated by using state equation + next_x_2 : float + next state, x_2 calculated by using state equation + """ + k0 = [0. for _ in range(2)] + + functions = [self.func_x_1, self.func_x_2] + + for i, func in enumerate(functions): + k0[i] = dt * func(x_1, x_2, u) + + next_x_1 = x_1 + k0[0] + next_x_2 = x_2 + k0[1] + + return next_x_1, next_x_2 + + def func_x_1(self, y_1, y_2, u): + """calculating \dot{x_1} + Parameters + ------------ + y_1 : float + means x_1 + y_2 : float + means x_2 + u : float + means system input + Returns + --------- + y_dot : float + means \dot{x_1} + """ + y_dot = y_2 + return y_dot + + def func_x_2(self, y_1, y_2, u): + """calculating \dot{x_2} + Parameters + ------------ + y_1 : float + means x_1 + y_2 : float + means x_2 + u : float + means system input + Returns + --------- + y_dot : float + means \dot{x_2} + """ + y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u + return y_dot + + def _adjoint_state_with_oylar(self, x_1, x_2, lam_1, lam_2, u, dt): + """ + Parameters + ------------ + x_1 : float + system state + x_2 : float + system state + lam_1 : float + adjoint state + lam_2 : float + adjoint state + u : float + system input + dt : float in seconds + sampling time + Returns + -------- + pre_lam_1 : float + pre, 1 step before lam_1 calculated by using adjoint equation + pre_lam_2 : float + pre, 1 step before lam_2 calculated by using adjoint equation + """ + k0 = [0. for _ in range(2)] + + functions = [self._func_lam_1, self._func_lam_2] + + for i, func in enumerate(functions): + k0[i] = dt * func(x_1, x_2, lam_1, lam_2, u) + + pre_lam_1 = lam_1 + k0[0] + pre_lam_2 = lam_2 + k0[1] + + return pre_lam_1, pre_lam_2 + + def _func_lam_1(self, y_1, y_2, y_3, y_4, u): + """calculating -\dot{lam_1} + Parameters + ------------ + y_1 : float + means x_1 + y_2 : float + means x_2 + y_3 : float + means lam_1 + y_4 : float + means lam_2 + u : float + means system input + Returns + --------- + y_dot : float + means -\dot{lam_1} + """ + y_dot = y_1 - (2. * y_1 * y_2 + 1.) * y_4 + return y_dot + + def _func_lam_2(self, y_1, y_2, y_3, y_4, u): + """calculating -\dot{lam_2} + Parameters + ------------ + y_1 : float + means x_1 + y_2 : float + means x_2 + y_3 : float + means lam_1 + y_4 : float + means lam_2 + u : float + means system input + Returns + --------- + y_dot : float + means -\dot{lam_2} + """ + y_dot = y_2 + y_3 + (-3. * (y_2**2) - y_1**2 + 1. ) * y_4 + return y_dot + +class NMPCController_with_CGMRES(): + """ + Attributes + ------------ + zeta : float + gain of optimal answer stability + ht : float + update value of NMPC this should be decided by zeta + tf : float + predict time + alpha : float + gain of predict time + N : int + predicte step, discritize value + threshold : float + cgmres's threshold value + input_num : int + system input length, this should include dummy u and constraint variables + max_iteration : int + decide by the solved matrix size + simulator : NMPCSimulatorSystem class + us : list of float + estimated optimal system input + dummy_us : list of float + estimated dummy input + raws : list of float + estimated constraint variable + history_u : list of float + time history of actual system input + history_dummy_u : list of float + time history of actual dummy u + history_raw : list of float + time history of actual raw + history_f : list of float + time history of error of optimal + """ + def __init__(self): + """ + Parameters + ----------- + None + """ + # parameters + self.zeta = 100. # 安定化ゲイン + self.ht = 0.01 # 差分近似の幅 + self.tf = 1. # 最終時間 + self.alpha = 0.5 # 時間の上昇ゲイン + self.N = 10 # 分割数 + self.threshold = 0.001 # break値 + + self.input_num = 3 # dummy, 制約条件に対するuにも合わせた入力の数 + self.max_iteration = self.input_num * self.N + + # simulator + self.simulator = NMPCSimulatorSystem() + + # initial + self.us = np.zeros(self.N) + self.dummy_us = np.ones(self.N) * 0.49 + self.raws = np.ones(self.N) * 0.011 + + # for fig + self.history_u = [] + self.history_dummy_u = [] + self.history_raw = [] + self.history_f = [] + + def calc_input(self, x_1, x_2, time): + """ + Parameters + ------------ + x_1 : float + current state + x_2 : float + current state + time : float in seconds + now time + Returns + -------- + us : list of float + estimated optimal system input + """ + # calculating sampling time + dt = self.tf * (1. - np.exp(-self.alpha * time)) / float(self.N) + + # x_dot + x_1_dot = self.simulator.func_x_1(x_1, x_2, self.us[0]) + x_2_dot = self.simulator.func_x_2(x_1, x_2, self.us[0]) + + dx_1 = x_1_dot * self.ht + dx_2 = x_2_dot * self.ht + + x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us, self.N, dt) + + # Fxt + Fxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, + self.raws, self.N, dt) + + # F + x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) + + F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, + self.raws, self.N, dt) + + right = -self.zeta * F - ((Fxt - F) / self.ht) + + du = self.us * self.ht + ddummy_u = self.dummy_us * self.ht + draw = self.raws * self.ht + + x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us + du, self.N, dt) + + Fuxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, + self.raws + draw, self.N, dt) + + left = ((Fuxt - Fxt) / self.ht) + + # calculationg cgmres + r0 = right - left + r0_norm = np.linalg.norm(r0) + + vs = np.zeros((self.max_iteration, self.max_iteration + 1)) # 数×iterarion回数 + + vs[:, 0] = r0 / r0_norm # 最初の基底を算出 + + hs = np.zeros((self.max_iteration + 1, self.max_iteration + 1)) + + e = np.zeros((self.max_iteration + 1, 1)) # in this case the state is 3(u and dummy_u) + e[0] = 1. + + for i in range(self.max_iteration): + du = vs[::3, i] * self.ht + ddummy_u = vs[1::3, i] * self.ht + draw = vs[2::3, i] * self.ht + + x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, self.us + du, self.N, dt) + + Fuxt = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us + du, self.dummy_us + ddummy_u, + self.raws + draw, self.N, dt) + + Av = (( Fuxt - Fxt) / self.ht) + + sum_Av = np.zeros(self.max_iteration) + + for j in range(i + 1): # グラムシュミットの直交化法です、和を取って差分を取って算出します + hs[j, i] = np.dot(Av, vs[:, j]) + sum_Av = sum_Av + hs[j, i] * vs[:, j] + + v_est = Av - sum_Av + + hs[i+1, i] = np.linalg.norm(v_est) + + vs[:, i+1] = v_est / hs[i+1, i] + + inv_hs = np.linalg.pinv(hs[:i+1, :i]) # この辺は教科書（実時間の方）にのっています + ys = np.dot(inv_hs, r0_norm * e[:i+1]) + + judge_value = r0_norm * e[:i+1] - np.dot(hs[:i+1, :i], ys[:i]) + + if np.linalg.norm(judge_value) < self.threshold or i == self.max_iteration-1: + update_value = np.dot(vs[:, :i-1], ys_pre[:i-1]).flatten() + du_new = du + update_value[::3] + ddummy_u_new = du + update_value[1::3] + draw_new = du + update_value[2::3] + break + + ys_pre = ys + + # update + self.us += du_new * self.ht + self.dummy_us += ddummy_u_new * self.ht + self.raws += draw_new * self.ht + + x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt) + + F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, self.us, self.dummy_us, + self.raws, self.N, dt) + + print("check F = {0}".format(np.linalg.norm(F))) + + # for save + self.history_f.append(np.linalg.norm(F)) + self.history_u.append(self.us[0]) + self.history_dummy_u.append(self.dummy_us[0]) + self.history_raw.append(self.raws[0]) + + return self.us + + def _calc_f(self, x_1s, x_2s, lam_1s, lam_2s, us, dummy_us, raws, N, dt): + """ + Parameters + ------------ + x_1s : list of float + predicted x_1s for N steps + x_2s : list of float + predicted x_2s for N steps + lam_1s : list of float + adjoint state of x_1s, lam_1s for N steps + lam_2s : list of float + adjoint state of x_2s, lam_2s for N steps + us : list of float + estimated optimal system input + dummy_us : list of float + estimated dummy input + raws : list of float + estimated constraint variable + N : int + predict time step + dt : float + sampling time of system + """ + F = [] + + for i in range(N): + F.append(us[i] + lam_2s[i] + 2. * raws[i] * us[i]) + F.append(-0.01 + 2. * raws[i] * dummy_us[i]) + F.append(us[i]**2 + dummy_us[i]**2 - 0.5**2) + + return np.array(F) + +def main(): + # simulation time + dt = 0.01 + iteration_time = 20. + iteration_num = int(iteration_time/dt) + + # plant + plant_system = SampleSystem(init_x_1=2., init_x_2=0.) + + # controller + controller = NMPCController_with_CGMRES() + + # for i in range(iteration_num) + for i in range(1, iteration_num): + time = float(i) * dt + x_1 = plant_system.x_1 + x_2 = plant_system.x_2 + # make input + us = controller.calc_input(x_1, x_2, time) + # update state + plant_system.update_state(us[0]) + + # figure + fig = plt.figure() + + x_1_fig = fig.add_subplot(321) + x_2_fig = fig.add_subplot(322) + u_fig = fig.add_subplot(323) + dummy_fig = fig.add_subplot(324) + raw_fig = fig.add_subplot(325) + f_fig = fig.add_subplot(326) + + x_1_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_1) + x_1_fig.set_xlabel("time [s]") + x_1_fig.set_ylabel("x_1") + + x_2_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_2) + x_2_fig.set_xlabel("time [s]") + x_2_fig.set_ylabel("x_2") + + u_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u) + u_fig.set_xlabel("time [s]") + u_fig.set_ylabel("u") + + dummy_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u) + dummy_fig.set_xlabel("time [s]") + dummy_fig.set_ylabel("dummy u") + + raw_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw) + raw_fig.set_xlabel("time [s]") + raw_fig.set_ylabel("raw") + + f_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_f) + f_fig.set_xlabel("time [s]") + f_fig.set_ylabel("optimal error") + + fig.tight_layout() + + plt.show() + + +if __name__ == "__main__": + main() + + +