add mpc with noise

mpc/with_disturbance/animation.py

@@ -0,0 +1,233 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.animation as ani
+import matplotlib.font_manager as fon
+import sys
+import math
+
+# default setting of figures
+plt.rcParams["mathtext.fontset"] = 'stix' # math fonts
+plt.rcParams['xtick.direction'] = 'in' # x axis in
+plt.rcParams['ytick.direction'] = 'in' # y axis in 
+plt.rcParams["font.size"] = 10
+plt.rcParams['axes.linewidth'] = 1.0 # axis line width
+plt.rcParams['axes.grid'] = True # make grid
+
+def coordinate_transformation_in_angle(positions, base_angle):
+    '''
+    Transformation the coordinate in the angle
+
+    Parameters
+    -------
+    positions : numpy.ndarray
+        this parameter is composed of xs, ys 
+        should have (2, N) shape 
+    base_angle : float [rad]
+    
+    Returns
+    -------
+    traslated_positions : numpy.ndarray
+        the shape is (2, N)
+    
+    '''
+    if positions.shape[0] != 2:
+        raise ValueError('the input data should have (2, N)')
+
+    positions = np.array(positions)
+    positions = positions.reshape(2, -1)
+
+    rot_matrix = [[np.cos(base_angle), np.sin(base_angle)],
+                  [-1*np.sin(base_angle), np.cos(base_angle)]]
+
+    rot_matrix = np.array(rot_matrix)
+    
+    translated_positions = np.dot(rot_matrix, positions)
+
+    return translated_positions
+
+def square_make_with_angles(center_x, center_y, size, angle):
+    '''
+    Create square matrix with angle line matrix(2D)
+    
+    Parameters
+    -------
+    center_x : float in meters
+        the center x position of the square
+    center_y : float in meters
+        the center y position of the square
+    size : float in meters
+        the square's half-size
+    angle : float in radians
+
+    Returns
+    -------
+    square xs : numpy.ndarray
+        lenght is 5 (counterclockwise from right-up)
+    square ys : numpy.ndarray
+        length is 5 (counterclockwise from right-up)
+    angle line xs : numpy.ndarray
+    angle line ys : numpy.ndarray
+    '''
+
+    # start with the up right points
+    # create point in counterclockwise
+    square_xys = np.array([[size, 0.5 * size], [-size, 0.5 * size], [-size, -0.5 * size], [size, -0.5 * size], [size, 0.5 * size]])
+    trans_points = coordinate_transformation_in_angle(square_xys.T, -angle) # this is inverse type
+    trans_points += np.array([[center_x], [center_y]])
+
+    square_xs = trans_points[0, :]
+    square_ys = trans_points[1, :]
+
+    angle_line_xs = [center_x, center_x + math.cos(angle) * size]
+    angle_line_ys = [center_y, center_y + math.sin(angle) * size]
+
+    return square_xs, square_ys, np.array(angle_line_xs), np.array(angle_line_ys)
+
+
+class AnimDrawer():
+    """create animation of path and robot
+    
+    Attributes
+    ------------
+    cars : 
+    anim_fig : figure of matplotlib
+    axis : axis of matplotlib
+
+    """
+    def __init__(self, objects):
+        """
+        Parameters
+        ------------
+        objects : list of objects
+        """
+        self.lead_car_history_state = objects[0]
+        self.follow_car_history_state = objects[1]
+        
+        self.history_xs = [self.lead_car_history_state[:, 0], self.follow_car_history_state[:, 0]]
+        self.history_ys = [self.lead_car_history_state[:, 1], self.follow_car_history_state[:, 1]]
+        self.history_ths = [self.lead_car_history_state[:, 2], self.follow_car_history_state[:, 2]]
+
+        # setting up figure
+        self.anim_fig = plt.figure(dpi=150)
+        self.axis = self.anim_fig.add_subplot(111)
+
+        # imgs
+        self.object_imgs = []
+        self.traj_imgs = []
+
+    def draw_anim(self, interval=50):
+        """draw the animation and save
+
+        Parameteres
+        -------------
+        interval : int, optional
+            animation's interval time, you should link the sampling time of systems
+            default is 50 [ms]
+        """
+        self._set_axis()
+        self._set_img()
+
+        self.skip_num = 1
+        frame_num = int((len(self.history_xs[0])-1) / self.skip_num)
+
+        animation = ani.FuncAnimation(self.anim_fig, self._update_anim, interval=interval, frames=frame_num)
+
+        # self.axis.legend()
+        print('save_animation?')
+        shuold_save_animation = int(input())
+
+        if shuold_save_animation: 
+            print('animation_number?')
+            num = int(input())
+            animation.save('animation_{0}.mp4'.format(num), writer='ffmpeg')
+            # animation.save("Sample.gif", writer = 'imagemagick') # gif保存
+
+        plt.show()
+
+    def _set_axis(self):
+        """ initialize the animation axies
+        """
+        # (1) set the axis name
+        self.axis.set_xlabel(r'$\it{x}$ [m]')
+        self.axis.set_ylabel(r'$\it{y}$ [m]')
+        self.axis.set_aspect('equal', adjustable='box')
+
+        # (2) set the xlim and ylim        
+        self.axis.set_xlim(-5, 50)
+        self.axis.set_ylim(-2, 5)         
+
+    def _set_img(self):
+        """ initialize the imgs of animation
+            this private function execute the make initial imgs for animation
+        """
+        # object imgs
+        obj_color_list = ["k", "k", "m", "m"]
+        obj_styles = ["solid", "solid", "solid", "solid"]
+
+        for i in range(len(obj_color_list)):
+            temp_img, = self.axis.plot([], [], color=obj_color_list[i], linestyle=obj_styles[i])
+            self.object_imgs.append(temp_img)
+        
+        traj_color_list = ["k", "m"]
+
+        for i in range(len(traj_color_list)):
+            temp_img, = self.axis.plot([],[], color=traj_color_list[i], linestyle="dashed")
+            self.traj_imgs.append(temp_img)
+    
+    def _update_anim(self, i):
+        """the update animation
+        this function should be used in the animation functions
+
+        Parameters
+        ------------
+        i : int
+            time step of the animation
+            the sampling time should be related to the sampling time of system
+
+        Returns
+        -----------
+        object_imgs : list of img
+        traj_imgs : list of img
+        """
+        i = int(i * self.skip_num)
+
+        self._draw_objects(i)
+        self._draw_traj(i)
+
+        return self.object_imgs, self.traj_imgs,
+        
+    def _draw_objects(self, i):
+        """
+        This private function is just divided thing of
+        the _update_anim to see the code more clear
+
+        Parameters
+        ------------
+        i : int
+            time step of the animation
+            the sampling time should be related to the sampling time of system
+        """
+        # cars
+        for j in range(2):
+            fix_j = j * 2
+            object_x, object_y, angle_x, angle_y = square_make_with_angles(self.history_xs[j][i],
+                                                                           self.history_ys[j][i], 
+                                                                           1.0,
+                                                                           self.history_ths[j][i])
+
+            self.object_imgs[fix_j].set_data([object_x, object_y])
+            self.object_imgs[fix_j + 1].set_data([angle_x, angle_y])
+    
+    def _draw_traj(self, i):
+        """
+        This private function is just divided thing of
+        the _update_anim to see the code more clear
+
+        Parameters
+        ------------
+        i : int
+            time step of the animation
+            the sampling time should be related to the sampling time of system
+        """
+        for j in range(2):
+            self.traj_imgs[j].set_data(self.history_xs[j][:i], self.history_ys[j][:i])

mpc/with_disturbance/main_track.py

@@ -9,10 +9,12 @@ from control import matlab
 
 class WheeledSystem():
     """SampleSystem, this is the simulator
+        Kinematic model of car
+
     Attributes
     -----------
     xs : numpy.ndarray
-        system states, [x, y, theta]
+        system states, [x, y, phi, beta]
     history_xs : list
         time history of state
     """
@@ -23,7 +25,11 @@ class WheeledSystem():
         init_state : float, optional, shape(3, )
             initial state of system default is None
         """
-        self.xs = np.zeros(3)
+        self.NUM_STATE = 4
+        self.xs = np.zeros(self.NUM_STATE)
+
+        self.FRONT_WHEELE_BASE = 1.0
+        self.REAR_WHEELE_BASE = 1.0
 
         if init_states is not None:
             self.xs = copy.deepcopy(init_states)
@@ -40,36 +46,37 @@ class WheeledSystem():
             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(3)]
+        k0 = [0.0 for _ in range(self.NUM_STATE)]
-        k1 = [0.0 for _ in range(3)]
+        k1 = [0.0 for _ in range(self.NUM_STATE)]
-        k2 = [0.0 for _ in range(3)]
+        k2 = [0.0 for _ in range(self.NUM_STATE)]
-        k3 = [0.0 for _ in range(3)]
+        k3 = [0.0 for _ in range(self.NUM_STATE)]
 
-        functions = [self._func_x_1, self._func_x_2, self._func_x_3]
+        functions = [self._func_x_1, self._func_x_2, self._func_x_3, self._func_x_4]
 
         # solve Runge-Kutta
         for i, func in enumerate(functions):
-            k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], us[0], us[1])
+            k0[i] = dt * func(self.xs[0], self.xs[1], self.xs[2], self.xs[3], us[0], us[1])
 
         for i, func in enumerate(functions):
-            k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1])
+            k1[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2.,  self.xs[3] + k0[3]/2, us[0], us[1])
         
         for i, func in enumerate(functions):
-            k2[i] = dt * func(self.xs[0] + k0[0]/2., self.xs[1] + k0[1]/2., self.xs[2] + k0[2]/2., us[0], us[1])
+            k2[i] = dt * func(self.xs[0] + k1[0]/2., self.xs[1] + k1[1]/2., self.xs[2] + k1[2]/2., self.xs[3] + k1[3]/2., us[0], us[1])
         
         for i, func in enumerate(functions):
-            k3[i] =  dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], us[0], us[1])
+            k3[i] =  dt * func(self.xs[0] + k2[0], self.xs[1] + k2[1], self.xs[2] + k2[2], self.xs[3] + k2[3], us[0], us[1])
         
         self.xs[0] += (k0[0] + 2. * k1[0] + 2. * k2[0] + k3[0]) / 6.
         self.xs[1] += (k0[1] + 2. * k1[1] + 2. * k2[1] + k3[1]) / 6.
         self.xs[2] += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6.
+        self.xs[3] += (k0[3] + 2. * k1[3] + 2. * k2[3] + k3[3]) / 6.
     
         # save
         save_states = copy.deepcopy(self.xs)
         self.history_xs.append(save_states)
         print(self.xs)
 
-    def _func_x_1(self, y_1, y_2, y_3, u_1, u_2):
+    def _func_x_1(self, y_1, y_2, y_3, y_4, u_1, u_2):
         """
         Parameters
         ------------
@@ -81,10 +88,10 @@ class WheeledSystem():
         u_2 : float
             system input
         """
-        y_dot = math.cos(y_3) * u_1
+        y_dot = u_1 * math.cos(y_3 + y_4)
         return y_dot
     
-    def _func_x_2(self, y_1, y_2, y_3, u_1, u_2):
+    def _func_x_2(self, y_1, y_2, y_3, y_4, u_1, u_2):
         """
         Parameters
         ------------
@@ -96,10 +103,10 @@ class WheeledSystem():
         u_2 : float
             system input
         """
-        y_dot = math.sin(y_3) * u_1
+        y_dot = u_1 * math.sin(y_3 + y_4)
         return y_dot
     
-    def _func_x_3(self, y_1, y_2, y_3, u_1, u_2):
+    def _func_x_3(self, y_1, y_2, y_3, y_4, u_1, u_2):
         """
         Parameters
         ------------
@@ -111,11 +118,18 @@ class WheeledSystem():
         u_2 : float
             system input
         """
-        y_dot = u_2
+        y_dot = u_1 / self.REAR_WHEELE_BASE * math.sin(y_4)
+        return y_dot
+
+    def _func_x_4(self, y_1, y_2, y_3, y_4, u_1, u_2):
+        """
+        """
+        y_dot = math.atan2(self.REAR_WHEELE_BASE / (self.REAR_WHEELE_BASE + self.FRONT_WHEELE_BASE) * math.tan(u_2))
+
         return y_dot
 
 def main():
-    dt = 0.05
+    dt = 0.016
     simulation_time = 10 # in seconds
     iteration_num = int(simulation_time / dt)
 
@@ -123,38 +137,33 @@ def main():
     # these A and B are for continuos system if you want to use discret system matrix please skip this step
     # lineared car system
     V = 5.0
-    A = np.array([[0., V], [0., 0.]])
+    Ad = np.array([[1., 0., 0., 0.],
-    B = np.array([[0.], [1.]])
+                   [0., 1,  V, 0.],
+                   [0., 0., 1., 0.],
+                   [0., 0., 1., 0.]]) * dt
 
-    C = np.eye(2)
+    Bd = np.array([[0.], [0.], [0.], [0.3]]) * dt
-    D = np.zeros((2, 1))
+
+    W_D = np.array([[V], [0.], [0.], [0.]]) * dt
 
     # make simulator with coninuous matrix
-    init_xs_lead = np.array([5., 0., 0.])
+    init_xs_lead = np.array([5., 0., 0. ,0.])
-    init_xs_follow = np.array([0., 0., 0.])
+    init_xs_follow = np.array([0., 0., 0., 0.])
-    lead_car = TwoWheeledSystem(init_states=init_xs_lead)
+    lead_car = WheeledSystem(init_states=init_xs_lead)
-    follow_car = TwoWheeledSystem(init_states=init_xs_follow)
+    follow_car = WheeledSystem(init_states=init_xs_follow)
-
-    # create system
-    sysc = matlab.ss(A, B, C, D)
-    # discrete system
-    sysd = matlab.c2d(sysc, dt)
-
-    Ad = sysd.A
-    Bd = sysd.B
 
     # evaluation function weight
-    Q = np.diag([1., 1.])
+    Q = np.diag([1., 1., 1., 1.])
     R = np.diag([5.])
-    pre_step = 15
+    pre_step = 2
 
     # make controller with discreted matrix
     # please check the solver, if you want to use the scipy, set the MpcController_scipy
-    lead_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
+    lead_controller = MpcController_cvxopt(Ad, Bd, W_D, Q, R, pre_step,
                                dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
                                input_upper=np.array([30.]), input_lower=np.array([-30.]))
 
-    follow_controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
+    follow_controller = MpcController_cvxopt(Ad, Bd, W_D, Q, R, pre_step,
                                dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
                                input_upper=np.array([30.]), input_lower=np.array([-30.]))
 

mpc/with_disturbance/mpc_func_with_cvxopt.py

@@ -0,0 +1,297 @@
+import numpy as np
+np.set_printoptions(threshold=np.inf)
+
+import matplotlib.pyplot as plt
+import math
+import copy
+
+from cvxopt import matrix, solvers
+
+class MpcController():
+    """
+    Attributes
+    ------------
+    A : numpy.ndarray
+        system matrix
+    B : numpy.ndarray
+        input matrix
+    W_D : numpy.ndarray
+        distubance matrix in state equation
+    Q : numpy.ndarray
+        evaluation function weight for states
+    Qs : numpy.ndarray
+        concatenated evaluation function weight for states
+    R : numpy.ndarray
+        evaluation function weight for inputs
+    Rs : numpy.ndarray
+        concatenated evaluation function weight for inputs
+    pre_step : int
+        prediction step
+    state_size : int
+        state size of the plant
+    input_size : int
+        input size of the plant
+    dt_input_upper : numpy.ndarray, shape(input_size, ), optional
+        constraints of input dt, default is None
+    dt_input_lower : numpy.ndarray, shape(input_size, ), optional
+        constraints of input dt, default is None
+    input_upper : numpy.ndarray, shape(input_size, ), optional
+        constraints of input, default is None
+    input_lower : numpy.ndarray, shape(input_size, ), optional
+        constraints of input, default is None
+    """
+    def __init__(self, A, B, W_D, Q, R, pre_step, initial_input=None, dt_input_upper=None, dt_input_lower=None, input_upper=None, input_lower=None):
+        """
+        Parameters
+        ------------
+        A : numpy.ndarray
+            system matrix
+        B : numpy.ndarray
+            input matrix
+        W_D : numpy.ndarray
+            distubance matrix in state equation
+        Q : numpy.ndarray
+            evaluation function weight for states
+        R : numpy.ndarray
+            evaluation function weight for inputs
+        pre_step : int
+            prediction step
+        dt_input_upper : numpy.ndarray, shape(input_size, ), optional
+            constraints of input dt, default is None
+        dt_input_lower : numpy.ndarray, shape(input_size, ), optional
+            constraints of input dt, default is None
+        input_upper : numpy.ndarray, shape(input_size, ), optional
+            constraints of input, default is None
+        input_lower : numpy.ndarray, shape(input_size, ), optional
+            constraints of input, default is None
+        history_us : list
+            time history of optimal input us(numpy.ndarray)
+        """
+        self.A = np.array(A)
+        self.B = np.array(B)
+        self.W_D = np.array(W_D)
+        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 = [np.zeros(self.input_size)]
+
+        # initial state
+        if initial_input is not None:
+            self.history_us = [initial_input]
+
+        # constraints
+        self.dt_input_lower = dt_input_lower
+        self.dt_input_upper = dt_input_upper
+        self.input_upper = input_upper
+        self.input_lower = input_lower
+        
+        # about mpc matrix
+        self.W = None
+        self.omega = None
+        self.F = None
+        self.f = None
+        
+    def initialize_controller(self):
+        """
+        make matrix to calculate optimal control input
+        
+        """
+        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))
+        
+        # disturbance
+        print("A_factorials_mat = \n{0}".format(A_factorials))
+        A_factorials_mat = np.array(A_factorials).reshape(-1, self.state_size)
+        print("A_factorials_mat = \n{0}".format(A_factorials_mat))
+
+        eye = np.eye(self.state_size)
+        self.dist_mat = np.vstack((eye, A_factorials_mat[:-self.state_size, :]))
+        base_mat = copy.deepcopy(self.dist_mat)
+
+        print("base_mat = \n{0}".format(base_mat))
+
+        for i in range(self.pre_step - 1):
+            temp_mat = np.zeros_like(A_factorials_mat)
+            temp_mat[int((i + 1)*self.state_size): , :] = base_mat[:-int((i + 1)*self.state_size) , :]
+            self.dist_mat = np.hstack((self.dist_mat, temp_mat))
+
+        print("dist_mat = \n{0}".format(self.dist_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)])
+        
+        self.Qs = np.diag(diag_Qs.flatten())
+        self.Rs = np.diag(diag_Rs.flatten())
+
+        print("Qs = \n{0}".format(self.Qs))
+        print("Rs = \n{0}".format(self.Rs))
+
+        # constraints
+        # about dt U
+        if self.input_lower is not None:
+            # initialize
+            self.F = np.zeros((self.input_size * 2, self.pre_step * self.input_size))
+            for i in range(self.input_size):
+                self.F[i * 2: (i + 1) * 2, i] = np.array([1.,  -1.])
+                temp_F = copy.deepcopy(self.F)
+
+            print("F = \n{0}".format(self.F))
+
+            for i in range(self.pre_step - 1):
+                temp_F = copy.deepcopy(temp_F)
+
+                for j in range(self.input_size):
+                    temp_F[j * 2: (j + 1) * 2, ((i+1) * self.input_size) + j] = np.array([1.,  -1.])
+                
+                self.F = np.vstack((self.F, temp_F))
+
+            self.F1 = self.F[:, :self.input_size]
+            
+            temp_f = []
+
+            for i in range(self.input_size):
+                temp_f.append(-1 * self.input_upper[i])
+                temp_f.append(self.input_lower[i])
+
+            self.f = np.array([temp_f for _ in range(self.pre_step)]).flatten()
+
+            print("F = \n{0}".format(self.F))
+            print("F1 = \n{0}".format(self.F1))
+            print("f = \n{0}".format(self.f))
+
+        # about dt_u
+        if self.dt_input_lower is not None:
+            self.W = np.zeros((2, self.pre_step * self.input_size))
+            self.W[:, 0] = np.array([1.,  -1.])
+
+            for i in range(self.pre_step * self.input_size - 1):
+                temp_W = np.zeros((2, self.pre_step * self.input_size))
+                temp_W[:, i+1] = np.array([1.,  -1.])
+                self.W = np.vstack((self.W, temp_W))
+
+            temp_omega = []
+
+            for i in range(self.input_size):
+                temp_omega.append(self.dt_input_upper[i])
+                temp_omega.append(-1. * self.dt_input_lower[i])
+
+            self.omega = np.array([temp_omega for _ in range(self.pre_step)]).flatten()
+
+            print("W = \n{0}".format(self.W))
+            print("omega = \n{0}".format(self.omega))
+
+        # about state
+        print("check the matrix!! if you think rite, plese push enter")
+        input()
+
+    def calc_input(self, states, references):
+        """calculate optimal input
+        Parameters
+        -----------
+        states : numpy.ndarray, shape(state length, )
+            current state of system
+        references : numpy.ndarray, shape(state length * pre_step, )
+            reference of the system, you should set this value as reachable goal
+
+        References
+        ------------
+        opt_input : numpy.ndarray, shape(input_length, )
+            optimal input
+        """
+        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.reshape(-1, 1) - temp_1 - temp_2 - self.dist_mat
+
+        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
+
+        # constraints
+        A = [] 
+        b = []
+
+        if self.W is not None:
+            A.append(self.W)
+            b.append(self.omega.reshape(-1, 1))
+
+        if self.F is not None:
+            b_F = - np.dot(self.F1, self.history_us[-1].reshape(-1, 1)) - self.f.reshape(-1, 1)
+            A.append(self.F)
+            b.append(b_F)
+
+        A = np.array(A).reshape(-1, self.input_size * self.pre_step)
+
+        ub = np.array(b).flatten()
+
+        # make cvxpy problem formulation
+        P = 2*matrix(H)
+        q = matrix(-1 * G)
+        A = matrix(A)
+        b = matrix(ub)
+
+        # constraint
+        if self.W is not None or self.F is not None :
+            opt_result = solvers.qp(P, q, G=A, h=b)
+        
+        opt_dt_us = list(opt_result['x'])
+        
+        opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
+        
+        # save
+        self.history_us.append(opt_u)
+
+        return opt_u
+
+    def update_system_model(self, A, B, W_D):
+        """update system model
+        A : numpy.ndarray
+            system matrix
+        B : numpy.ndarray
+            input matrix
+        W_D : numpy.ndarray
+            distubance matrix in state equation
+        """
+
+        self.A = A
+        self.B = B
+        self.W_D = W_D
+        
+        self.initialize_controller()