add newton method

README.md

@@ -8,6 +8,7 @@ Now you can see the examples of control theories as following
 
 - **Iterative Linear Quadratic Regulator (iLQR)**
 - **Nonlinear Model Predictive Control (NMPC) with CGMRES**
+- **Nonlinear Model Predictive Control (NMPC) with Newton method**
 - **Linear Model Predictive Control (MPC)**(as generic function such as matlab tool box)
 - **Extended Linear Model Predictive Control for vehicle model**
 

nmpc/newton/README.md

@@ -1,7 +1,8 @@
-# CGMRES method of Nonlinear Model Predictive Control
+# Newton method of Nonlinear Model Predictive Control
-This program is about Continuous gmres method for NMPC
+This program is about NMPC with newton method.
-Although usually we have to calculate the partial differential of optimal matrix, it could be really complicated.
+Usually we have to calculate the partial differential of optimal matrix.
-By using CGMRES, we can pass the calculating step and get the optimal input quickly.
+In this program, in stead of using any paticular methods to calculate the partial differential of optimal matrix, I used numerical differentiation.
+Therefore, I believe that it easy to understand and extend your model.
 
 # Problem Formulation
 
@@ -13,39 +14,26 @@ By using CGMRES, we can pass the calculating step and get the optimal input quic
 
 - evaluation function
 
+To consider the constraints of input u, I introduced dummy input.
+
 <a href="https://www.codecogs.com/eqnedit.php?latex=J&space;=&space;\frac{1}{2}(x_1^2(t&plus;T)&plus;x_2^2(t&plus;T))&plus;\int_{t}^{t&plus;T}\frac{1}{2}(x_1^2&plus;x_2^2&plus;u^2)-0.01vd\tau" target="_blank"><img src="https://latex.codecogs.com/gif.latex?J&space;=&space;\frac{1}{2}(x_1^2(t&plus;T)&plus;x_2^2(t&plus;T))&plus;\int_{t}^{t&plus;T}\frac{1}{2}(x_1^2&plus;x_2^2&plus;u^2)-0.01vd\tau" title="J = \frac{1}{2}(x_1^2(t+T)+x_2^2(t+T))+\int_{t}^{t+T}\frac{1}{2}(x_1^2+x_2^2+u^2)-0.01vd\tau" /></a>
 
 
 - **two wheeled model**
 
-- model
+coming soon !
-
-<a href="https://www.codecogs.com/eqnedit.php?latex=\frac{d}{dt}&space;\boldsymbol{X}=&space;\frac{d}{dt}&space;\begin{bmatrix}&space;x&space;\\&space;y&space;\\&space;\theta&space;\end{bmatrix}&space;=&space;\begin{bmatrix}&space;\cos(\theta)&space;&&space;0&space;\\&space;\sin(\theta)&space;&&space;0&space;\\&space;0&space;&&space;1&space;\\&space;\end{bmatrix}&space;\begin{bmatrix}&space;u_v&space;\\&space;u_\omega&space;\\&space;\end{bmatrix}&space;=&space;\boldsymbol{B}\boldsymbol{U}" target="_blank"><img src="https://latex.codecogs.com/gif.latex?\frac{d}{dt}&space;\boldsymbol{X}=&space;\frac{d}{dt}&space;\begin{bmatrix}&space;x&space;\\&space;y&space;\\&space;\theta&space;\end{bmatrix}&space;=&space;\begin{bmatrix}&space;\cos(\theta)&space;&&space;0&space;\\&space;\sin(\theta)&space;&&space;0&space;\\&space;0&space;&&space;1&space;\\&space;\end{bmatrix}&space;\begin{bmatrix}&space;u_v&space;\\&space;u_\omega&space;\\&space;\end{bmatrix}&space;=&space;\boldsymbol{B}\boldsymbol{U}" title="\frac{d}{dt} \boldsymbol{X}= \frac{d}{dt} \begin{bmatrix} x \\ y \\ \theta \end{bmatrix} = \begin{bmatrix} \cos(\theta) & 0 \\ \sin(\theta) & 0 \\ 0 & 1 \\ \end{bmatrix} \begin{bmatrix} u_v \\ u_\omega \\ \end{bmatrix} = \boldsymbol{B}\boldsymbol{U}" /></a>
-
-- evaluation function
-
-<a href="https://www.codecogs.com/eqnedit.php?latex=J&space;=&space;\boldsymbol{X}(t_0&plus;T)^2&space;&plus;&space;\int_{t_0}^{t_0&space;&plus;&space;T}&space;\boldsymbol{U}(t)^2&space;-&space;0.01&space;dummy_{u_v}&space;-&space;dummy_{u_\omega}&space;dt" target="_blank"><img src="https://latex.codecogs.com/gif.latex?J&space;=&space;\boldsymbol{X}(t_0&plus;T)^2&space;&plus;&space;\int_{t_0}^{t_0&space;&plus;&space;T}&space;\boldsymbol{U}(t)^2&space;-&space;0.01&space;dummy_{u_v}&space;-&space;dummy_{u_\omega}&space;dt" title="J = \boldsymbol{X}(t_0+T)^2 + \int_{t_0}^{t_0 + T} \boldsymbol{U}(t)^2 - 0.01 dummy_{u_v} - dummy_{u_\omega} dt" /></a>
-
-
-if you want to see more detail about this methods, you should go https://qiita.com/MENDY/items/4108190a579395053924.
-However, it is written in Japanese
 
 # Expected Results
 
 - example
 
-![Figure_1.png](https://qiita-image-store.s3.amazonaws.com/0/261584/3347fb3c-3fce-63fe-36d5-8a7bb053531a.png)
+![Figure_1.png]()
+
+you can confirm that the my method could consider the constraints of input.
 
 - two wheeled model
 
-- trajectory
+coming soon !
-
-![image.png](https://qiita-image-store.s3.amazonaws.com/0/261584/8e39150d-24ed-af13-51f0-0ca97cb5f5ec.png)
-
-- time history
-
-![Figure_1.png](https://qiita-image-store.s3.amazonaws.com/0/261584/e67794f3-e8ef-5162-ea84-eb6adefd4241.png)
-![Figure_2.png](https://qiita-image-store.s3.amazonaws.com/0/261584/d74fa06d-2eae-5aea-4a33-6c63e284341e.png)
 
 # Usage
 
@@ -57,9 +45,7 @@ $ python main_example.py
 
 - for two wheeled
 
-```
+coming soon !
-$ python main_two_wheeled.py
-```
 
 # Requirement
 

nmpc/newton/main_example.py

@@ -407,19 +407,13 @@ class NMPCControllerWithNewton():
     """
     Attributes
     ------------
-    zeta : float
-        gain of optimal answer stability
-    tf : float
-        predict time
-    alpha : float
-        gain of predict time
     N : int
         predicte step, discritize value
     threshold : float
-        cgmres's threshold value
+        newton's threshold value
-    input_num : int
+    NUM_INPUT : int
         system input length, this should include dummy u and constraint variables
-    max_iteration : int
+    MAX_ITERATION : int
         decide by the solved matrix size
     simulator : NMPCSimulatorSystem class
     us : list of float
@@ -444,11 +438,10 @@ class NMPCControllerWithNewton():
         None
         """
         # parameters
-        self.tf = 1. # 最終時間
+        self.N = 10 # time step
-        self.N = 10 # 分割数
+        self.threshold = 0.0001 # break
-        self.threshold = 0.0001 # break値
 
-        self.NUM_INPUT = 3 # dummy, 制約条件に対するrawにも合わせた入力の数
+        self.NUM_INPUT = 3 # u with dummy,  and 制約条件に対するrawにも合わせた入力の数
         self.Jacobian_size = self.NUM_INPUT * self.N
 
         # newton parameters
@@ -492,11 +485,6 @@ class NMPCControllerWithNewton():
         
         # Newton method
         for i in range(self.MAX_ITERATION):
-            # check            
-            # print("all_us = {}".format(all_us))
-            # print("newton iteration in {}".format(i))
-            # input()
-
             # calc all state
             x_1s, x_2s, lam_1s, lam_2s = self.simulator.calc_predict_and_adjoint_state(x_1, x_2, self.us, self.N, dt)
             
@@ -504,8 +492,6 @@ class NMPCControllerWithNewton():
             F_hat = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, all_us, self.N, dt)
 
             # judge
-            # print("F_hat = {}".format(F_hat))
-            # print(np.linalg.norm(F_hat))
             if np.linalg.norm(F_hat) < self.threshold:
                 # print("break!!")
                 break
@@ -531,9 +517,6 @@ class NMPCControllerWithNewton():
 
         F = self._calc_f(x_1s, x_2s, lam_1s, lam_2s, all_us, self.N, dt)
 
-        # print("check val of F = {0}".format(np.linalg.norm(F)))
-        # input()
-
         # for save
         self.history_f.append(np.linalg.norm(F))
         self.history_u.append(self.us[0])
@@ -642,6 +625,3 @@ def main():
 
 if __name__ == "__main__":
     main()
-
-
-