diff --git a/LICENSE b/LICENSE
new file mode 100755
index 0000000..9a5e36b
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2018 Shunichi Sekiguchi
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
\ No newline at end of file
diff --git a/README.md b/README.md
index 44b53c6..7cc86fd 100644
--- a/README.md
+++ b/README.md
@@ -1,2 +1,17 @@
+[![MIT licensed](https://img.shields.io/badge/license-MIT-blue.svg)](LICENSE)
+
 # nonlinear_control
 Implementing the nonlinear model predictive control, sliding mode control
+
+# Usage
+you can see the usage in each folder
+
+# Requirement
+You should install following software
+
+- python(3.5>)
+- numpy
+- matplotlib
+
+# License
+MIT
diff --git a/nmpc/cgmres/main_example.py b/nmpc/cgmres/main_example.py
index c5a13ab..696742c 100644
--- a/nmpc/cgmres/main_example.py
+++ b/nmpc/cgmres/main_example.py
@@ -519,8 +519,8 @@ class NMPCController_with_CGMRES():
             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]
+                ddummy_u_new = ddummy_u + update_value[1::3]
+                draw_new = draw + update_value[2::3]
                 break
 
             ys_pre = ys
diff --git a/nmpc/cgmres/main_two_wheeled.py b/nmpc/cgmres/main_two_wheeled.py
index c5a13ab..1ecd903 100644
--- a/nmpc/cgmres/main_two_wheeled.py
+++ b/nmpc/cgmres/main_two_wheeled.py
@@ -2,7 +2,7 @@ import numpy as np
 import matplotlib.pyplot as plt
 import math
 
-class SampleSystem():
+class TwoWheeledSystem():
     """SampleSystem, this is the simulator
     Attributes
     -----------
@@ -15,7 +15,7 @@ class SampleSystem():
     history_x_2 : list
         time history of system state 2 (x_2)
     """
-    def __init__(self, init_x_1=0., init_x_2=0.):
+    def __init__(self, init_x_1=0., init_x_2=0., init_x_3=0.):
         """
         Palameters
         -----------
@@ -23,73 +23,101 @@ class SampleSystem():
             initial value of x_1, default is 0.
         init_x_2 : float, optional
             initial value of x_2, default is 0.
+        init_x_3 : float, optional
+            initial value of x_3, default is 0.
         """
         self.x_1 = init_x_1
         self.x_2 = init_x_2
+        self.x_3 = init_x_3
         self.history_x_1 = [init_x_1]
         self.history_x_2 = [init_x_2]
+        self.history_x_3 = [init_x_3]
 
-    def update_state(self, u, dt=0.01):
+    def update_state(self, u_1, u_2, dt=0.01):
         """
         Palameters
         ------------
-        u : float
-            input of system in some cases this means the reference
+        u_1 : float
+            input of system in some cases this means the reference, u_velocity
+        u_2 : float
+            input of system in some cases this means the reference, u_omega
         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)]
+        k0 = [0.0 for _ in range(3)]
+        k1 = [0.0 for _ in range(3)]
+        k2 = [0.0 for _ in range(3)]
+        k3 = [0.0 for _ in range(3)]
 
-        functions = [self._func_x_1, self._func_x_2]
+        functions = [self._func_x_1, self._func_x_2, self._func_x_3]
 
         # solve Runge-Kutta
         for i, func in enumerate(functions):
-            k0[i] = dt * func(self.x_1, self.x_2, u)
+            k0[i] = dt * func(self.x_1, self.x_2, self.x_3, u_1, u_2)
         
         for i, func in enumerate(functions):
-            k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., u)
+            k1[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2)
         
         for i, func in enumerate(functions):
-            k2[i] = dt * func(self.x_1 + k1[0]/2., self.x_2 + k1[1]/2., u)
+            k2[i] = dt * func(self.x_1 + k0[0]/2., self.x_2 + k0[1]/2., self.x_3 + k0[2]/2., u_1, u_2)
         
         for i, func in enumerate(functions):
-            k3[i] =  dt * func(self.x_1 + k2[0], self.x_2 + k2[1], u)
+            k3[i] =  dt * func(self.x_1 + k2[0], self.x_2 + k2[1], self.x_3 + k2[2], u_1, u_2)
         
         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.
+        self.x_3 += (k0[2] + 2. * k1[2] + 2. * k2[2] + k3[2]) / 6.
     
         # save
         self.history_x_1.append(self.x_1)
-        self.history_x_2.append(self.x_2)      
+        self.history_x_2.append(self.x_2)
+        self.history_x_3.append(self.x_3)      
 
-    def _func_x_1(self, y_1, y_2, u):
+    def _func_x_1(self, y_1, y_2, y_3, u_1, u_2):
         """
         Parameters
         ------------
         y_1 : float
         y_2 : float
-        u : float
+        y_3 : float
+        u_1 : float
+            system input
+        u_2 : float
             system input
         """
-        y_dot = y_2
+        y_dot = math.cos(y_3) * u_1
         return y_dot
     
-    def _func_x_2(self, y_1, y_2, u):
+    def _func_x_2(self, y_1, y_2, y_3, u_1, u_2):
         """
         Parameters
         ------------
         y_1 : float
         y_2 : float
-        u : float
+        y_3 : float
+        u_1 : float
+            system input
+        u_2 : float
             system input
         """
-        y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u
+        y_dot = math.sin(y_3) * u_1
+        return y_dot
+    
+    def _func_x_3(self, y_1, y_2, y_3, u_1, u_2):
+        """
+        Parameters
+        ------------
+        y_1 : float
+        y_2 : float
+        y_3 : float
+        u_1 : float
+            system input
+        u_2 : float
+            system input
+        """
+        y_dot = u_2
         return y_dot
-
 
 class NMPCSimulatorSystem():
     """SimulatorSystem for nmpc, this is the simulator of nmpc
@@ -107,7 +135,7 @@ class NMPCSimulatorSystem():
         """
         pass
 
-    def calc_predict_and_adjoint_state(self, x_1, x_2, us, N, dt):
+    def calc_predict_and_adjoint_state(self, x_1, x_2, x_3, u_1s, u_2s, N, dt):
         """main
         Parameters
         ------------
@@ -115,7 +143,11 @@ class NMPCSimulatorSystem():
             current state
         x_2 : float
             current state
-        us : list of float
+        x_3 : float
+            current state
+        u_1s : list of float
+            estimated optimal input Us for N steps
+        u_2s : list of float
             estimated optimal input Us for N steps
         N : int
             predict step
@@ -128,18 +160,21 @@ class NMPCSimulatorSystem():
             predicted x_1s for N steps
         x_2s : list of float
             predicted x_2s for N steps
+        x_3s : list of float
+            predicted x_3s 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
+        lam_3s : list of float
+            adjoint state of x_3s, lam_3s for N steps
         """
+        x_1s, x_2s, x_3s = self._calc_predict_states(x_1, x_2, x_3, u_1s, u_2s, N, dt) # by usin state equation
+        lam_1s, lam_2s, lam_3s = self._calc_adjoint_states(x_1s, x_2s, x_3s, u_1s, u_2s, N, dt) # by using adjoint equation
 
-        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, x_3s, lam_1s, lam_2s, lam_3s
 
-        return x_1s, x_2s, lam_1s, lam_2s
-
-    def _calc_predict_states(self, x_1, x_2, us, N, dt):
+    def _calc_predict_states(self, x_1, x_2, x_3, u_1s, u_2s, N, dt):
         """ 
         Parameters
         ------------
@@ -147,7 +182,11 @@ class NMPCSimulatorSystem():
             current state
         x_2 : float
             current state
-        us : list of float
+        x_3 : float
+            current state
+        u_1s : list of float
+            estimated optimal input Us for N steps
+        u_2s : list of float
             estimated optimal input Us for N steps
         N : int
             predict step
@@ -160,19 +199,23 @@ class NMPCSimulatorSystem():
             predicted x_1s for N steps
         x_2s : list of float
             predicted x_2s for N steps
+        x_3s : list of float
+            predicted x_3s for N steps
         """
         # initial state
         x_1s = [x_1]
         x_2s = [x_2]
+        x_3s = [x_3]
 
         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)
+            temp_x_1, temp_x_2, temp_x_3 = self._predict_state_with_oylar(x_1s[i], x_2s[i], x_3s[i], u_1s[i], u_2s[i], dt)
             x_1s.append(temp_x_1)
             x_2s.append(temp_x_2)
+            x_3s.append(temp_x_3)
 
-        return x_1s, x_2s
+        return x_1s, x_2s, x_3s
 
-    def _calc_adjoint_states(self, x_1s, x_2s, us, N, dt):
+    def _calc_adjoint_states(self, x_1s, x_2s, x_3s, u_1s, u_2s, N, dt):
         """
         Parameters
         ------------
@@ -180,7 +223,11 @@ class NMPCSimulatorSystem():
             predicted x_1s for N steps
         x_2s : list of float
             predicted x_2s for N steps
-        us : list of float
+        x_3s : list of float
+            predicted x_3s for N steps
+        u_1s : list of float
+            estimated optimal input Us for N steps
+        u_2s : list of float
             estimated optimal input Us for N steps
         N : int
             predict step
@@ -193,25 +240,29 @@ class NMPCSimulatorSystem():
             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
+        lam_3s : 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]]
+        lam_3s = [x_3s[-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)
+            temp_lam_1, temp_lam_2, temp_lam_3 = self._adjoint_state_with_oylar(x_1s[i], x_2s[i], x_3s[i], lam_1s[0] ,lam_2s[0], lam_3s[0], u_1s[i], u_2s[i], dt)
             lam_1s.insert(0, temp_lam_1)
             lam_2s.insert(0, temp_lam_2)
+            lam_3s.insert(0, temp_lam_3)
 
-        return lam_1s, lam_2s
+        return lam_1s, lam_2s, lam_3s
 
     def final_state_func(self):
         """this func usually need
         """
         pass
 
-    def _predict_state_with_oylar(self, x_1, x_2, u, dt):
+    def _predict_state_with_oylar(self, x_1, x_2, x_3, u_1, u_2, dt):
         """in this case this function is the same as simulator
         Parameters
         ------------
@@ -219,7 +270,11 @@ class NMPCSimulatorSystem():
             system state
         x_2 : float
             system state
-        u : float
+        x_3 : float
+            system state
+        u_1 : float
+            system input
+        u_2 : float
             system input
         dt : float in seconds
             sampling time
@@ -229,56 +284,68 @@ class NMPCSimulatorSystem():
             next state, x_1 calculated by using state equation
         next_x_2 : float
             next state, x_2 calculated by using state equation
+        next_x_3 : float
+            next state, x_3 calculated by using state equation
         """
-        k0 = [0. for _ in range(2)]
+        k0 = [0. for _ in range(3)]
 
-        functions = [self.func_x_1, self.func_x_2]
+        functions = [self.func_x_1, self.func_x_2, self.func_x_3]
 
         for i, func in enumerate(functions):
-            k0[i] = dt * func(x_1, x_2, u)
+            k0[i] = dt * func(x_1, x_2, x_3, u_1, u_2)
                 
         next_x_1 = x_1 + k0[0]
         next_x_2 = x_2 + k0[1]
+        next_x_3 = x_3 + k0[2]
 
-        return next_x_1, next_x_2
+        return next_x_1, next_x_2, next_x_3
 
-    def func_x_1(self, y_1, y_2, u):
-        """calculating \dot{x_1}
+    def func_x_1(self, y_1, y_2, y_3, u_1, u_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_1}
+        y_3 : float
+        u_1 : float
+            system input
+        u_2 : float
+            system input
         """
-        y_dot = y_2
+        y_dot = math.cos(y_3) * u_1
         return y_dot
     
-    def func_x_2(self, y_1, y_2, u):
-        """calculating \dot{x_2}
+    def func_x_2(self, y_1, y_2, y_3, u_1, u_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_3 : float
+        u_1 : float
+            system input
+        u_2 : float
+            system input
         """
-        y_dot = (1. - y_1**2 - y_2**2) * y_2 - y_1 + u
+        y_dot = math.sin(y_3) * u_1
+        return y_dot
+    
+    def func_x_3(self, y_1, y_2, y_3, u_1, u_2):
+        """
+        Parameters
+        ------------
+        y_1 : float
+        y_2 : float
+        y_3 : float
+        u_1 : float
+            system input
+        u_2 : float
+            system input
+        """
+        y_dot = u_2
         return y_dot
 
-    def _adjoint_state_with_oylar(self, x_1, x_2, lam_1, lam_2, u, dt):
+    def _adjoint_state_with_oylar(self, x_1, x_2, x_3, lam_1, lam_2, lam_3, u_1, u_2, dt):
         """
         Parameters
         ------------
@@ -286,11 +353,17 @@ class NMPCSimulatorSystem():
             system state
         x_2 : float
             system state
+        x_3 : float
+            system state
         lam_1 : float
             adjoint state
         lam_2 : float
             adjoint state
-        u : float
+        lam_3 : float
+            adjoint state
+        u_1 : float
+            system input
+        u_2 : float
             system input
         dt : float in seconds
             sampling time
@@ -300,20 +373,23 @@ class NMPCSimulatorSystem():
             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
+        pre_lam_3 : float
+            pre, 1 step before lam_3 calculated by using adjoint equation
         """
-        k0 = [0. for _ in range(2)]
+        k0 = [0. for _ in range(3)]
 
-        functions = [self._func_lam_1, self._func_lam_2]
+        functions = [self._func_lam_1, self._func_lam_2, self._func_lam_3]
 
         for i, func in enumerate(functions):
-            k0[i] = dt * func(x_1, x_2, lam_1, lam_2, u)
+            k0[i] = dt * func(x_1, x_2, x_3, lam_1, lam_2, lam_3, u_1, u_2)
                 
         pre_lam_1 = lam_1 + k0[0]
         pre_lam_2 = lam_2 + k0[1]
+        pre_lam_3 = lam_3 + k0[2]
 
-        return pre_lam_1, pre_lam_2
+        return pre_lam_1, pre_lam_2, pre_lam_3
 
-    def _func_lam_1(self, y_1, y_2, y_3, y_4, u):
+    def _func_lam_1(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2):
         """calculating -\dot{lam_1}
         Parameters
         ------------
@@ -322,20 +398,26 @@ class NMPCSimulatorSystem():
         y_2 : float
             means x_2
         y_3 : float
-            means lam_1
+            means x_3
         y_4 : float
+            means lam_1
+        y_5 : float
             means lam_2
-        u : float
+        y_6 : float
+            means lam_3
+        u_1 : float
+            means system input
+        u_2 : float
             means system input
         Returns
         ---------
         y_dot : float
             means -\dot{lam_1}
         """
-        y_dot = y_1 - (2. * y_1 * y_2 + 1.) * y_4
+        y_dot = 0.
         return y_dot
 
-    def _func_lam_2(self, y_1, y_2, y_3, y_4, u):
+    def _func_lam_2(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2):
         """calculating -\dot{lam_2}
         Parameters
         ------------
@@ -344,17 +426,51 @@ class NMPCSimulatorSystem():
         y_2 : float
             means x_2
         y_3 : float
-            means lam_1
+            means x_3
         y_4 : float
+            means lam_1
+        y_5 : float
             means lam_2
-        u : float
+        y_6 : float
+            means lam_3
+        u_1 : float
+            means system input
+        u_2 : 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
+        y_dot = 0.
+        return y_dot
+
+    def _func_lam_3(self, y_1, y_2, y_3, y_4, y_5, y_6, u_1, u_2):
+        """calculating -\dot{lam_3}
+        Parameters
+        ------------
+        y_1 : float
+            means x_1
+        y_2 : float
+            means x_2
+        y_3 : float
+            means x_3
+        y_4 : float
+            means lam_1
+        y_5 : float
+            means lam_2
+        y_6 : float
+            means lam_3
+        u_1 : float
+            means system input
+        u_2 : float
+            means system input
+        Returns
+        ---------
+        y_dot : float
+            means -\dot{lam_3}
+        """
+        y_dot = - y_4 * math.sin(y_3) * u_1 + y_5 * math.cos(y_3) * u_1 
         return y_dot
 
 class NMPCController_with_CGMRES():
@@ -378,18 +494,30 @@ class NMPCController_with_CGMRES():
     max_iteration : int
         decide by the solved matrix size
     simulator : NMPCSimulatorSystem class
-    us : list of float
+    u_1s : list of float
         estimated optimal system input
-    dummy_us : list of float
+    u_2s : list of float
+        estimated optimal system input
+    dummy_u_1s : list of float
         estimated dummy input
-    raws : list of float
+    dummy_u_2s : list of float
+        estimated dummy input
+    raw_1s : list of float
         estimated constraint variable
-    history_u : list of float
+    raw_2s : list of float
+        estimated constraint variable
+    history_u_1 : 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_u_2 : list of float
+        time history of actual system input
+    history_dummy_u_1 : list of float
+        time history of actual dummy u_1
+    history_dummy_u_2 : list of float
+        time history of actual dummy u_2
+    history_raw_1 : list of float
+        time history of actual raw_1
+    history_raw_2 : list of float
+        time history of actual raw_2
     history_f : list of float
         time history of error of optimal
     """
@@ -407,24 +535,30 @@ class NMPCController_with_CGMRES():
         self.N = 10 # 分割数
         self.threshold = 0.001 # break値
 
-        self.input_num = 3 # dummy, 制約条件に対するuにも合わせた入力の数
+        self.input_num = 6 # 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
+        self.u_1s = np.ones(self.N) * 1.
+        self.u_2s = np.ones(self.N) * 0.1
+        self.dummy_u_1s = np.ones(self.N) * 0.1
+        self.dummy_u_2s = np.ones(self.N) * 2.5
+        self.raw_1s = np.ones(self.N) * 0.8
+        self.raw_2s = np.ones(self.N) * 0.8
 
         # for fig
-        self.history_u = []
-        self.history_dummy_u = []
-        self.history_raw = []
+        self.history_u_1 = []
+        self.history_u_2 = []
+        self.history_dummy_u_1 = []
+        self.history_dummy_u_2 = []
+        self.history_raw_1 = []
+        self.history_raw_2 = []
         self.history_f = []
 
-    def calc_input(self, x_1, x_2, time):
+    def calc_input(self, x_1, x_2, x_3, time):
         """
         Parameters
         ------------
@@ -432,45 +566,54 @@ class NMPCController_with_CGMRES():
             current state
         x_2 : float
             current state
+        x_3 : float
+            current state
         time : float in seconds
             now time
         Returns
         --------
-        us : list of float
+        u_1s : list of float
+            estimated optimal system input
+        u_2s : 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])
+        x_1_dot = self.simulator.func_x_1(x_1, x_2,  x_3, self.u_1s[0], self.u_2s[0])
+        x_2_dot = self.simulator.func_x_2(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0])
+        x_3_dot = self.simulator.func_x_3(x_1, x_2, x_3, self.u_1s[0], self.u_2s[0])
 
         dx_1 = x_1_dot * self.ht
         dx_2 = x_2_dot * self.ht
+        dx_3 = x_3_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)
+        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2,  x_3 + dx_3, self.u_1s, self.u_2s, 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)
+        Fxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s,  self.dummy_u_2s,
+                            self.raw_1s, self.raw_2s, 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)
+        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, x_3, self.u_1s, self.u_2s, 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)
+        F = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s,  self.dummy_u_2s,
+                            self.raw_1s, self.raw_2s, 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
+        du_1 = self.u_1s * self.ht
+        du_2 = self.u_2s * self.ht
+        ddummy_u_1 = self.dummy_u_1s * self.ht
+        ddummy_u_2 = self.dummy_u_2s * self.ht
+        draw_1 = self.raw_1s * self.ht
+        draw_2 = self.raw_2s * 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)
+        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s + du_1, self.u_2s + du_2, 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)
+        Fuxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s + du_1, self.u_2s + du_2, self.dummy_u_1s + ddummy_u_1,  self.dummy_u_2s + ddummy_u_2, 
+                           self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt)
 
         left = ((Fuxt - Fxt) / self.ht)
 
@@ -488,14 +631,17 @@ class NMPCController_with_CGMRES():
         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
+            du_1 = vs[::self.input_num, i] * self.ht
+            du_2 = vs[1::self.input_num, i] * self.ht
+            ddummy_u_1 = vs[2::self.input_num, i] * self.ht
+            ddummy_u_2 = vs[3::self.input_num, i] * self.ht            
+            draw_1 = vs[4::self.input_num, i] * self.ht
+            draw_2 = vs[5::self.input_num, 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)
+            x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1 + dx_1, x_2 + dx_2, x_3 + dx_3, self.u_1s + du_1, self.u_2s + du_2, 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)
+            Fuxt = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s + du_1, self.u_2s + du_2, self.dummy_u_1s + ddummy_u_1,  self.dummy_u_2s + ddummy_u_2, 
+                           self.raw_1s + draw_1, self.raw_2s + draw_2, self.N, dt)
 
             Av = (( Fuxt - Fxt) / self.ht)
 
@@ -518,34 +664,43 @@ class NMPCController_with_CGMRES():
 
             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]
+                du_1_new = du_1 + update_value[::self.input_num]
+                du_2_new = du_2 + update_value[1::self.input_num]
+                ddummy_u_1_new = ddummy_u_1 + update_value[2::self.input_num]
+                ddummy_u_2_new = ddummy_u_2 + update_value[3::self.input_num]
+                draw_1_new = draw_1 + update_value[4::self.input_num]
+                draw_2_new = draw_2 + update_value[5::self.input_num]
                 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
+        self.u_1s += du_1_new * self.ht
+        self.u_2s += du_2_new * self.ht
+        self.dummy_u_1s += ddummy_u_1_new * self.ht
+        self.dummy_u_2s += ddummy_u_2_new * self.ht
+        self.raw_1s += draw_1_new * self.ht
+        self.raw_2s += draw_2_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)
+        x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, x_3, self.u_1s, self.u_2s, 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)
+        F = self._calc_f(x_1s, x_2s, x_3s, lam_1s, lam_2s, lam_3s, self.u_1s, self.u_2s, self.dummy_u_1s,  self.dummy_u_2s,
+                         self.raw_1s, self.raw_2s, 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])
+        self.history_u_1.append(self.u_1s[0])
+        self.history_u_2.append(self.u_2s[0])
+        self.history_dummy_u_1.append(self.dummy_u_1s[0])
+        self.history_dummy_u_2.append(self.dummy_u_2s[0])
+        self.history_raw_1.append(self.raw_1s[0])
+        self.history_raw_2.append(self.raw_2s[0])
 
-        return self.us
+        return self.u_1s, self.u_2s
 
-    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, x_3s, lam_1s, lam_2s, lam_3s, u_1s, u_2s, dummy_u_1s, dummy_u_2s, raw_1s, raw_2s, N, dt):
         """
         Parameters
         ------------
@@ -553,15 +708,25 @@ class NMPCController_with_CGMRES():
             predicted x_1s for N steps
         x_2s : list of float
             predicted x_2s for N steps
+        x_3s : list of float
+            predicted x_3s 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
+        lam_3s : list of float
+            adjoint state of x_2s, lam_3s for N steps
+        u_1s : list of float
             estimated optimal system input
-        dummy_us : list of float
+        u_2s : list of float
+            estimated optimal system input
+        dummy_u_1s : list of float
             estimated dummy input
-        raws : list of float
+        dummy_u_2s : list of float
+            estimated dummy input
+        raw_1s : list of float
+            estimated constraint variable
+        raw_2s : list of float
             estimated constraint variable
         N : int
             predict time step
@@ -571,20 +736,58 @@ class NMPCController_with_CGMRES():
         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)
+            F.append(u_1s[i] + lam_1s[i] * math.cos(x_3s[i]) + lam_2s[i] * math.sin(x_3s[i]) + 2 * raw_1s[i] * u_1s[i])
+            F.append(u_2s[i] + lam_3s[i] + 2 * raw_2s[i] * u_2s[i])
+            F.append(-0.01 + 2. * raw_1s[i] * dummy_u_1s[i])
+            F.append(-0.01 + 2. * raw_2s[i] * dummy_u_2s[i])
+            F.append(u_1s[i]**2 + dummy_u_1s[i]**2 - 1.**2)
+            F.append(u_2s[i]**2 + dummy_u_2s[i]**2 - 1.5**2)
         
         return np.array(F)
 
+def circle_make_with_angles(center_x, center_y, radius, angle):
+    '''
+    Create circle matrix with angle line matrix
+    
+    Parameters
+    -------
+    center_x : float
+        the center x position of the circle
+    center_y : float
+        the center y position of the circle
+    radius : float
+    angle : float [rad]
+    
+    Returns
+    -------
+    circle xs : numpy.ndarray
+    circle ys : numpy.ndarray
+    angle line xs : numpy.ndarray
+    angle line ys : numpy.ndarray
+    '''
+
+    point_num = 100 # 分解能
+
+    circle_xs = []
+    circle_ys = []
+
+    for i in range(point_num + 1):
+        circle_xs.append(center_x + radius * math.cos(i*2*math.pi/point_num))
+        circle_ys.append(center_y + radius * math.sin(i*2*math.pi/point_num))
+
+    angle_line_xs = [center_x, center_x + math.cos(angle) * radius]
+    angle_line_ys = [center_y, center_y + math.sin(angle) * radius]
+
+    return np.array(circle_xs), np.array(circle_ys), np.array(angle_line_xs), np.array(angle_line_ys)
+
 def main():
     # simulation time
     dt = 0.01
-    iteration_time = 20.
+    iteration_time = 15.
     iteration_num = int(iteration_time/dt)
 
     # plant
-    plant_system = SampleSystem(init_x_1=2., init_x_2=0.)
+    plant_system = TwoWheeledSystem(init_x_1=-4.5, init_x_2=1.5, init_x_3=0.25)
 
     # controller
     controller = NMPCController_with_CGMRES()
@@ -594,20 +797,35 @@ def main():
         time = float(i) * dt
         x_1 = plant_system.x_1
         x_2 = plant_system.x_2
+        x_3 = plant_system.x_3
         # make input
-        us = controller.calc_input(x_1, x_2, time)
+        u_1s, u_2s = controller.calc_input(x_1, x_2, x_3, time)
         # update state
-        plant_system.update_state(us[0])
+        plant_system.update_state(u_1s[0], u_2s[0])
     
     # figure
-    fig = plt.figure()
+    # time history
+    fig_p = plt.figure()
+    fig_u = plt.figure()
+    fig_f = 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)
+    # traj
+    fig_t = plt.figure()
+    fig_traj = fig_t.add_subplot(111)
+    fig_traj.set_aspect('equal')
+
+    x_1_fig = fig_p.add_subplot(311)
+    x_2_fig = fig_p.add_subplot(312)
+    x_3_fig = fig_p.add_subplot(313)
+
+    u_1_fig = fig_u.add_subplot(411)
+    u_2_fig = fig_u.add_subplot(412)
+    dummy_1_fig = fig_u.add_subplot(413)
+    dummy_2_fig = fig_u.add_subplot(414)
+
+    raw_1_fig = fig_f.add_subplot(311)
+    raw_2_fig = fig_f.add_subplot(312)
+    f_fig = fig_f.add_subplot(313)
 
     x_1_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_1)
     x_1_fig.set_xlabel("time [s]")
@@ -616,24 +834,54 @@ def main():
     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")
+
+    x_3_fig.plot(np.arange(iteration_num)*dt, plant_system.history_x_3)
+    x_3_fig.set_xlabel("time [s]")
+    x_3_fig.set_ylabel("x_3")
     
-    u_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u)
-    u_fig.set_xlabel("time [s]")
-    u_fig.set_ylabel("u")
+    u_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u_1)
+    u_1_fig.set_xlabel("time [s]")
+    u_1_fig.set_ylabel("u_v")
 
-    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")
+    u_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_u_2)
+    u_2_fig.set_xlabel("time [s]")
+    u_2_fig.set_ylabel("u_omega")
 
-    raw_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw)
-    raw_fig.set_xlabel("time [s]")
-    raw_fig.set_ylabel("raw")
+    dummy_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u_1)
+    dummy_1_fig.set_xlabel("time [s]")
+    dummy_1_fig.set_ylabel("dummy u_1")
+
+    dummy_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_dummy_u_2)
+    dummy_2_fig.set_xlabel("time [s]")
+    dummy_2_fig.set_ylabel("dummy u_2")
+
+    raw_1_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw_1)
+    raw_1_fig.set_xlabel("time [s]")
+    raw_1_fig.set_ylabel("raw_1")
+
+    raw_2_fig.plot(np.arange(iteration_num - 1)*dt, controller.history_raw_2)
+    raw_2_fig.set_xlabel("time [s]")
+    raw_2_fig.set_ylabel("raw_2")
 
     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()
+    fig_traj.plot(plant_system.history_x_1, plant_system.history_x_2, color="b", linestyle="dashed")
+    fig_traj.set_xlabel("x [m]")
+    fig_traj.set_ylabel("y [m]")
+
+    write_obj_num = 5
+    count_num = int(iteration_num / write_obj_num)
+
+    for i in np.arange(0, iteration_num, count_num):
+        obj_xs, obj_ys, obj_line_xs, obj_line_ys = circle_make_with_angles(plant_system.history_x_1[i], plant_system.history_x_2[i], 0.5, plant_system.history_x_3[i])
+        fig_traj.plot(obj_xs, obj_ys, color="k")
+        fig_traj.plot(obj_line_xs, obj_line_ys, color="k")
+
+    fig_p.tight_layout()
+    fig_u.tight_layout()
+    fig_f.tight_layout()
 
     plt.show()