diff --git a/mpc/basic_IOC/README.md b/mpc/basic_IOC/README.md
new file mode 100644
index 0000000..0d64664
--- /dev/null
+++ b/mpc/basic_IOC/README.md
@@ -0,0 +1,146 @@
+# Model Predictive Control Basic Tool
+This program is about template, generic function of linear model predictive control
+
+# Documentation of the MPC function
+Linear model predicitive control should have state equation.
+So if you want to use this function, you should model the plant as state equation.
+Therefore, the parameters of this class are as following
+
+**class MpcController()**
+
+Attributes :
+
+- A : numpy.ndarray
+ - system matrix
+- B : numpy.ndarray
+ - input matrix
+- 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
+
+Methods:
+
+- initialize_controller() initialize the controller
+- calc_input(states, references) calculating optimal input
+
+More details, please look the **mpc_func_with_scipy.py** and **mpc_func_with_cvxopt.py**
+
+We have two function, mpc_func_with_cvxopt.py and mpc_func_with_scipy.py
+Both functions have same variable and member function. However the solver is different.
+Plese choose the right method for your environment.
+
+- example of import
+
+```py
+from mpc_func_with_scipy import MpcController as MpcController_scipy
+from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
+```
+
+# Examples
+## Problem Formulation
+
+- **first order system**
+
+
+
+- **ACC (Adaptive cruise control)**
+
+The two wheeled model are expressed the following equation.
+
+&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} "\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}")
+
+However, if we assume the velocity are constant, we can approximate the equation as following,
+
+
+
+then we can apply this model to linear mpc, we should give the model reference V although.
+
+- **evaluation function**
+
+the both examples have same evaluation function form as following equation.
+
+-\boldsymbol{r}(k+i|k)&space;||^2_{{\boldsymbol{Q}}(i)}&space;+&space;||\boldsymbol{\Delta&space;{U}}(k+i|k)||^2_{{\boldsymbol{R}}(i)} "J = \sum_{i = 0}^{prestep}||\boldsymbol{\hat{X}}(k+i|k)-\boldsymbol{r}(k+i|k) ||^2_{{\boldsymbol{Q}}(i)} + ||\boldsymbol{\Delta {U}}(k+i|k)||^2_{{\boldsymbol{R}}(i)}")
+
+- 
is predicit state by using predict input
+
+- 
is reference state
+
+- 
is predict amount of change of input
+
+- 
are evaluation function weights
+
+## Expected Results
+
+- first order system
+
+- time history
+
+
+
+- input
+
+
+
+- ACC (Adaptive cruise control)
+
+- time history of states
+
+
+
+- animation
+
+
+
+
+# Usage
+
+- for example(first order system)
+
+```
+$ python main_example.py
+```
+
+- for example(ACC (Adaptive cruise control))
+
+```
+$ python main_ACC.py
+```
+
+- for comparing two methods of optimization solvers
+
+```
+$ python test_compare_methods.py
+```
+
+# Requirement
+
+- python3.5 or more
+- numpy
+- matplotlib
+- cvxopt
+- scipy1.2.0 or more
+- python-control
+
+# Reference
+I`m sorry that main references are written in Japanese
+
+- モデル予測制御―制約のもとでの最適制御 著：Jan M. Maciejowski 訳：足立修一 東京電機大学出版局
\ No newline at end of file
diff --git a/mpc/basic_IOC/animation.py b/mpc/basic_IOC/animation.py
new file mode 100755
index 0000000..6ece541
--- /dev/null
+++ b/mpc/basic_IOC/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])
\ No newline at end of file
diff --git a/mpc/basic_IOC/main_ACC.py b/mpc/basic_IOC/main_ACC.py
new file mode 100644
index 0000000..a868559
--- /dev/null
+++ b/mpc/basic_IOC/main_ACC.py
@@ -0,0 +1,246 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import copy
+
+from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
+from animation import AnimDrawer
+from control import matlab
+
+class TwoWheeledSystem():
+ """SampleSystem, this is the simulator
+ Attributes
+ -----------
+ xs : numpy.ndarray
+ system states, [x, y, theta]
+ history_xs : list
+ time history of state
+ """
+ def __init__(self, init_states=None):
+ """
+ Palameters
+ -----------
+ init_state : float, optional, shape(3, )
+ initial state of system default is None
+ """
+ self.xs = np.zeros(3)
+
+ if init_states is not None:
+ self.xs = copy.deepcopy(init_states)
+
+ self.history_xs = [init_states]
+
+ def update_state(self, us, dt=0.01):
+ """
+ Palameters
+ ------------
+ u : numpy.ndarray
+ inputs of system in some cases this means the reference
+ 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(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, self._func_x_3]
+
+ # 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])
+
+ 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])
+
+ 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])
+
+ 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])
+
+ 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.
+
+ # 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):
+ """
+ Parameters
+ ------------
+ y_1 : float
+ y_2 : float
+ y_3 : float
+ u_1 : float
+ system input
+ u_2 : float
+ system input
+ """
+ y_dot = math.cos(y_3) * u_1
+ return y_dot
+
+ def _func_x_2(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 = 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 main():
+ dt = 0.05
+ simulation_time = 10 # 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
+ # lineared car system
+ V = 5.0
+ A = np.array([[0., V], [0., 0.]])
+ B = np.array([[0.], [1.]])
+
+ C = np.eye(2)
+ D = np.zeros((2, 1))
+
+ # make simulator with coninuous matrix
+ init_xs_lead = np.array([5., 0., 0.])
+ init_xs_follow = np.array([0., 0., 0.])
+ lead_car = TwoWheeledSystem(init_states=init_xs_lead)
+ follow_car = TwoWheeledSystem(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.])
+ R = np.diag([5.])
+ pre_step = 15
+
+ # 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,
+ 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,
+ dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
+ input_upper=np.array([30.]), input_lower=np.array([-30.]))
+
+ lead_controller.initialize_controller()
+ follow_controller.initialize_controller()
+
+ # reference
+ lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten()
+
+ for i in range(iteration_num):
+ print("simulation time = {0}".format(i))
+ # make lead car's move
+ if i > int(iteration_num / 3):
+ lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten()
+
+ lead_states = lead_car.xs
+ lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference)
+ lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
+
+ # make follow car
+ follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten()
+ follow_states = follow_car.xs
+
+ follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference)
+ follow_opt_u = np.hstack((np.array([V]), follow_opt_u))
+
+ lead_car.update_state(lead_opt_u, dt=dt)
+ follow_car.update_state(follow_opt_u, dt=dt)
+
+ # figures and animation
+ lead_history_states = np.array(lead_car.history_xs)
+ follow_history_states = np.array(follow_car.history_xs)
+
+ time_history_fig = plt.figure()
+ x_fig = time_history_fig.add_subplot(311)
+ y_fig = time_history_fig.add_subplot(312)
+ theta_fig = time_history_fig.add_subplot(313)
+
+ car_traj_fig = plt.figure()
+ traj_fig = car_traj_fig.add_subplot(111)
+ traj_fig.set_aspect('equal')
+
+ x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead")
+ x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow")
+ x_fig.set_xlabel("time [s]")
+ x_fig.set_ylabel("x")
+ x_fig.legend()
+
+ y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead")
+ y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow")
+ y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed")
+ y_fig.set_xlabel("time [s]")
+ y_fig.set_ylabel("y")
+ y_fig.legend()
+
+ theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead")
+ theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow")
+ theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
+ theta_fig.set_xlabel("time [s]")
+ theta_fig.set_ylabel("theta")
+ theta_fig.legend()
+
+ time_history_fig.tight_layout()
+
+ traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead")
+ traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow")
+ traj_fig.set_xlabel("x")
+ traj_fig.set_ylabel("y")
+ traj_fig.legend()
+ plt.show()
+
+ lead_history_us = np.array(lead_controller.history_us)
+ follow_history_us = np.array(follow_controller.history_us)
+ input_history_fig = plt.figure()
+ u_1_fig = input_history_fig.add_subplot(111)
+
+ u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead")
+ u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow")
+ u_1_fig.set_xlabel("time [s]")
+ u_1_fig.set_ylabel("u_omega")
+
+ input_history_fig.tight_layout()
+ plt.show()
+
+ animdrawer = AnimDrawer([lead_history_states, follow_history_states])
+ animdrawer.draw_anim()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/mpc/basic_IOC/main_ACC_TEMP.py b/mpc/basic_IOC/main_ACC_TEMP.py
new file mode 100644
index 0000000..9ad7c97
--- /dev/null
+++ b/mpc/basic_IOC/main_ACC_TEMP.py
@@ -0,0 +1,243 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import copy
+
+from mpc_func_with_cvxopt import MpcController as MpcController_cvxopt
+from animation import AnimDrawer
+# from control import matlab
+
+class TwoWheeledSystem():
+ """SampleSystem, this is the simulator
+ Attributes
+ -----------
+ xs : numpy.ndarray
+ system states, [x, y, theta]
+ history_xs : list
+ time history of state
+ """
+ def __init__(self, init_states=None):
+ """
+ Palameters
+ -----------
+ init_state : float, optional, shape(3, )
+ initial state of system default is None
+ """
+ self.xs = np.zeros(3)
+
+ if init_states is not None:
+ self.xs = copy.deepcopy(init_states)
+
+ self.history_xs = [init_states]
+
+ def update_state(self, us, dt=0.01):
+ """
+ Palameters
+ ------------
+ u : numpy.ndarray
+ inputs of system in some cases this means the reference
+ 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(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, self._func_x_3]
+
+ # 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])
+
+ 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])
+
+ 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])
+
+ 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])
+
+ 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.
+
+ # 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):
+ """
+ Parameters
+ ------------
+ y_1 : float
+ y_2 : float
+ y_3 : float
+ u_1 : float
+ system input
+ u_2 : float
+ system input
+ """
+ y_dot = math.cos(y_3) * u_1
+ return y_dot
+
+ def _func_x_2(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 = 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 main():
+ dt = 0.05
+ simulation_time = 10 # 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
+ # lineared car system
+ V = 5.0
+ Ad = np.array([[1., V * dt], [0., 1.]])
+ Bd = np.array([[0.], [1. * dt]])
+
+ C = np.eye(2)
+ D = np.zeros((2, 1))
+
+ # make simulator with coninuous matrix
+ init_xs_lead = np.array([5., 0., 0.])
+ init_xs_follow = np.array([0., 0., 0.])
+ lead_car = TwoWheeledSystem(init_states=init_xs_lead)
+ follow_car = TwoWheeledSystem(init_states=init_xs_follow)
+
+ # create system
+ # sysc = matlab.ss(A, B, C, D)
+ # discrete system
+ # sysd = matlab.c2d(sysc, dt)
+
+ # evaluation function weight
+ Q = np.diag([1., 1.])
+ R = np.diag([5.])
+ pre_step = 15
+
+ # 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,
+ 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,
+ dt_input_upper=np.array([30 * dt]), dt_input_lower=np.array([-30 * dt]),
+ input_upper=np.array([30.]), input_lower=np.array([-30.]))
+
+ lead_controller.initialize_controller()
+ follow_controller.initialize_controller()
+
+ # reference
+ lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten()
+
+ for i in range(iteration_num):
+ print("simulation time = {0}".format(i))
+ # make lead car's move
+ if i > int(iteration_num / 3):
+ lead_reference = np.array([[4., 0.] for _ in range(pre_step)]).flatten()
+
+ lead_states = lead_car.xs
+ lead_opt_u = lead_controller.calc_input(lead_states[1:], lead_reference)
+ lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
+
+ # make follow car
+ follow_reference = np.array([lead_states[1:] for _ in range(pre_step)]).flatten()
+ follow_states = follow_car.xs
+
+ follow_opt_u = follow_controller.calc_input(follow_states[1:], follow_reference)
+ follow_opt_u = np.hstack((np.array([V]), follow_opt_u))
+
+ lead_car.update_state(lead_opt_u, dt=dt)
+ follow_car.update_state(follow_opt_u, dt=dt)
+
+ # figures and animation
+ lead_history_states = np.array(lead_car.history_xs)
+ follow_history_states = np.array(follow_car.history_xs)
+
+ time_history_fig = plt.figure()
+ x_fig = time_history_fig.add_subplot(311)
+ y_fig = time_history_fig.add_subplot(312)
+ theta_fig = time_history_fig.add_subplot(313)
+
+ car_traj_fig = plt.figure()
+ traj_fig = car_traj_fig.add_subplot(111)
+ traj_fig.set_aspect('equal')
+
+ x_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 0], label="lead")
+ x_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 0], label="follow")
+ x_fig.set_xlabel("time [s]")
+ x_fig.set_ylabel("x")
+ x_fig.legend()
+
+ y_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 1], label="lead")
+ y_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 1], label="follow")
+ y_fig.plot(np.arange(0, simulation_time+0.01, dt), [4. for _ in range(iteration_num+1)], linestyle="dashed")
+ y_fig.set_xlabel("time [s]")
+ y_fig.set_ylabel("y")
+ y_fig.legend()
+
+ theta_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_states[:, 2], label="lead")
+ theta_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_states[:, 2], label="follow")
+ theta_fig.plot(np.arange(0, simulation_time+0.01, dt), [0. for _ in range(iteration_num+1)], linestyle="dashed")
+ theta_fig.set_xlabel("time [s]")
+ theta_fig.set_ylabel("theta")
+ theta_fig.legend()
+
+ time_history_fig.tight_layout()
+
+ traj_fig.plot(lead_history_states[:, 0], lead_history_states[:, 1], label="lead")
+ traj_fig.plot(follow_history_states[:, 0], follow_history_states[:, 1], label="follow")
+ traj_fig.set_xlabel("x")
+ traj_fig.set_ylabel("y")
+ traj_fig.legend()
+ plt.show()
+
+ lead_history_us = np.array(lead_controller.history_us)
+ follow_history_us = np.array(follow_controller.history_us)
+ input_history_fig = plt.figure()
+ u_1_fig = input_history_fig.add_subplot(111)
+
+ u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), lead_history_us[:, 0], label="lead")
+ u_1_fig.plot(np.arange(0, simulation_time+0.01, dt), follow_history_us[:, 0], label="follow")
+ u_1_fig.set_xlabel("time [s]")
+ u_1_fig.set_ylabel("u_omega")
+
+ input_history_fig.tight_layout()
+ plt.show()
+
+ animdrawer = AnimDrawer([lead_history_states, follow_history_states])
+ animdrawer.draw_anim()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/mpc/basic_IOC/main_example.py b/mpc/basic_IOC/main_example.py
new file mode 100644
index 0000000..082693a
--- /dev/null
+++ b/mpc/basic_IOC/main_example.py
@@ -0,0 +1,184 @@
+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
+ -----------
+ xs : numpy.ndarray
+ system states
+ A : numpy.ndarray
+ system matrix
+ B : numpy.ndarray
+ control matrix
+ C : numpy.ndarray
+ observation matrix
+ history_xs : 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
+ D : 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 : numpy.ndarray
+ inputs 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))
+
+ # save
+ save_states = copy.deepcopy(self.xs)
+ self.history_xs.append(save_states)
+
+def main():
+ dt = 0.05
+ simulation_time = 30 # 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 = 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., 1., 1.])
+ R = np.diag([1., 1.])
+ pre_step = 10
+
+ # make controller with discreted matrix
+ # 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., -5., 7.5] for _ in range(pre_step)]).flatten()
+ states = plant.xs
+ opt_u = controller.calc_input(states, reference)
+ plant.update_state(opt_u, dt=dt)
+
+ history_states = np.array(plant.history_xs)
+
+ time_history_fig = plt.figure()
+ 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[:, 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()
+ plt.show()
+
+ history_us = np.array(controller.history_us)
+ input_history_fig = plt.figure()
+ 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[:, 0])
+ u_1_fig.set_xlabel("time [s]")
+ u_1_fig.set_ylabel("u_x")
+
+ u_2_fig.plot(np.arange(0, simulation_time+0.01, dt), history_us[:, 1])
+ u_2_fig.set_xlabel("time [s]")
+ u_2_fig.set_ylabel("u_y")
+ input_history_fig.tight_layout()
+ plt.show()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/mpc/basic_IOC/memo.md b/mpc/basic_IOC/memo.md
new file mode 100644
index 0000000..65d983c
--- /dev/null
+++ b/mpc/basic_IOC/memo.md
@@ -0,0 +1,5 @@
+ # ラプラス近似
+ 参考
+ - http://triadsou.hatenablog.com/entry/20090217/1234865055
+
+ #
\ No newline at end of file
diff --git a/mpc/basic_IOC/mpc_func_with_cvxopt.py b/mpc/basic_IOC/mpc_func_with_cvxopt.py
new file mode 100644
index 0000000..4e04736
--- /dev/null
+++ b/mpc/basic_IOC/mpc_func_with_cvxopt.py
@@ -0,0 +1,256 @@
+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 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, 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
+ 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.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))
+
+ # 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
+
+ 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
\ No newline at end of file
diff --git a/mpc/basic_IOC/mpc_func_with_scipy.py b/mpc/basic_IOC/mpc_func_with_scipy.py
new file mode 100644
index 0000000..54ef3c9
--- /dev/null
+++ b/mpc/basic_IOC/mpc_func_with_scipy.py
@@ -0,0 +1,262 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import copy
+
+from scipy.optimize import minimize
+from scipy.optimize import LinearConstraint
+
+class MpcController():
+ """
+ Attributes
+ ------------
+ A : numpy.ndarray
+ system matrix
+ B : numpy.ndarray
+ input matrix
+ 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, 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
+ 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.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.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
+
+ 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()
+
+ def optimized_func(dt_us):
+ """
+ """
+ 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]
+
+ # 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 :
+ opt_result = minimize(optimized_func, init_dt_us, constraints=[linear_cons])
+
+ opt_dt_us = 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
\ No newline at end of file
diff --git a/mpc/basic_IOC/test_compare_methods.py b/mpc/basic_IOC/test_compare_methods.py
new file mode 100644
index 0000000..5ddaea4
--- /dev/null
+++ b/mpc/basic_IOC/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
diff --git a/mpc/extend/main_track.py b/mpc/extend/main_track.py
index 9a1f062..3b8d6a2 100644
--- a/mpc/extend/main_track.py
+++ b/mpc/extend/main_track.py
@@ -317,7 +317,7 @@ def main():
# input()
# evaluation function weight
- Q = np.diag([100., 1000., 1.])
+ Q = np.diag([1000000., 10., 1.])
R = np.diag([0.1])
# System model update