From 20b9a9e7f4e22ba6d12d358ac1e5bd1f93180295 Mon Sep 17 00:00:00 2001
From: Shunichi09 <quick1st97of@gmail.com>
Date: Tue, 25 Dec 2018 02:11:46 +0900
Subject: [PATCH] add mpc

---
 mpc/__pycache__/mpc_func.cpython-36.pyc | Bin 0 -> 3196 bytes
 mpc/main_example.py                     |  63 ++++++
 mpc/mpc_func.py                         | 126 +++++++++++
 mpc/simulator_func.py                   | 264 ++++++++++++++++++++++++
 4 files changed, 453 insertions(+)
 create mode 100644 mpc/__pycache__/mpc_func.cpython-36.pyc
 create mode 100644 mpc/main_example.py
 create mode 100644 mpc/mpc_func.py
 create mode 100644 mpc/simulator_func.py

diff --git a/mpc/__pycache__/mpc_func.cpython-36.pyc b/mpc/__pycache__/mpc_func.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cb3f01cbf8cb014b0a7649f82fa511cc09a33a9c
GIT binary patch
literal 3196
zcmbtWOOM>f5$<k2hQpm1tyZgj7>2+^fnmz=?#c-eBv=cUE!(+x5h(@&5D^H9&Dr6I
zLvouOXLrImtqjRWADrJ1|AYLF9Q&G+t~nXLBvr+k*#~UEfH|1zs(w{<J-+VW^!x5N
z8^8GLKYNV*hh6#%;6H$Wg+wyRGZt`a#n=c;;;g`;)(&iFMb?=(fy3FSOd8UB!K8UE
zg0AF$X1?_rc40?8*Ulu((@8o@V(eLdD{c3nCm~INkUTJCm-gbI(USHH1{yokf!3A+
z_H}jtvoiju$ZJ((S)yidxgLBE>sqDHrgc)0`0l08rJkeMS*XTV!2cuoS0f-b8}o*r
z^M-|jctN~Dyg|H4yh*&(STNI;IKM;GY1mkdyHe~j=yO3bKr%>LJI1~q`NW)c$rd#D
zNO4g88j0W6A}_TSDHWY*{!sG|HGi!66YZ2L39C9OwRl`<@ubpLHHykayHy?4Fa%x=
z+%zwzHSyXVrBz+1vv6AZjy9_#8>$YdP<Uw7MdBh^0}`yY3#Vn7sHKn<e@oQ2Ol`n~
z?|;8PDkjN(HJavWJWBT`XJK5(q}tDmJWKN=QXo?x_9tbG|8O|X<Gu1sJ7EY>ufy;L
zY*cLmVMdR)KU>(`76U#Iv#l$!+*>Y+(JlC)#WwsgTdMy;?<+QD$9&FfJ{HKI^H(f1
zP9N1KNS5R~(DR5reQz#md)#TbT-&8J=VER^j{~!e#u!3`ab3g5-3B7e*~wdfVYMs8
zcnxQqLpE@>(9xs0{R?Sso_=^u-@)8!^)(jh>w!MIu^V%n&CP}0eq%25_D}vn+6>ax
zzoHE!uxxO8dQHQSG~58|V8-0QR+{yC183gG6W-i}ldrKag*2u4r5Iu^8rR2hWI2Aw
zz^)BgIav8)aPl45WszNjg<bae)=M_z(w3bUPHPqV7)R$F*rOeFeqhvr(aokazJ1=t
zX^UiMC2xbryQFJtq05yn$=j_gO{n<Z?7^yPCeic6n?z9Sr(Runag@c=3@X7Z$~v7y
znYYMC0EV{gvDB0a*)toIqcnu2-us^W_k;Ot{WzLTqDwtTv-NtE)R((fd5Cy{yWGM&
z+yR1B{d67j5U>M$2>JB2I00*^aC}2qFh`p*jgG16e4)kOf##vl)lC?pX$p7y-aX8q
zX5(T~KKK~q3RT%`_PeW<fyI<Y8Pxl-<lu;VhgktGs2%{b+~en&tZt)m2ob(eTgY!C
zf8A(>F8BogOKY9S75Ek1>O5I+um1<1zeDh&&A32NkV#S!*I5KZTPn(r6T+j`XND|l
zZGQpy7C+Z^1lW{IZIvp`p?g>;3_gOH?ylmeyQ{cqcb2H43bXWiq77K15P!5iqWu&?
z2^~>)hLAROk}J3cnd$@ayA+wA_b?nrF@PwzyQ1R<PMu6j%#`2;Ds8hS#7@oAz@^dq
zG~U68hmWhEWu8=RJ}mr)@z%G0Fq9q)RlfzoEJqB)Cg0#2(5?ea_DFL07D~YP_#=V*
z9eziw^L6;o?*4>ii4jBbL+oE+>c2psyA5&}?m$FIybTd0+1Ql!E`vU^5sk6K8gqxu
zE!p0YfVN|MF}KrL=i*#!vagnVT^@s(I12kY+w3d;lD*_ZL3g|}#~r#b;u4mu0b+JQ
zpVK(=Zqt>3rDR2CmjRCAy=sno4b0`NaRC{e*}qrF@TW*clcY{m`vAbYzx)P389%x=
zEAJ0pKAn_jdvxWl_P_~{N*-RTUNtJFne;}{7m4>aE%UM@KdwjKJ@3B1TBDL-qLMsL
zVAECSPu^&Gzx&tF>KFqB-v_I5#v2hqg%Idhz$~MN?JI~F`j~i(BuG09Jj7@HD@HAl
zN!@`BH2>V^@Vct;u^=@6RP#Sx;F2B?q=F|TUFs36plWBv?BHs;AqvB7508)wK)Sk^
z-a2X*wjkp7sn*E~^(OY-MY4_rkEY&1vZ_pNQBk)bf$*q2ApTlguXY>Kz`ck>V7Dc?
zXy4OTqEw;O9xTJ>61+?3)PW0ThAOG1SsiqdZ@X<|r;FMZ>es09T_jY%?!|eJpp&ix
z<_`F#=!rYxmttV`gey$30S}(p&X2ju7e_~a2j59K<ZT#gHw-6*oMy=P!ti7oWs8wc
z7|J3JLxs;Mh3{o`9|^wEE)g_NVUZ&|R!AJn6z)xDk9y(dY*Es;IC~S|tjvl!OP|4e
z4LNNuTBFQrZNm6Sy#c!fgDOs;R+b@E6arLqNjcd<?r{#=V?4l&t?(*wxXT^xfP4NA
DhT{N7

literal 0
HcmV?d00001

diff --git a/mpc/main_example.py b/mpc/main_example.py
new file mode 100644
index 0000000..2b97037
--- /dev/null
+++ b/mpc/main_example.py
@@ -0,0 +1,63 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+
+from mpc_func import MpcController
+# from simulator_func import FirstOrderSystem
+from control import matlab
+
+def main():
+    dt = 0.01
+    simulation_time = 100 # 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.],
+                  [1., 0., 0., 0.], 
+                  [1., 0., 0., 0.]])
+    B = np.array([[1./tau, 0.],
+                  [0., 1./tau],
+                  [0., 0.],
+                  [0., 0.]])
+
+    C = np.eye(4)
+    D = np.zeros((4, 2))
+
+    # create system
+    sysc = matlab.ss(A, B, C, D)
+    # discrete system
+    sysd = matlab.c2d(sysc, dt)
+
+    Ad = sysd.A
+    Bd = sysd.B
+
+    Q = np.diag([1., 1., 1., 1.])
+    R = np.diag([1., 1.])
+    pre_step = 3
+
+    # make controller
+    controller = MpcController(Ad, Bd, Q, R, pre_step)
+    controller.initialize_controller()
+
+    # make simulator
+    # plant = FirstOrderSystem(tau)
+
+    """
+    for i in range(iteration_num):
+    """
+
+    # states = plant.states
+    # controller.calc_input
+
+if __name__ == "__main__":
+    main()
+
+
+
+
+
+
+
diff --git a/mpc/mpc_func.py b/mpc/mpc_func.py
new file mode 100644
index 0000000..9e18b24
--- /dev/null
+++ b/mpc/mpc_func.py
@@ -0,0 +1,126 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import copy
+
+from scipy.optimize import minimize
+
+class MpcController():
+    """
+    Attributes
+    ------------
+
+    """
+
+    def __init__(self, A, B, Q, R, pre_step, input_upper=None, input_lower=None):
+        """
+        """
+        self.A = np.array(A)
+        self.B = np.array(B)
+        self.Q = np.array(Q)
+        self.R = np.array(R)
+        self.pre_step = pre_step
+
+        self.Qs = None
+        self.Rs = None
+
+        self.state_size = self.A.shape[0]
+        self.input_size = self.B.shape[1]
+
+        self.history_us = []
+
+    def initialize_controller(self):
+        """
+        make matrix to calculate optimal controller
+
+        """
+        A_factorials = [self.A]
+
+        self.phi_mat = copy.deepcopy(self.A)
+
+        for _ in range(self.pre_step - 1):
+            temp_mat = np.dot(A_factorials[-1], self.A)
+            self.phi_mat = np.vstack((self.phi_mat, temp_mat))
+
+            A_factorials.append(temp_mat) # after we use this factorials
+            
+        print("phi_mat = \n{0}".format(self.phi_mat))
+
+        self.gamma_mat = copy.deepcopy(self.B)
+        gammma_mat_temp = copy.deepcopy(self.B)
+        
+        for i in range(self.pre_step - 1):
+            temp_1_mat = np.dot(A_factorials[i], self.B)
+            gammma_mat_temp = temp_1_mat + gammma_mat_temp
+            self.gamma_mat = np.vstack((self.gamma_mat, gammma_mat_temp))
+
+        print("gamma_mat = \n{0}".format(self.gamma_mat))
+
+        self.theta_mat = copy.deepcopy(self.gamma_mat)
+
+        for i in range(self.pre_step - 1):
+            temp_mat = np.zeros_like(self.gamma_mat)
+            temp_mat[int((i + 1)*self.state_size): , :] = self.gamma_mat[:-int((i + 1)*self.state_size) , :]
+
+            self.theta_mat = np.hstack((self.theta_mat, temp_mat))
+
+        print("theta_mat = \n{0}".format(self.theta_mat))
+
+        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)])
+        
+        self.Qs = np.diag(diag_Qs.flatten())
+        self.Rs = np.diag(diag_Rs.flatten())
+
+        print("Qs = {0}".format(self.Qs))
+        print("Rs = {0}".format(self.Rs))
+
+    def calc_input(self, states, references):
+        """
+        Parameters
+        -----------
+        states : numpy.array
+            the size should have (state length * 1)
+        references :
+            the size should have (state length * pre_step)
+
+        """
+        temp_1 = np.dot(self.phi_mat, states)
+        temp_2 = np.dot(self.gamma_mat, self.history_us[-1])
+
+        error = references - temp_1 - temp_2
+
+        G = 2. * np.dot(self.theta_mat.T, np.dot(self.Qs, error) )
+
+        H = np.dot(self.theta_mat.T, np.dot(self.Qs, self.theta_mat)) + self.Rs
+
+        def optimized_func(dt_us):
+            """
+            """
+            return np.dot(dt_us.T, np.dot(H, dt_us)) - np.dot(G.T, dt_us)
+
+        init_dt_us = np.zeros(self.pre_step)
+
+        opt_result = minimize(optimized_func, init_dt_us)
+
+        opt_dt_us = opt_result
+
+        opt_us = opt_dt_us[0] + self.history_us[-1]
+
+        # save
+        self.history_us.append(opt_us)
+        return opt_us
+
+
+    
+
+
+
+
+
+        
+
+        
+
+
+
diff --git a/mpc/simulator_func.py b/mpc/simulator_func.py
new file mode 100644
index 0000000..b4460ff
--- /dev/null
+++ b/mpc/simulator_func.py
@@ -0,0 +1,264 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+
+class FirstOrderSystem():
+    """FirstOrderSystem
+
+    Attributes
+    -----------
+    state : float
+        system state, this system should have one input - one output
+    a : float
+        parameter of the system
+    b : float
+        parameter of the system
+    history_state : list
+        time history of state
+    """
+    def __init__(self, a, b, init_state=0.0):
+        """
+        Parameters
+        -----------
+        a : float
+            parameter of the system
+        b : float
+            parameter of the system
+        init_state : float, optional
+            initial state of system default is 0.0
+        """
+        self.state = init_state
+        self.a = a
+        self.b = b
+        self.history_state = [init_state]
+
+    def update_state(self, u, dt=0.01):
+        """calculating input
+        Parameters
+        ------------
+        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]
+        """
+        # solve Runge-Kutta
+        k0 = dt * self._func(self.state, u)
+        k1 = dt * self._func(self.state + k0/2.0, u)
+        k2 = dt * self._func(self.state + k1/2.0, u)
+        k3 = dt * self._func(self.state + k2, u)
+
+        self.state +=  (k0 + 2 * k1 + 2 * k2 + k3) / 6.0
+
+        # for oylar
+        # self.state += k0
+
+        # save
+        self.history_state.append(self.state)
+
+    def _func(self, y, u):
+        """
+        Parameters
+        ------------
+        y : float
+            state of system
+        u : float
+            input of system in some cases this means the reference
+        """
+        y_dot = -self.a * y + self.b * u
+
+        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