diff --git a/mpc/__pycache__/mpc_func.cpython-36.pyc b/mpc/__pycache__/mpc_func.cpython-36.pyc
index 73049d5..4179dd6 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/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc b/mpc/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc
new file mode 100644
index 0000000..0071073
Binary files /dev/null and b/mpc/__pycache__/mpc_func_with_cvxopt.cpython-36.pyc differ
diff --git a/mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc b/mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc
new file mode 100644
index 0000000..9a16a4c
Binary files /dev/null and b/mpc/__pycache__/mpc_func_with_scipy.cpython-36.pyc differ
diff --git a/mpc/main_example.py b/mpc/main_example.py
index f594ba7..95251ad 100644
--- a/mpc/main_example.py
+++ b/mpc/main_example.py
@@ -3,7 +3,8 @@ import matplotlib.pyplot as plt
 import math
 import copy
 
-from mpc_func import MpcController
+from mpc_func_with_scipy import MpcController as MpcController_scipy
+from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
 from control import matlab
 
 class FirstOrderSystem():
@@ -86,7 +87,7 @@ class FirstOrderSystem():
 
 def main():
     dt = 0.05
-    simulation_time = 50 # in seconds
+    simulation_time = 100 # in seconds
     iteration_num = int(simulation_time / dt)
 
     # you must be care about this matrix
@@ -117,20 +118,21 @@ def main():
     Bd = sysd.B
 
     # evaluation function weight
-    Q = np.diag([1., 1., 1., 1.])
-    R = np.diag([0.1, 0.1])
+    Q = np.diag([1., 1., 10., 10.])
+    R = np.diag([0.01, 0.01])
     pre_step = 5
 
     # make controller with discreted matrix
-    controller = MpcController(Ad, Bd, Q, R, pre_step,
-                               dt_input_upper=np.array([0.5 * dt, 0.5 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
-                               input_upper=np.array([2. ,5.]), input_lower=np.array([-2., -5.]))
+    # please check the solver, if you want to use the scipy, set the MpcController_scipy
+    controller = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
+                               dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
+                               input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))
 
     controller.initialize_controller()
 
     for i in range(iteration_num):
         print("simulation time = {0}".format(i))
-        reference = np.array([[0., 0., -10., -5.] for _ in range(pre_step)]).flatten()   
+        reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten()   
         states = plant.xs
         opt_u = controller.calc_input(states, reference)
         plant.update_state(opt_u)
@@ -144,18 +146,22 @@ def main():
     v_y_fig = time_history_fig.add_subplot(414)
 
     v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 0])
+    v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
     v_x_fig.set_xlabel("time [s]")
     v_x_fig.set_ylabel("v_x")
 
     v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 1])
+    v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
     v_y_fig.set_xlabel("time [s]")
     v_y_fig.set_ylabel("v_y")
 
     x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 2])
+    x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed")
     x_fig.set_xlabel("time [s]")
     x_fig.set_ylabel("x")
 
     y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states[:, 3])
+    y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed")
     y_fig.set_xlabel("time [s]")
     y_fig.set_ylabel("y")
     time_history_fig.tight_layout()
diff --git a/mpc/mpc_func_with_cvxopt.py b/mpc/mpc_func_with_cvxopt.py
new file mode 100644
index 0000000..69fabe0
--- /dev/null
+++ b/mpc/mpc_func_with_cvxopt.py
@@ -0,0 +1,257 @@
+import numpy as np
+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
+    Q : numpy.ndarray
+        evaluation function weight
+    R : numpy.ndarray
+        evaluation function weight
+    pre_step : int
+        prediction step
+    dt_input_upper : numpy.ndarray
+        constraints of input dt
+    dt_input_lower : numpy.ndarray
+        constraints of input dt
+    input_upper : numpy.ndarray
+        constraints of input
+    input_lower : numpy.ndarray
+        constraints of input
+    history_
+    """
+    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):
+        """
+        A : numpy.ndarray
+            system matrix
+        B : numpy.ndarray
+            input matrix
+        Q : numpy.ndarray
+            evaluation function weight
+        R : numpy.ndarray
+            evaluation function weight
+        pre_step : int
+            prediction step
+        dt_input_upper : numpy.ndarray
+            constraints of input dt
+        dt_input_lower : numpy.ndarray
+            constraints of input dt
+        input_upper : numpy.ndarray
+            constraints of input
+        input_lower : numpy.ndarray
+            constraints of input
+        """
+        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 = [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
+
+        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))
+
+        # 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.array
+            the size should have (state length * 1)
+        references :
+            the size should have (state length * pre_step)
+
+        References
+        ------------
+        opt_input : numpy.ndarray
+            optimal input, size is (1, input_length)
+        """
+        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
+
+        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)
+        # b = np.array(b).reshape(-1, 1)
+        ub = np.array(b).flatten()
+        # print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
+
+        # 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 :
+            # print("consider constraint!")
+            opt_result = solvers.qp(P, q, G=A, h=b)
+        
+        # print(list(opt_result['x']))
+        opt_dt_us = list(opt_result['x'])
+        # print("current_u = {0}".format(self.history_us[-1]))
+        # print("opt_dt_u = {0}".format(np.round(opt_dt_us, 5)))
+        opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
+        # print("opt_u = {0}".format(np.round(opt_u, 5)))
+        # save
+        self.history_us.append(opt_u)
+        # a = input()
+        return opt_u
+
+
+
+
+"""
+constraint = []
+for i in range(self.pre_step * self.input_size):
+    sums = -1. * (np.dot(A[i], init_dt_us) - b[i])[0]
+    constraint.append(sums)
+"""
\ No newline at end of file
diff --git a/mpc/mpc_func.py b/mpc/mpc_func_with_scipy.py
similarity index 79%
rename from mpc/mpc_func.py
rename to mpc/mpc_func_with_scipy.py
index ae45087..f56907b 100644
--- a/mpc/mpc_func.py
+++ b/mpc/mpc_func_with_scipy.py
@@ -4,6 +4,7 @@ import math
 import copy
 
 from scipy.optimize import minimize
+from scipy.optimize import LinearConstraint
 
 class MpcController():
     """
@@ -220,7 +221,9 @@ class MpcController():
             b.append(b_F)
 
         A = np.array(A).reshape(-1, self.input_size * self.pre_step)
-        b = np.array(b).reshape(-1, 1)
+        # b = np.array(b).reshape(-1, 1)
+        ub = np.array(b).flatten()
+        # print(np.dot(self.F1, self.history_us[-1].reshape(-1, 1)))
 
         def optimized_func(dt_us):
             """
@@ -228,37 +231,26 @@ class MpcController():
             temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)])
 
             return (np.dot(temp_dt_us, np.dot(H, temp_dt_us.reshape(-1, 1))) - np.dot(G.T, temp_dt_us.reshape(-1, 1)))[0]
-                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   
-        def constraint_func(dt_us):
-            """ we consider the constraints in Ax <= b
-            however the scipy constraints is -(Ax - b) >= 0
-            so we should recalculate the A and B
-            """
-            temp_dt_us = np.array([dt_us[i] for i in range(self.input_size * self.pre_step)])
-            
-            constraint = []
-            for i in range(self.pre_step * self.input_size):
-                sums = -1. * (np.dot(A[i], temp_dt_us) - b[i])[0]
-                constraint.append(sums)
 
-            return np.array(sums)
+        # constraint
+        lb = np.array([-np.inf for _ in range(len(ub))])
+        linear_cons = LinearConstraint(A, lb, ub)
 
         init_dt_us = np.zeros(self.input_size * self.pre_step)
 
         # constraint
         if self.W is not None or self.F is not None :
-            print("consider constraint!")
-            cons = ({'type' : 'ineq', 'fun' : constraint_func})
-            opt_result = minimize(optimized_func, init_dt_us, constraints=cons)
+            # print("consider constraint!")
+            opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons])
         
         opt_dt_us = opt_result.x
-        print("opt_u = {0}".format(np.round(opt_dt_us, 5)))
+        # print("current_u = {0}".format(self.history_us[-1]))
+        # print("opt_dt_u = {0}".format(np.round(opt_dt_us, 5)))
         opt_u = opt_dt_us[:self.input_size] + self.history_us[-1]
-
+        # print("opt_u = {0}".format(np.round(opt_u, 5)))
         # save
         self.history_us.append(opt_u)
         
-
         return opt_u
 
 
diff --git a/mpc/simulator_func.py b/mpc/simulator_func.py
deleted file mode 100644
index bf38e16..0000000
--- a/mpc/simulator_func.py
+++ /dev/null
@@ -1,70 +0,0 @@
-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
-
diff --git a/mpc/test_compare_methods.py b/mpc/test_compare_methods.py
new file mode 100644
index 0000000..5ddaea4
--- /dev/null
+++ b/mpc/test_compare_methods.py
@@ -0,0 +1,211 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import copy
+
+from mpc_func_with_scipy import MpcController as MpcController_scipy
+from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
+from control import matlab
+
+class FirstOrderSystem():
+    """FirstOrderSystemWithStates
+
+    Attributes
+    -----------
+    states : float
+        system states
+    A : numpy.ndarray
+        system matrix
+    B : numpy.ndarray
+        control matrix
+    C : numpy.ndarray
+        observation matrix
+    history_state : list
+        time history of state
+    """
+    def __init__(self, A, B, C, D=None, init_states=None):
+        """
+        Parameters
+        -----------
+        A : numpy.ndarray
+            system matrix
+        B : numpy.ndarray
+            control matrix
+        C : numpy.ndarray
+            observation matrix
+        C : numpy.ndarray
+            directly matrix
+        init_state : float, optional
+            initial state of system default is None
+        history_xs : list
+            time history of system states
+        """
+
+        self.A = A
+        self.B = B
+        self.C = C
+
+        if D is not None:
+            self.D = D
+
+        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, 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]
+        """
+        temp_x = self.xs.reshape(-1, 1)
+        temp_u = u.reshape(-1, 1)
+
+        # solve Runge-Kutta
+        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()
+
+        # for oylar
+        # self.xs += k0.flatten()
+
+        # print("xs = {0}".format(self.xs))
+        # a = input()
+        # save
+        save_states = copy.deepcopy(self.xs)
+        self.history_xs.append(save_states)
+        # print(self.history_xs)
+
+def main():
+    dt = 0.05
+    simulation_time = 50 # 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.63
+    A = np.array([[-1./tau, 0., 0., 0.],
+                  [0., -1./tau, 0., 0.],
+                  [1., 0., 0., 0.], 
+                  [0., 1., 0., 0.]])
+    B = np.array([[1./tau, 0.],
+                  [0., 1./tau],
+                  [0., 0.],
+                  [0., 0.]])
+
+    C = np.eye(4)
+    D = np.zeros((4, 2))
+
+    # make simulator with coninuous matrix
+    init_xs = np.array([0., 0., 0., 0.])
+    plant_cvxopt = FirstOrderSystem(A, B, C, init_states=init_xs)
+    plant_scipy = FirstOrderSystem(A, B, C, init_states=init_xs)
+
+    # 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., 10., 10.])
+    R = np.diag([0.01, 0.01])
+    pre_step = 5
+
+    # make controller with discreted matrix
+    # please check the solver, if you want to use the scipy, set the MpcController_scipy
+    controller_cvxopt = MpcController_cvxopt(Ad, Bd, Q, R, pre_step,
+                               dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
+                               input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))
+    
+    controller_scipy = MpcController_scipy(Ad, Bd, Q, R, pre_step,
+                               dt_input_upper=np.array([0.25 * dt, 0.25 * dt]), dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
+                               input_upper=np.array([1. ,3.]), input_lower=np.array([-1., -3.]))
+
+    controller_cvxopt.initialize_controller()
+    controller_scipy.initialize_controller()
+
+    for i in range(iteration_num):
+        print("simulation time = {0}".format(i))
+        reference = np.array([[0., 0., -5., 7.5] for _ in range(pre_step)]).flatten()   
+
+        states_cvxopt = plant_cvxopt.xs
+        states_scipy = plant_scipy.xs
+
+        opt_u_cvxopt = controller_cvxopt.calc_input(states_cvxopt, reference)
+        opt_u_scipy = controller_scipy.calc_input(states_scipy, reference)
+
+        plant_cvxopt.update_state(opt_u_cvxopt)
+        plant_scipy.update_state(opt_u_scipy)
+
+    history_states_cvxopt = np.array(plant_cvxopt.history_xs)
+    history_states_scipy = np.array(plant_scipy.history_xs)
+
+    time_history_fig = plt.figure(dpi=75)
+    x_fig = time_history_fig.add_subplot(411)
+    y_fig = time_history_fig.add_subplot(412)
+    v_x_fig = time_history_fig.add_subplot(413)
+    v_y_fig = time_history_fig.add_subplot(414)
+
+    v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 0], label="cvxopt")
+    v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 0], label="scipy", linestyle="dashdot")
+    v_x_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
+    v_x_fig.set_xlabel("time [s]")
+    v_x_fig.set_ylabel("v_x")
+    v_x_fig.legend()
+
+    v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 1], label="cvxopt")
+    v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 1], label="scipy", linestyle="dashdot")
+    v_y_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
+    v_y_fig.set_xlabel("time [s]")
+    v_y_fig.set_ylabel("v_y")
+    v_y_fig.legend()
+
+    x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_cvxopt[:, 2], label="cvxopt")
+    x_fig.plot(np.arange(0, simulation_time+0.01, dt), history_states_scipy[:, 2], label="scipy", linestyle="dashdot")
+    x_fig.plot(np.arange(0, simulation_time+0.01, dt), [-5. for _ in range(iteration_num+1)], linestyle="dashed")
+    x_fig.set_xlabel("time [s]")
+    x_fig.set_ylabel("x")
+
+    y_fig.plot(np.arange(0, simulation_time+0.01, dt),  history_states_cvxopt[:, 3], label ="cvxopt")
+    y_fig.plot(np.arange(0, simulation_time+0.01, dt),  history_states_scipy[:, 3], label="scipy", linestyle="dashdot")
+    y_fig.plot(np.arange(0, simulation_time+0.01, dt), [7.5 for _ in range(iteration_num+1)], linestyle="dashed")
+    y_fig.set_xlabel("time [s]")
+    y_fig.set_ylabel("y")
+    time_history_fig.tight_layout()
+    plt.show()
+
+    history_us_cvxopt = np.array(controller_cvxopt.history_us)
+    history_us_scipy = np.array(controller_scipy.history_us)
+
+    input_history_fig = plt.figure(dpi=75)
+    u_1_fig = input_history_fig.add_subplot(211)
+    u_2_fig = input_history_fig.add_subplot(212)
+
+    u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 0], label="cvxopt")
+    u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 0], label="scipy", linestyle="dashdot")
+    u_1_fig.set_xlabel("time [s]")
+    u_1_fig.set_ylabel("u_x")
+    u_1_fig.legend()
+    
+    u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_cvxopt[:, 1], label="cvxopt")
+    u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us_scipy[:, 1], label="scipy", linestyle="dashdot")
+    u_2_fig.set_xlabel("time [s]")
+    u_2_fig.set_ylabel("u_y")
+    u_2_fig.legend()
+    input_history_fig.tight_layout()
+    plt.show()
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file