├── .gitignore ├── .idea ├── .gitignore ├── efficient-calibration-embedded-MPC.iml ├── inspectionProfiles │ └── profiles_settings.xml ├── misc.xml ├── modules.xml ├── other.xml └── vcs.xml ├── GLIS_BO_analysis.py ├── GLIS_BO_main.py ├── LICENSE.md ├── README.md ├── idwgopt ├── __init__.py ├── idwgopt.py ├── idwgopt_default.py ├── idwgopt_init.py └── readme.txt ├── kalman.py ├── objective_function.py ├── paper ├── fig │ ├── BEST_GLIS_PC.pdf │ ├── BEST_GLIS_PI.pdf │ ├── ITER_GLIS_PC.pdf │ ├── ITER_GLIS_PC_500.pdf │ ├── ITER_GLIS_PI_500.pdf │ ├── MPC_CPUTIME_GLIS_PI.pdf │ ├── cart_pole.pdf │ └── sym │ │ ├── F.svg │ │ ├── elle.svg │ │ ├── elleM.svg │ │ ├── m (Case Conflict).svg │ │ ├── m.svg │ │ ├── p.svg │ │ └── preview.svg ├── ms.bib ├── ms.pdf └── ms.tex ├── pendulum_MPC_sim.py ├── pendulum_model.py ├── pyMPC ├── __init__.py └── mpc.py └── slides ├── biblio.bib ├── img ├── GLIS │ ├── GLIS_06.pdf │ ├── GLIS_07.pdf │ ├── GLIS_08.pdf │ ├── acquisition.png │ └── surrogate.png ├── MPCloop.pdf ├── MPCloop.svg ├── example │ ├── BEST_GLIS_PC.pdf │ ├── BEST_GLIS_PI.pdf │ ├── COMPUTATION_BO_PI.pdf │ ├── COMPUTATION_GLIS_PI.pdf │ ├── HIL_PC.pdf │ ├── HIL_PI.pdf │ ├── ITER_GLIS_PC_500.pdf │ ├── ITER_GLIS_PI_500.pdf │ └── cart_pole.pdf └── teaser.pdf ├── preamble.tex ├── presentation_main.pdf └── presentation_main.tex /.gitignore: -------------------------------------------------------------------------------- 1 | /idwgopt/.pytest_cache/ 2 | -------------------------------------------------------------------------------- /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | # Default ignored files 2 | /workspace.xml -------------------------------------------------------------------------------- /.idea/efficient-calibration-embedded-MPC.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 40 | 41 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 94 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.idea/other.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /GLIS_BO_analysis.py: -------------------------------------------------------------------------------- 1 | import os 2 | import matplotlib 3 | if os.name == 'posix' and "DISPLAY" not in os.environ: 4 | matplotlib.use("Agg") 5 | 6 | import numpy as np 7 | from pendulum_MPC_sim import simulate_pendulum_MPC, get_parameter 8 | from numpy.random import seed 9 | import matplotlib.pyplot as plt 10 | from objective_function import f_x, get_simoptions_x 11 | from pendulum_model import RAD_TO_DEG 12 | import pickle 13 | import os 14 | from scipy.interpolate import interp1d 15 | if __name__ == '__main__': 16 | 17 | matplotlib.rc('text', usetex=True) 18 | 19 | algo = 'GLIS' # GLIS or BO 20 | machine = 'PI' # PC or PI 21 | eps_calc = 1.0 22 | iter_max_plot = 500 23 | 24 | plt.close('all') 25 | res_filename = f"res_slower{eps_calc:.0f}_500iter_{algo}_{machine}.pkl" 26 | results = pickle.load(open(res_filename, "rb")) 27 | 28 | 29 | # In[] 30 | FIG_FOLDER = 'fig' 31 | if not os.path.isdir(FIG_FOLDER): 32 | os.makedirs(FIG_FOLDER) 33 | 34 | # In[Re-simulate] 35 | 36 | ## Re-simulate with the optimal point 37 | x_opt = results['x_opt'] 38 | 39 | simopt = get_simoptions_x(x_opt) 40 | t_ref_vec = np.array([0.0, 5.0, 10.0, 13.0, 20.0, 22.0, 25.0, 30.0, 35.0, 40.0, 100.0]) 41 | p_ref_vec = np.array([0.0, 0.4, 0.0, 0.9, 0.9, 0.4, 0.4, 0.4, 0.0, 0.0, 0.0]) 42 | rp_fun = interp1d(t_ref_vec, p_ref_vec, kind='linear') 43 | def xref_fun_def(t): 44 | return np.array([rp_fun(t), 0.0, 0.0, 0.0]) 45 | simopt['xref_fun'] = xref_fun_def 46 | 47 | 48 | simout = simulate_pendulum_MPC(simopt) 49 | 50 | t = simout['t'] 51 | x = simout['x'] 52 | u = simout['u'] 53 | y = simout['y'] 54 | y_meas = simout['y_meas'] 55 | x_ref = simout['x_ref'] 56 | x_MPC_pred = simout['x_MPC_pred'] 57 | x_fast = simout['x_fast'] 58 | x_ref_fast = simout['x_ref_fast'] 59 | y_ref = x_ref[:, [0, 2]] # on-line predictions from the Kalman Filter 60 | uref = get_parameter({}, 'uref') 61 | u_fast = simout['u_fast'] 62 | 63 | t_int = simout['t_int_fast'] 64 | t_fast = simout['t_fast'] 65 | t_calc = simout['t_calc'] 66 | 67 | fig, axes = plt.subplots(3, 1, figsize=(8, 6)) 68 | # axes[0].plot(t, y_meas[:, 0], "r", label='p_meas') 69 | axes[0].plot(t_fast, x_fast[:, 0], "k", label='$p$') 70 | axes[0].plot(t, y_ref[:, 0], "r--", label="$p^{\mathrm{ref}}$", linewidth=2) 71 | axes[0].set_ylim(-0.2, 1.0) 72 | axes[0].set_ylabel("Position (m)") 73 | 74 | # axes[1].plot(t, y_meas[:, 1] * RAD_TO_DEG, "r", label='phi_meas') 75 | axes[1].plot(t_fast, x_fast[:, 2] * RAD_TO_DEG, 'k', label="$\phi$") 76 | idx_pred = 0 77 | axes[1].set_ylim(-12, 12) 78 | axes[1].set_ylabel("Angle (deg)") 79 | 80 | axes[2].plot(t, u[:, 0], 'k', label="$u$") 81 | #axes[2].plot(t, uref * np.ones(np.shape(t)), "r--", label="u_ref") 82 | axes[2].set_ylim(-8, 8) 83 | axes[2].set_ylabel("Force (N)") 84 | axes[2].set_xlabel("Simulation time (s)") 85 | 86 | for ax in axes: 87 | ax.grid(True) 88 | ax.legend(loc='upper right') 89 | 90 | fig_name = f"BEST_{algo}_{machine}.pdf" 91 | fig_path = os.path.join(FIG_FOLDER, fig_name) 92 | fig.savefig(fig_path, bbox_inches='tight') 93 | 94 | # MPC time check 95 | # In[MPC computation time ] 96 | fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True) 97 | axes[0].plot(t, y_meas[:, 0], "r", label='$p_{meas}$') 98 | axes[0].plot(t_fast, x_fast[:, 0], "k", label='$p$') 99 | axes[0].step(t, y_ref[:, 0], "k--", where='post', label="$p_{ref}$") 100 | axes[0].set_ylim(-0.2, 1.0) 101 | axes[0].set_xlabel("Simulation time (s)") 102 | axes[0].set_ylabel("Position (m)") 103 | 104 | axes[1].step(t, t_calc[:, 0] * 1e3, "b", where='post', label='$T_{MPC}$') 105 | axes[1].set_xlabel("Simulation time (s)") 106 | axes[1].set_ylabel("MPC time (ms)") 107 | axes[1].set_ylim(0, 40) 108 | axes[2].step(t_fast[1:], t_int[1:, 0] * 1e3, "b", where='post', label='$T_{ODE}$') 109 | axes[2].set_xlabel("Simulation time (s)") 110 | axes[2].set_ylabel("ODE time (ms)") 111 | axes[2].set_ylim(0, 2) 112 | axes[3].step(t, u[:, 0], where='post', label="$F$") 113 | axes[3].step(t_fast, u_fast[:, 0], where='post', label="$F_{d}$") 114 | axes[3].set_xlabel("Simulation time (s)") 115 | axes[3].set_ylabel("Force (N)") 116 | 117 | for ax in axes: 118 | ax.grid(True) 119 | ax.legend() 120 | 121 | fig_name = f"MPC_CPUTIME_{algo}_{machine}.pdf" 122 | fig_path = os.path.join(FIG_FOLDER, fig_name) 123 | fig.savefig(fig_path, bbox_inches='tight') 124 | 125 | # In[Iteration plot] 126 | 127 | J = results['J_sample'] 128 | Ts_MPC = simout['Ts_MPC'] 129 | 130 | J_best_curr = np.zeros(np.shape(J)) 131 | J_best_val = J[0] 132 | iter_best_val = 0 133 | 134 | fig, axes = plt.subplots(1, 1, figsize=(6, 4)) 135 | axes = [axes] 136 | for i in range(len(J_best_curr)): 137 | if J[i] < J_best_val: 138 | J_best_val = J[i] 139 | iter_best_val = i 140 | J_best_curr[i] = J_best_val 141 | 142 | N = len(J) 143 | iter = np.arange(1, N + 1, dtype=np.int) 144 | axes[0].plot(iter, J, 'k*', label='Current test point') 145 | # axes[0].plot(iter, Y_best_curr, 'r', label='Current best point') 146 | axes[0].plot(iter, J_best_val * np.ones(J.shape), '-', label='Overall best point', color='red') 147 | axes[0].set_xlabel("Iteration index $n$ (-)") 148 | axes[0].set_ylabel(r"Performance cost $\tilde {J}^{\mathrm{cl}}$") 149 | 150 | for ax in axes: 151 | ax.grid(True) 152 | ax.legend(loc='upper right') 153 | 154 | axes[0].set_xlim((0, iter_max_plot)) 155 | axes[0].set_ylim((-1, 25)) 156 | 157 | fig_name = f"ITER_{algo}_{machine}_{iter_max_plot:.0f}.pdf" 158 | fig_path = os.path.join(FIG_FOLDER, fig_name) 159 | fig.savefig(fig_path, bbox_inches='tight') 160 | 161 | 162 | # In[Recompute optimum] 163 | #print(Y_best_val) 164 | 165 | J_opt = f_x(x_opt, eps_calc=results['eps_calc']) 166 | #print(J_opt) 167 | 168 | # In[Optimization computation time] 169 | 170 | t_unknown = results['time_iter'] - ( 171 | results['time_f_eval'] + results['time_opt_acquisition'] + results['time_fit_surrogate']) 172 | fig, ax = plt.subplots(1, 1, figsize=(6, 4)) 173 | ax.step(iter, results['time_iter'], 'k', where='post', label='Total') 174 | ax.step(iter, results['time_f_eval'], 'r', where='post', label='Eval') 175 | ax.step(iter, results['time_opt_acquisition'], 'y', where='post', label='Opt') 176 | ax.step(iter, results['time_fit_surrogate'], 'g', where='post', label='Fit') 177 | ax.grid(True) 178 | ax.legend() 179 | 180 | fig, ax = plt.subplots(1, 1, figsize=(6, 4)) 181 | ax.step(iter, np.cumsum(results['time_iter']), 'k', where='post', label='Total') 182 | ax.step(iter, np.cumsum(results['time_f_eval']), 'r', where='post', label='Function evaluation') 183 | ax.step(iter, np.cumsum(results['time_fit_surrogate']), 'g', where='post', label='Surrogate fitting') 184 | ax.step(iter, np.cumsum(results['time_opt_acquisition']), 'y', where='post', label='Surrogate optimization') 185 | # ax.step(iter, np.cumsum(t_unknown), 'g', where='post', label='Unknown') 186 | ax.set_xlabel("Iteration index i (-)") 187 | ax.set_ylabel("Comulative computational time (s)") 188 | ax.grid(True) 189 | ax.legend() 190 | 191 | fig_name = f"COMPUTATION_{algo}_{machine}.pdf" 192 | fig_path = os.path.join(FIG_FOLDER, fig_name) 193 | fig.savefig(fig_path, bbox_inches='tight') 194 | 195 | residual_time = np.sum(results['time_iter']) - np.sum(results['time_f_eval']) - np.sum( 196 | results['time_opt_acquisition']) - np.sum(results['time_fit_surrogate']) 197 | -------------------------------------------------------------------------------- /GLIS_BO_main.py: -------------------------------------------------------------------------------- 1 | import os 2 | import matplotlib 3 | 4 | if os.name == 'posix' and "DISPLAY" not in os.environ: 5 | matplotlib.use("Agg") 6 | import GPyOpt 7 | import idwgopt.idwgopt 8 | import numpy as np 9 | from pendulum_MPC_sim import simulate_pendulum_MPC, get_parameter, get_default_parameters 10 | import matplotlib.pyplot as plt 11 | from objective_function import dict_to_x, f_x, get_simoptions_x 12 | import pickle 13 | import time 14 | import numba as nb 15 | 16 | if __name__ == '__main__': 17 | 18 | np.random.seed(0) 19 | # initial random points 20 | 21 | # optimization parameters 22 | # Run the optimization 23 | # n_init = 10 changed to 2*n_var (GLIS default choice) 24 | max_iter = 500 # evaluation budget 25 | max_time = np.inf # time budget 26 | eps = 0.0 # Minimum allows distance between the las two observations 27 | eps_calc = 1.0 28 | 29 | method = "GLIS" # GLIS or BO 30 | machine = 'PC' # PC or PI 31 | 32 | 33 | dict_x0 = { 34 | 'QDu_scale': 0.1, 35 | 'Qy11': 0.1, 36 | 'Qy22': 0.9, 37 | 'Np': 40, 38 | 'Nc_perc': 1.0, 39 | 'Ts_MPC': 25e-3, 40 | 'QP_eps_abs_log': -3, 41 | 'QP_eps_rel_log': -2, 42 | 'Q_kal_11': 0.1, 43 | 'Q_kal_22': 0.4, 44 | 'Q_kal_33': 0.1, 45 | 'Q_kal_44': 0.4, 46 | 'R_kal_11': 0.5, 47 | 'R_kal_22': 0.5 48 | } 49 | 50 | dict_context = {} 51 | 52 | n_var = len(dict_x0) 53 | n_init = 2 * n_var 54 | 55 | x0 = dict_to_x(dict_x0) 56 | 57 | bounds = [ 58 | {'name': 'QDu_scale', 'type': 'continuous', 'domain': (1e-16, 1)}, # 0 59 | {'name': 'Qy11', 'type': 'continuous', 'domain': (1e-16, 1)}, # 1 60 | {'name': 'Qy22', 'type': 'continuous', 'domain': (1e-16, 1)}, # 2 61 | {'name': 'Np', 'type': 'continuous', 'domain': (5, 300)}, # 3 62 | {'name': 'Nc_perc', 'type': 'continuous', 'domain': (0.3, 1)}, # 4 63 | {'name': 'Ts_MPC', 'type': 'continuous', 'domain': (1e-3, 50e-3)}, # 5 64 | {'name': 'QP_eps_abs_log', 'type': 'continuous', 'domain': (-7, -1)}, # 6 65 | {'name': 'QP_eps_rel_log', 'type': 'continuous', 'domain': (-7, -1)}, # 7 66 | {'name': 'Q_kal_11', 'type': 'continuous', 'domain': (1e-16, 1)}, # 8 67 | {'name': 'Q_kal_22', 'type': 'continuous', 'domain': (1e-16, 1)}, # 9 68 | {'name': 'Q_kal_33', 'type': 'continuous', 'domain': (1e-16, 1)}, # 10 69 | {'name': 'Q_kal_44', 'type': 'continuous', 'domain': (1e-16, 1)}, # 11 70 | {'name': 'R_kal_11', 'type': 'continuous', 'domain': (1e-16, 1)}, # 12 71 | {'name': 'R_kal_22', 'type': 'continuous', 'domain': (1e-16, 1)}, # 13 72 | ] 73 | 74 | constraints = [ 75 | # {'name': 'min_time', 'constraint': '-x[:,4]*x[:,6] + 0.1'}, # prediction horizon in seconds large enough 76 | ] 77 | 78 | 79 | def f_x_calc(x): 80 | return f_x(x, eps_calc) 81 | 82 | 83 | feasible_region = GPyOpt.Design_space(space=bounds, 84 | constraints=constraints) # , constraints=constraints_context) 85 | # unfeasible_region = GPyOpt.Design_space(space=bounds) 86 | X_init = GPyOpt.experiment_design.initial_design('random', feasible_region, n_init) 87 | 88 | time_optimization_start = time.perf_counter() 89 | if method == "BO": 90 | myBopt = GPyOpt.methods.BayesianOptimization(f_x_calc, 91 | X=X_init, 92 | domain=bounds, 93 | model_type='GP', 94 | acquisition_type='EI', 95 | normalize_Y=True, 96 | exact_feval=False) 97 | 98 | myBopt.run_optimization(max_iter=max_iter - n_init, max_time=max_time, eps=eps, verbosity=False) 99 | 100 | x_opt = myBopt.x_opt 101 | J_opt = myBopt.fx_opt 102 | X_sample = myBopt.X 103 | J_sample = myBopt.Y 104 | idx_opt = np.argmin(J_sample) 105 | 106 | if method == "GLIS": 107 | 108 | # IDWGOPT initialization 109 | nvars = len(bounds) 110 | lb = np.zeros((nvars, 1)).flatten("c") 111 | ub = lb.copy() 112 | for i in range(0, nvars): 113 | lb[i] = bounds[i]['domain'][0] 114 | ub[i] = bounds[i]['domain'][1] 115 | 116 | problem = idwgopt.idwgopt.default(nvars) 117 | problem["nsamp"] = n_init 118 | problem["maxevals"] = max_iter 119 | # problem["g"] = lambda x: np.array([-x[5]*x[7]+0.1]) 120 | problem["lb"] = lb 121 | problem["ub"] = ub 122 | problem["f"] = f_x_calc 123 | problem["useRBF"] = 1 # use Radial Basis Functions 124 | # problem["useRBF"] = 0 # Inverse Distance Weighting 125 | if problem["useRBF"]: 126 | epsil = .5 127 | 128 | 129 | @nb.guvectorize([(nb.float64[:], nb.float64[:], nb.float64[:])], '(n),(n)->()') 130 | def fun_rbf(x1, x2, res): 131 | res[0] = 1 / (1 + epsil ** 2 * np.sum((x1 - x2) ** 2)) 132 | 133 | 134 | problem['rbf'] = fun_rbf 135 | 136 | problem["alpha"] = 1 137 | problem["delta"] = .5 138 | 139 | problem["svdtol"] = 1e-6 140 | # problem["globoptsol"] = "direct" 141 | problem["globoptsol"] = "pswarm" 142 | problem["display"] = 0 143 | 144 | problem["scalevars"] = 1 145 | 146 | problem["constraint_penalty"] = 1e3 147 | problem["feasible_sampling"] = False 148 | 149 | problem["shrink_range"] = 0 # 0 = don't shrink lb/ub 150 | 151 | tic = time.perf_counter() 152 | out = idwgopt.idwgopt.solve(problem) 153 | toc = time.perf_counter() 154 | 155 | x_opt = out["xopt"] 156 | J_opt = out["fopt"] 157 | J_sample = out["F"] 158 | X_sample = out["X"] 159 | idx_opt = np.argmin(J_sample, axis=0) 160 | 161 | time_optimization = time.perf_counter() - time_optimization_start 162 | 163 | print(f"J_best_val: {J_opt:.3f}") 164 | 165 | # In[Re-simulate with the optimal point] 166 | 167 | simopt = get_simoptions_x(x_opt) 168 | simout = simulate_pendulum_MPC(simopt) 169 | 170 | tsim = simout['t'] 171 | xsim = simout['x'] 172 | usim = simout['u'] 173 | 174 | x_ref = simout['x_ref'] 175 | uref = get_parameter({}, 'uref') 176 | 177 | fig, axes = plt.subplots(3, 1, figsize=(10, 10)) 178 | axes[0].plot(tsim, xsim[:, 0], "k", label='p') 179 | axes[0].plot(tsim, x_ref[:, 0], "r--", label="p_ref") 180 | axes[0].set_title("Position (m)") 181 | 182 | axes[1].plot(tsim, xsim[:, 2] * 360 / 2 / np.pi, label="phi") 183 | axes[1].plot(tsim, x_ref[:, 2] * 360 / 2 / np.pi, "r--", label="phi_ref") 184 | axes[1].set_title("Angle (deg)") 185 | 186 | axes[2].plot(tsim, usim[:, 0], label="u") 187 | axes[2].plot(tsim, uref * np.ones(np.shape(tsim)), "r--", label="u_ref") 188 | axes[2].set_title("Force (N)") 189 | 190 | for ax in axes: 191 | ax.grid(True) 192 | ax.legend() 193 | 194 | J_best_curr = np.zeros(np.shape(J_sample)) 195 | J_best_val = J_sample[0] 196 | iter_best_val = 0 197 | 198 | fig, axes = plt.subplots(1, 1, figsize=(8, 5)) 199 | axes = [axes] 200 | for i in range(len(J_best_curr)): 201 | if J_sample[i] < J_best_val: 202 | J_best_val = J_sample[i] 203 | iter_best_val = i 204 | J_best_curr[i] = J_best_val 205 | 206 | N = len(J_sample) 207 | iter = np.arange(0, N, dtype=np.int) 208 | axes[0].plot(iter, J_sample, 'k*', label='Current test boint') 209 | axes[0].plot(iter, J_best_curr, 'g', label='Current best point') 210 | axes[0].plot(iter_best_val, J_best_val, 's', label='Overall best point') 211 | 212 | for ax in axes: 213 | ax.grid(True) 214 | ax.legend() 215 | 216 | # In[Re-evaluate optimal controller] 217 | J_opt = f_x(x_opt) 218 | print(J_opt) 219 | 220 | plt.show() 221 | 222 | # In[Store results] 223 | 224 | result = {} 225 | if method == 'BO': 226 | myBopt.f = None # hack to solve the issues pickling the myBopt object 227 | myBopt.objective = None 228 | 229 | results = {'X_sample': myBopt.X, 'J_sample': myBopt.Y, 230 | 'idx_opt': idx_opt, 'x_opt': x_opt, 'J_opt': J_opt, 231 | 'eps_calc': eps_calc, 232 | 'time_iter': myBopt.time_iter, 'time_f_eval': myBopt.time_f_eval, 233 | 'time_opt_acquisition': myBopt.time_opt_acquisition, 'time_fit_surrogate': myBopt.time_fit_surrogate, 234 | 'myBopt': myBopt, 'method': method 235 | } 236 | 237 | if method == 'GLIS': 238 | results = {'X_sample': X_sample, 'J_sample': J_sample, 239 | 'idx_opt': idx_opt, 'x_opt': x_opt, 'J_opt': J_opt, 240 | 'eps_calc': eps_calc, 241 | 'time_iter': out['time_iter'], 'time_f_eval': out['time_f_eval'], 242 | 'time_opt_acquisition': out['time_opt_acquisition'], 'time_fit_surrogate': out['time_fit_surrogate'], 243 | 'out': out, 'method': method 244 | } 245 | 246 | res_filename = f"res_slower{eps_calc:.0f}_{max_iter:.0f}iter_{method}_{machine}.pkl" 247 | 248 | with open(res_filename, "wb") as file: 249 | pickle.dump(results, file) 250 | 251 | 252 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Marco Forgione 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Efficient Calibration of Embedded MPC 2 | 3 | This repository contains the Python code to reproduce the results of the paper [Efficient Calibration of Embedded MPC](https://arxiv.org/pdf/1911.13021) by Marco Forgione, Dario Piga, and Alberto Bemporad. 4 | 5 | The code performs an efficient data-driven MPC calibration by tuning: 6 | 7 | * MPC weight matrices 8 | * MPC sampling time $T_{\mathrm{s}}^{\mathrm{MPC}}$ 9 | * Prediction and control horizon 10 | * Kalman filter matrices 11 | * QP solver relative and absolute tolerances 12 | 13 | to optimize a closed-loop objective function $J^{cl}$ , under the constraint that $T_{\mathrm{s}}^{\mathrm{MPC}} \leq \eta T_{\mathrm{s}}^{\mathrm{MPC}}$ where $T_{\mathrm{calc}}^{\mathrm{MPC}}$ is the (worst-case) time required to compute the MPC 14 | control low. This constraints guarantees that the controller can run in real-time. 15 | 16 | ## Main scripts: 17 | 18 | The main script to be executed for MPC calibration is 19 | 20 | `` GLIS_BO_main.py`` 21 | 22 | The results of the MPC calibration are saved in the results_*.pkl file 23 | on the disk and are read by the script 24 | 25 | ``GLIS_BO_analysis.py`` 26 | 27 | that produces the relevant plots. 28 | ## Other files: 29 | * ``pendulum_model.py``: dynamic equations of the pendulum 30 | * ``pendulum_MPC_sim``: performs a single closed-loop MPC simulation 31 | * ``objective_function.py``: objective function 32 | * ``kalman.py``: implements a kalman filter 33 | 34 | ## Included dependencies: 35 | * ``pyMPC``: containts the pyMPC library for Model Predictive Control. Copied from branch dev-BO of my repository , 36 | * ``idwgopt``: contains the python version of the GLIS package version 1.1. Copied from 37 | ## Other dependencies: 38 | 39 | Simulations were performed on a Python 3.6 conda environment with 40 | 41 | * numpy 42 | * scipy 43 | * matplotlib 44 | * OSQP (a QP solver used by the MPC controller) 45 | * python-control (used to solve the DARE of the Kalman Filter) 46 | * GPyOpt (for Bayesian Optimization, optional) 47 | 48 | These dependencies may be installed through the commands: 49 | ``` 50 | conda install numpy scipy matplotlib 51 | pip install osqp 52 | pip install control 53 | pip install gpyopt 54 | ``` 55 | ## Citing 56 | 57 | If you find this project useful, we encourage you to 58 | 59 | * Star this repository :star: 60 | * Cite the [paper](https://arxiv.org/pdf/1911.13021) 61 | ``` 62 | @inproceedings{forgione2020efficient, 63 | title={{E}fficient {C}alibration of {E}mbedded {MPC}}, 64 | author={Forgione, Marco and Piga, Dario and Bemporad, Alberto}, 65 | booktitle={Proc. of the 21st IFAC World Congress, Berlin, Germany}, 66 | year={2020} 67 | } 68 | ``` 69 | -------------------------------------------------------------------------------- /idwgopt/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/idwgopt/__init__.py -------------------------------------------------------------------------------- /idwgopt/idwgopt.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | import numba as nb 4 | 5 | @nb.guvectorize([(nb.float64[:], nb.float64[:], nb.float64[:])], '(n),(n)->()') 6 | def fast_dist(X1, X2, res): 7 | res[0] = 0.0 8 | n = X1.shape[0] 9 | for i in range(n): 10 | res[0] += (X1[i] - X2[i])**2 11 | 12 | def solve(prob): 13 | """ 14 | Solve global optimization problem using inverse distance 15 | weighting and radial basis functions. 16 | 17 | (C) 2019 A. Bemporad 18 | 19 | sol = idwgopt.solve(prob) solves the global optimization problem 20 | 21 | min f(x) 22 | s.t. lb <= x <=ub, A*x <=b, g(x)<=0 23 | 24 | using the global optimization algorithm described in [1]. The approach is 25 | particularly useful when f(x) is time-consuming to evaluate, as it 26 | attempts at minimizing the number of function evaluations. 27 | 28 | The default problem structure is 29 | 30 | prob = idwgopt.default(nvars) 31 | 32 | where nvars = dimension of optimization vector x. See function idwgopt_default 33 | for a description of all available options. 34 | 35 | The output argument 'out' is a structure reporting the following information: 36 | 37 | out.X: trace of all samples x at which f(x) has been evaluated 38 | out.F: trace of all function evaluations f(x) 39 | out.W: final set of weights (only meaningful for RBFs) 40 | out.xopt: best sample found during search 41 | out.fopt: best value found during search, fopt=f(xopt) 42 | 43 | 44 | Required Python packages: 45 | pyDOE: https://pythonhosted.org/pyDOE/ 46 | nlopt: https://nlopt.readthedocs.io (required only if DIRECT solver is used) 47 | pyswarm: https://pythonhosted.org/pyswarm/ (required only if PSO solver is used) 48 | 49 | [1] A. Bemporad, "Global optimization via inverse weighting and radial 50 | basis functions", arXiv:1906.06498v1, June 15, 2019. 51 | https://arxiv.org/pdf/1906.06498.pdf 52 | 53 | """ 54 | 55 | import idwgopt.idwgopt_init as idwgopt_init 56 | from pyswarm import pso # https://pythonhosted.org/pyswarm/ 57 | 58 | from numpy.linalg import svd 59 | 60 | from numpy import zeros, ones, diag, inf 61 | from numpy import where, maximum, exp 62 | from math import sqrt, atan, pi 63 | import contextlib 64 | import io 65 | 66 | def get_rbf_weights(M, F, NX, svdtol): 67 | # Solve M*W = F using SVD 68 | 69 | U, dS, V = svd(M[0:NX, 0:NX]) 70 | ii = where(dS >= svdtol) 71 | ns = max(ii[0]) + 1 72 | W = (V[0:ns, ].T).dot(diag(1 / dS[0:ns].flatten('C')).dot((U[:, 0:ns].T).dot(F[0:NX]))) 73 | 74 | return W 75 | 76 | def facquisition(xx, X, F, N, alpha, delta, dF, W, rbf, useRBF): 77 | # Acquisition function to minimize to get next sample 78 | 79 | #d = sum(((X[0:N, ] - (ones((N, 1)) * xx)) ** 2).T) 80 | 81 | #d = np.sum((X[0:N, ] - xx) ** 2, axis=-1) 82 | d = fast_dist(X[0:N,:], xx) 83 | 84 | ii = where(d < 1e-12) 85 | if ii[0].size > 0: 86 | fhat = F[ii[0]][0] 87 | dhat = 0 88 | else: 89 | w = exp(-d) / d 90 | sw = np.sum(w) 91 | 92 | if useRBF: 93 | v = rbf(X[0:N,:],xx) 94 | fhat = v.ravel().dot(W.ravel()) 95 | # v = zeros((N, 1)) 96 | # for j in range(N): 97 | # v[j] = rbf(X[j,].T, xx) 98 | # fhat = sum(v * W) 99 | else: 100 | fhat = np.sum(F[0:N, ] * w) / sw 101 | 102 | # dhat = (delta * atan(1 / np.sum(1 / d)) * 2 / pi * dF + 103 | # alpha * sqrt(np.sum(w * (F[0:N, ] - fhat).flatten("c") ** 2) / sw)) 104 | dhat = delta * atan(1 / np.sum(1 / d)) * 2 / pi * dF 105 | f_err = (F[0:N, ] - fhat).flatten("c") 106 | f_err_w = w * f_err**2 107 | dhat += alpha * sqrt(np.sum(f_err_w) / sw) 108 | 109 | f = fhat - dhat 110 | 111 | return f 112 | 113 | (f, lb, ub, nvar, Aineq, bineq, g, isLinConstrained, isNLConstrained, 114 | X, F, z, nsamp, maxevals, epsDeltaF, alpha, delta, rhoC, display, svdtol, 115 | dd, d0, useRBF, rbf, M, scalevars, globoptsol, DIRECTopt, 116 | PSOiters, PSOswarmsize) = idwgopt_init.init(prob) 117 | 118 | 119 | time_iter = [] 120 | time_f_eval = [] 121 | time_opt_acquisition = [] 122 | time_fit_surrogate = [] 123 | 124 | for i in range(nsamp): 125 | time_fun_eval_start = time.perf_counter() 126 | F[i] = f(X[i,].T) 127 | time_fun_eval_i = time.perf_counter() - time_fun_eval_start 128 | time_iter.append(time_fun_eval_i) 129 | time_f_eval.append(time_fun_eval_i) 130 | time_opt_acquisition.append(0.0) 131 | time_fit_surrogate.append(0.0) 132 | 133 | if useRBF: 134 | W = get_rbf_weights(M, F, nsamp, svdtol) 135 | else: 136 | W = [] 137 | 138 | fbest = inf 139 | zbest = zeros((nsamp, 1)) 140 | for i in range(nsamp): 141 | isfeas = True 142 | if isLinConstrained: 143 | isfeas = isfeas and all(Aineq.dot(X[i,].T) <= bineq.flatten("c")) 144 | if isNLConstrained: 145 | isfeas = isfeas and all(g(X[i,]) <= 0) 146 | if isfeas and fbest > F[i]: 147 | fbest = F[i] 148 | zbest = X[i,] 149 | 150 | Fmax = max(F[0:nsamp]) 151 | Fmin = min(F[0:nsamp]) 152 | 153 | N = nsamp 154 | 155 | while N < maxevals: 156 | 157 | time_iter_start = time.perf_counter() 158 | 159 | dF = Fmax - Fmin 160 | 161 | if isLinConstrained or isNLConstrained: 162 | penalty = rhoC * dF 163 | if isLinConstrained and isNLConstrained: 164 | constrpenalty = lambda x: (penalty * (sum(maximum((Aineq.dot(x) - bineq).flatten("c"), 0) ** 2) 165 | + sum(maximum(g(x), 0) ** 2))) 166 | elif isLinConstrained and not isNLConstrained: 167 | constrpenalty = lambda x: penalty * (sum(maximum((Aineq.dot(x) - bineq).flatten("c"), 0) ** 2)) 168 | elif not isLinConstrained and isNLConstrained: 169 | constrpenalty = lambda x: penalty * sum(maximum(g(x), 0) ** 2) 170 | else: 171 | constrpenalty = lambda x: 0 172 | 173 | acquisition = lambda x: (facquisition(x, X, F, N, alpha, delta, dF, W, rbf, useRBF) 174 | + constrpenalty(x)) 175 | 176 | time_opt_acq_start = time.perf_counter() 177 | if globoptsol == "pswarm": 178 | # pso(func, lb, ub, ieqcons=[], f_ieqcons=None, args=(), kwargs={}, 179 | # swarmsize=100, omega=0.5, phip=0.5, phig=0.5, maxiter=100, minstep=1e-8, 180 | # minfunc=1e-8, debug=False) 181 | if display == 0: 182 | with contextlib.redirect_stdout(io.StringIO()): 183 | z, cost = pso(acquisition, lb, ub, swarmsize=PSOswarmsize, 184 | minfunc=dF * 1e-8, maxiter=PSOiters) 185 | else: 186 | z, cost = pso(acquisition, lb, ub, swarmsize=PSOswarmsize, 187 | minfunc=dF * 1e-8, maxiter=PSOiters) 188 | 189 | elif globoptsol == "direct": 190 | DIRECTopt.set_min_objective(lambda x, grad: acquisition(x)[0]) 191 | z = DIRECTopt.optimize(z.flatten("c")) 192 | time_opt_acquisition.append(time.perf_counter() - time_opt_acq_start) 193 | 194 | time_fun_eval_start = time.perf_counter() 195 | fz = f(z) # function evaluation 196 | time_f_eval.append(time.perf_counter() - time_fun_eval_start) 197 | 198 | N = N + 1 199 | 200 | X[N - 1,] = z.T 201 | F[N - 1] = fz 202 | 203 | Fmax = max(Fmax, fz) 204 | Fmin = min(Fmin, fz) 205 | 206 | time_fit_surrogate_start = time.perf_counter() 207 | if useRBF: 208 | # Just update last row and column of M 209 | for h in range(N): 210 | mij = rbf(X[h,], X[N - 1,]) 211 | M[h, N - 1] = mij 212 | M[N - 1, h] = mij 213 | 214 | W = get_rbf_weights(M, F, N, svdtol) 215 | time_fit_surrogate.append(time.perf_counter() - time_fit_surrogate_start) 216 | 217 | if fbest > fz: 218 | fbest = fz.copy() 219 | zbest = z.copy() 220 | 221 | if display > 0: 222 | 223 | print("N = %4d, cost = %8g, best = %8g" % (N, fz, fbest)) 224 | 225 | string = "" 226 | for j in range(nvar): 227 | aux = zbest[j] 228 | if scalevars: 229 | aux = aux * dd[j] + d0[j] 230 | 231 | string = string + " x" + str(j + 1) + " = " + str(aux) 232 | print(string) 233 | 234 | time_iter.append(time.perf_counter() - time_iter_start) 235 | 236 | # end while 237 | 238 | xopt = zbest.copy() 239 | if scalevars: 240 | # Scale variables back 241 | xopt = xopt * dd + d0 242 | X = X * (ones((N, 1)) * dd) + ones((N, 1)) * d0 243 | 244 | fopt = fbest.copy() 245 | 246 | if not useRBF: 247 | W = [] 248 | 249 | out = {"xopt": xopt, 250 | "fopt": fopt, 251 | "X": X, 252 | "F": F, 253 | "W": W, 254 | 'time_iter': np.array(time_iter), 255 | 'time_opt_acquisition': np.array(time_opt_acquisition), 256 | 'time_fit_surrogate': np.array(time_fit_surrogate), 257 | 'time_f_eval': np.array(time_f_eval)} 258 | 259 | return out 260 | 261 | 262 | def default(nvars): 263 | """ Generate default problem structure for IDW-RBF Global Optimization. 264 | 265 | problem=idwgopt.default(n) generate a default problem structure for a 266 | an optimization with n variables. 267 | 268 | (C) 2019 by A. Bemporad. 269 | """ 270 | 271 | import idwgopt.idwgopt_default as idwgopt_default 272 | problem = idwgopt_default.set(nvars) 273 | 274 | return problem 275 | -------------------------------------------------------------------------------- /idwgopt/idwgopt_default.py: -------------------------------------------------------------------------------- 1 | def set(nvars): 2 | """ 3 | Generate default problem structure for IDW-RBF Global Optimization. 4 | 5 | problem = idwgopt_default.set(n) generate a default problem structure for a 6 | an optimization with n variables. 7 | 8 | (C) 2019 A. Bemporad, July 6, 2019 9 | """ 10 | 11 | from numpy import zeros, ones 12 | from numpy import sum as vecsum 13 | 14 | problem = { 15 | "f": "[set cost function here]", # cost function to minimize 16 | "lb": -1 * ones((nvars, 1)), # lower bounds on the optimization variables 17 | "ub": 1 * ones((nvars, 1)), # upper bounds on the optimization variables 18 | "maxevals": 20, # maximum number of function evaluations 19 | "alpha": 1, # weight on function uncertainty variance measured by IDW 20 | "delta": 0.5, # weight on distance from previous samples 21 | "nsamp": 2*nvars, # number of initial samples 22 | "useRBF": 1, # 1 = use RBFs, 0 = use IDW interpolation 23 | "rbf": lambda x1,x2: 1/(1+0.25*vecsum((x1-x2)**2)), # inverse quadratic 24 | # RBF function (only used if useRBF=1) 25 | "scalevars": 1, # scale problem variables 26 | "svdtol": 1e-6, # tolerance used to discard small singular values 27 | "Aineq": zeros((0,nvars)), # matrix A defining linear inequality constraints 28 | "bineq": zeros((0,1)), # right hand side of constraints A*x <= b 29 | "g": 0, # constraint function. Example: problem.g = lambda x: x[0]**2+x[1]**2-1 30 | "shrink_range": 1, # flag. If 0, disable shrinking lb and ub to bounding box of feasible set 31 | "constraint_penalty": 1000, # penalty term on violation of linear inequality 32 | # and nonlinear constraint 33 | "feasible_sampling": False, # if True, initial samples are forced to be feasible 34 | "globoptsol": "direct", # nonlinear solver used during acquisition. 35 | # interfaced solvers are: 36 | # "direct" DIRECT from NLopt tool (nlopt.readthedocs.io) 37 | # "pswarm" PySwarm solver (pythonhosted.org/pyswarm/) 38 | "display": 0, # verbosity level (0=minimum) 39 | "PSOiters": 500, # number of iterations in PSO solver 40 | "PSOswarmsize": 20, # swarm size in PSO solver 41 | "epsDeltaF": 1e-4 # minimum value used to scale the IDW distance function 42 | } 43 | 44 | return problem 45 | -------------------------------------------------------------------------------- /idwgopt/idwgopt_init.py: -------------------------------------------------------------------------------- 1 | def init(prob): 2 | """ 3 | Init function for idwgopt.py 4 | 5 | (C) 2019 A. Bemporad, July 6, 2019 6 | """ 7 | 8 | from pyDOE import lhs #https://pythonhosted.org/pyDOE/ 9 | # import nlopt # https://nlopt.readthedocs.io 10 | from pyswarm import pso # https://pythonhosted.org/pyswarm/ 11 | 12 | from scipy.optimize import linprog as linprog 13 | 14 | from numpy import size, zeros, ones, diag 15 | from numpy import where, maximum 16 | from math import ceil 17 | import sys 18 | import contextlib 19 | import io 20 | 21 | 22 | # input arguments 23 | f0 = prob["f"] 24 | lb = prob["lb"].copy() 25 | ub = prob["ub"].copy() 26 | maxevals = prob["maxevals"] 27 | alpha = prob["alpha"] 28 | delta = prob["delta"] 29 | nsamp = prob["nsamp"] 30 | useRBF = prob["useRBF"] 31 | rbf = prob["rbf"] 32 | scalevars = prob["scalevars"] 33 | svdtol = prob["svdtol"] 34 | Aineq = prob["Aineq"].copy() 35 | bineq = prob["bineq"].copy() 36 | g0 = prob["g"] 37 | shrink_range = prob["shrink_range"] 38 | rhoC = prob["constraint_penalty"] 39 | feasible_sampling = prob["feasible_sampling"] 40 | globoptsol = prob["globoptsol"] 41 | display = prob["display"] 42 | PSOiters = prob["PSOiters"] 43 | PSOswarmsize = prob["PSOswarmsize"] 44 | epsDeltaF = prob["epsDeltaF"] 45 | 46 | nvar = size(lb) # number of optimization variables 47 | 48 | isLinConstrained = (size(bineq) > 0) 49 | isNLConstrained = (g0 != 0) 50 | if not isLinConstrained and not isNLConstrained: 51 | feasible_sampling = False 52 | 53 | f = f0 54 | g = g0 55 | if scalevars: 56 | # Rescale problem variables in [-1,1] 57 | dd = (ub-lb)/2 58 | d0 = (ub+lb)/2 59 | f = lambda x: f0(x*dd+d0) 60 | 61 | lb = -ones(nvar) 62 | ub = ones(nvar) 63 | 64 | if isLinConstrained: 65 | bineq = bineq-Aineq.dot(d0) 66 | Aineq = Aineq.dot(diag(dd.flatten('C'))) 67 | 68 | if isNLConstrained: 69 | g = lambda x: g0(x*dd+d0) 70 | 71 | # set solver options 72 | if globoptsol=="pswarm": 73 | # nothing to do 74 | pass 75 | DIRECTopt = [] 76 | 77 | elif globoptsol=="direct": 78 | #DIRECTopt = nlopt.opt(nlopt.GN_DIRECT_L, 2) 79 | DIRECTopt = nlopt.opt(nlopt.GN_DIRECT, 2) 80 | DIRECTopt.set_lower_bounds(lb.flatten("C")) 81 | DIRECTopt.set_upper_bounds(ub.flatten("C")) 82 | DIRECTopt.set_ftol_abs(1e-5) 83 | DIRECTopt.set_maxeval(2000) 84 | DIRECTopt.set_xtol_rel(1e-5) 85 | 86 | else: 87 | print("Unknown solver") 88 | sys.exit(1) 89 | 90 | 91 | if shrink_range == 1: 92 | # possibly shrink lb,ub to constraints 93 | if not isNLConstrained and isLinConstrained: 94 | flin=zeros((nvar,1)) 95 | 96 | for i in range(nvar): 97 | flin[i]=1 98 | res=linprog(flin, Aineq, bineq, bounds=(None,None)) 99 | aux=max(lb[i],res.fun) 100 | lb[i]=aux 101 | flin[i]=-1 102 | res=linprog(flin, Aineq, bineq, bounds=(None,None)) 103 | aux=min(ub[i],-res.fun) 104 | ub[i]=aux 105 | flin[i]=0 106 | 107 | elif isNLConstrained: 108 | NLpenaltyfun = lambda x: sum(maximum(g(x),0)**2) 109 | if isLinConstrained: 110 | LINpenaltyfun = lambda x: sum(maximum((Aineq.dot(x)-bineq).flatten("c"),0)**2) 111 | else: 112 | LINpenaltyfun = lambda x: 0 113 | 114 | for i in range(0,nvar): 115 | obj_fun = lambda x: x[i] + 1.0e4*(NLpenaltyfun(x) + LINpenaltyfun(x)) 116 | if globoptsol=="pswarm": 117 | if display == 0: 118 | with contextlib.redirect_stdout(io.StringIO()): 119 | z, cost = pso(obj_fun, lb, ub, swarmsize=30, 120 | minfunc=1e-8, maxiter=2000) 121 | else: 122 | z, cost = pso(obj_fun, lb, ub, swarmsize=30, 123 | minfunc=1e-8, maxiter=2000) 124 | else: # globoptsol=="direct": 125 | DIRECTopt.set_min_objective(lambda x,grad: obj_fun(x)[0]) 126 | z = DIRECTopt.optimize(z.flatten("c")) 127 | lb[i] = max(lb[i],z[i]) 128 | 129 | obj_fun = lambda x: -x[i] + 1.0e4*(NLpenaltyfun(x) + LINpenaltyfun(x)) 130 | 131 | if globoptsol=="pswarm": 132 | if display == 0: 133 | with contextlib.redirect_stdout(io.StringIO()): 134 | z, cost = pso(obj_fun, lb, ub, swarmsize=30, 135 | minfunc=1e-8, maxiter=2000) 136 | else: 137 | z, cost = pso(obj_fun, lb, ub, swarmsize=30, 138 | minfunc=1e-8, maxiter=2000) 139 | else: # globoptsol=="direct": 140 | DIRECTopt.set_min_objective(lambda x,grad: obj_fun(x)[0]) 141 | z = DIRECTopt.optimize(z.flatten("c")) 142 | ub[i] = min(ub[i],z[i]) 143 | 144 | if maxevals x[k+1|k] 123 | self.y = self.C @ self.x #+ self.D @ u 124 | 125 | def sim(self, u_seq, x=None): 126 | 127 | if x is None: 128 | x = self.x 129 | Np = __first_dim__(u_seq) 130 | nu = __second_dim__(u_seq) 131 | assert(nu == self.nu) 132 | 133 | y = np.zeros((Np,self.ny)) 134 | x_tmp = x 135 | for i in range(Np): 136 | u_tmp = u_seq[i] 137 | y[i,:] = self.C @ x_tmp + self.D @ u_tmp 138 | x_tmp = self.A @ x_tmp + self.B @ u_tmp 139 | 140 | #y[Np] = self.C @ x_tmp + self.D @ u_tmp # not really true for D. Here it is 0 anyways 141 | return y 142 | 143 | 144 | if __name__ == '__main__': 145 | 146 | # Constants # 147 | Ts = 0.2 # sampling time (s) 148 | M = 2 # mass (Kg) 149 | b = 0.3 # friction coefficient (N*s/m) 150 | 151 | Ad = np.array([ 152 | [1.0, Ts], 153 | [0, 1.0 -b/M*Ts] 154 | ]) 155 | 156 | Bd = np.array([ 157 | [0.0], 158 | [Ts/M]]) 159 | 160 | Cd = np.array([[1, 0]]) 161 | Dd = np.array([[0]]) 162 | 163 | [nx, nu] = Bd.shape # number of states and number or inputs 164 | ny = np.shape(Cd)[0] 165 | 166 | ## General design ## 167 | Bd_kal = np.hstack([Bd, Bd]) 168 | Dd_kal = np.array([[0, 0]]) 169 | Q_kal = np.array([[100]]) # nw x nw matrix, w general (here, nw = nu) 170 | R_kal = np.eye(ny) # ny x ny) 171 | L_general,P_general,W_general = kalman_design(Ad, Bd_kal, Cd, Dd_kal, Q_kal, R_kal, type='predictor') 172 | 173 | # Simple design 174 | Q_kal = 10 * np.eye(nx) 175 | R_kal = np.eye(ny) 176 | L_simple,P_simple,W_simple = kalman_design_simple(Ad, Bd, Cd, Dd, Q_kal, R_kal, type='predictor') 177 | 178 | # Simple design written in general form 179 | Bd_kal = np.hstack([Bd, np.eye(nx)]) 180 | Dd_kal = np.hstack([Dd, np.zeros((ny, nx))]) 181 | Q_kal = 10 * np.eye(nx)#np.eye(nx) * 100 182 | R_kal = np.eye(ny) * 1 183 | L_gensim,P_gensim,W_gensim = kalman_design_simple(Ad, Bd, Cd, Dd, Q_kal, R_kal, type='predictor') 184 | 185 | assert(np.isclose(L_gensim[0], L_simple[0])) 186 | 187 | L, _, _ = kalman_design_simple(Ad, Bd, Cd, Dd, Q_kal, R_kal, type='predictor') 188 | x0 = np.zeros(nx) 189 | KF = LinearStateEstimator(x0, Ad, Bd, Cd, Dd, L) 190 | KF.L = L 191 | 192 | L_predictor = Ad@P_simple@np.transpose(Cd)/([Cd@P_simple@np.transpose(Cd)+R_kal]) 193 | L_filter = P_simple@np.transpose(Cd)/([Cd@P_simple@np.transpose(Cd)+R_kal]) 194 | -------------------------------------------------------------------------------- /objective_function.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pendulum_MPC_sim import simulate_pendulum_MPC, get_parameter, Ts_fast, get_default_parameters 3 | from scipy import sparse 4 | from pendulum_model import * 5 | 6 | N_eval = 0 7 | 8 | VAR_NAMES = ['QDu_scale', 9 | 'Qy11', 10 | 'Qy22', 11 | 'Np', 12 | 'Nc_perc', 13 | 'Ts_MPC', 14 | 'QP_eps_abs_log', 15 | 'QP_eps_rel_log', 16 | 'Q_kal_11', 17 | 'Q_kal_22', 18 | 'Q_kal_33', 19 | 'Q_kal_44', 20 | 'R_kal_11', 21 | 'R_kal_22', 22 | ] 23 | 24 | 25 | def dict_to_x(dict_x): 26 | N_vars1 = len(dict_x) 27 | N_vars2 = len(VAR_NAMES) 28 | assert (N_vars1 == N_vars2) 29 | N_vars = N_vars1 30 | 31 | x = np.zeros(N_vars) 32 | for var_idx in range(N_vars): 33 | x[var_idx] = dict_x[VAR_NAMES[var_idx]] 34 | return x 35 | 36 | 37 | def x_to_dict(x): 38 | if len(x.shape) == 2: 39 | x = x[0] 40 | N_vars1 = len(x) 41 | N_vars2 = len(VAR_NAMES) 42 | assert (N_vars1 == N_vars2) 43 | N_vars = N_vars1 44 | 45 | dict_x = {} 46 | for var_idx in range(N_vars): 47 | dict_x[VAR_NAMES[var_idx]] = x[var_idx] 48 | return dict_x 49 | 50 | 51 | def get_simoptions_x(x): 52 | so = x_to_dict(x) # simopt 53 | 54 | # MPC cost: weight matrices 55 | Qx = sparse.diags([so['Qy11'], 0, so['Qy22'], 0]) # /sum_MPC_Qy # Quadratic cost for states x0, x1, ..., x_N-1 56 | QxN = Qx 57 | QDu = so['QDu_scale'] * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 58 | 59 | so['Qx'] = Qx 60 | so['QxN'] = QxN 61 | so['Qu'] = 0.0 * sparse.eye(1) 62 | so['QDu'] = QDu 63 | 64 | # MPC cost: prediction and control horizon 65 | so['Np'] = np.int(round(so['Np'])) 66 | so['Nc'] = np.int(round(so['Np'] * so['Nc_perc'])) 67 | 68 | # MPC cost: sample time 69 | # Make Ts_MPC a multiple of Ts_fast 70 | Ts_MPC = so['Ts_MPC'] 71 | Ts_MPC = ((Ts_MPC // Ts_fast)) * Ts_fast # make Ts_MPC an integer multiple of Ts_fast 72 | so['Ts_MPC'] = Ts_MPC 73 | 74 | # MPC: solver settings 75 | so['QP_eps_abs'] = 10 ** so['QP_eps_abs_log'] 76 | so['QP_eps_rel'] = 10 ** so['QP_eps_rel_log'] 77 | 78 | # Kalman filter: matrices 79 | Q_kal = np.diag([so['Q_kal_11'], so['Q_kal_22'], so['Q_kal_33'], so['Q_kal_44']]) 80 | R_kal = np.diag([so['R_kal_11'], so['R_kal_22']]) 81 | 82 | so['Q_kal'] = Q_kal 83 | so['R_kal'] = R_kal 84 | 85 | # Fixed simulation settings 86 | so['std_nphi'] = 0.01 87 | so['std_npos'] = 0.02 88 | 89 | so['std_dF'] = 0.1 90 | so['w_F'] = 5 91 | 92 | return so 93 | 94 | 95 | def f_x(x, eps_calc=1.0, seed_val=None): 96 | global N_eval 97 | 98 | if seed_val is None: 99 | seed_val = N_eval 100 | 101 | simoptions = get_simoptions_x(x) 102 | simoptions['seed_val'] = seed_val 103 | 104 | sim_failed = False 105 | try: 106 | simout = simulate_pendulum_MPC(simoptions) 107 | except ValueError as e: 108 | print(e) 109 | sim_failed = True 110 | 111 | if not sim_failed: 112 | t = simout['t'] 113 | y_meas = simout['y_meas'] 114 | x_ref = simout['x_ref'] 115 | p_meas = y_meas[:, 0] 116 | phi_meas = y_meas[:, 1] 117 | 118 | p_ref = x_ref[:, 0] 119 | phi_ref = x_ref[:, 2] 120 | 121 | J_perf = 10 * np.mean(np.abs(p_ref - p_meas)) + 0.0 * np.max(np.abs(p_ref - p_meas)) + \ 122 | 30 * np.mean(np.abs(np.abs(phi_ref - phi_meas))) # + 15*np.max(np.abs(np.abs(phi_ref - phi_meas))) 123 | 124 | # Computation of the barrier function 125 | t_calc = simout['t_calc'] 126 | eps_margin = 0.8 127 | 128 | t_calc = eps_calc * t_calc 129 | t_calc_wc = np.max(t_calc) # worst-case computational cost (max computational time) 130 | 131 | Ts_MPC = simout['Ts_MPC'] 132 | t_available = Ts_MPC * eps_margin 133 | 134 | delay_wc = (t_calc_wc - t_available) 135 | delay_wc = delay_wc * (delay_wc >= 0) 136 | J_calc = (delay_wc / t_available) * 1e3 137 | 138 | emergency = simout['emergency_fast'] 139 | emergency_time, _ = np.where(emergency > 0) 140 | if len(emergency_time) > 0: 141 | J_emergency = (len(emergency) - emergency_time[0]) / len(emergency) * 1e3 142 | else: 143 | J_emergency = 0.0 144 | 145 | else: 146 | J_perf = 1e3 147 | J_calc = 1e3 148 | J_emergency = 1e3 # (len(emergency) - emergency_time[0]) / len(emergency) * 1e3 149 | # J_perf = 2e1 150 | # J_fit = 2e1 151 | 152 | J_cl = np.log(J_perf) + np.log(1 + J_calc) + np.log(1+J_emergency) 153 | 154 | print(f"N_eval: {N_eval}, J_perf:{J_perf:.2f}, J_calc:{J_calc:.2f}, J_emergency:{J_emergency:.2f}, J_cl:{J_cl:.2f}") 155 | N_eval += 1 156 | 157 | return J_cl # + J_fit 158 | 159 | 160 | if __name__ == '__main__': 161 | 162 | import matplotlib.pyplot as plt 163 | 164 | dict_x0 = { 165 | 166 | 'QDu_scale': 0.001, 167 | 'Qy11': 0.1, 168 | 'Qy22': 0.5, 169 | 'Np': 100, 170 | 'Nc_perc': 0.5, 171 | 'Ts_MPC': 10e-3, 172 | 'QP_eps_abs_log': -3, 173 | 'QP_eps_rel_log': -3, 174 | 'Q_kal_11': 0.1, 175 | 'Q_kal_22': 0.9, 176 | 'Q_kal_33': 0.1, 177 | 'Q_kal_44': 0.9, 178 | 'R_kal_11': 0.5, 179 | 'R_kal_22': 0.5 180 | } 181 | x0 = dict_to_x(dict_x0) 182 | 183 | f_x0 = x_to_dict(x0) 184 | J_tot = f_x(x0) 185 | 186 | simopt = get_simoptions_x(x0) 187 | simout = simulate_pendulum_MPC(simopt) 188 | 189 | t = simout['t'] 190 | x = simout['x'] 191 | u = simout['u'] 192 | y = simout['y'] 193 | y_meas = simout['y_meas'] 194 | x_ref = simout['x_ref'] 195 | x_MPC_pred = simout['x_MPC_pred'] 196 | x_fast = simout['x_fast'] 197 | x_ref_fast = simout['x_ref_fast'] 198 | y_ref = x_ref[:, [0, 2]] # on-line predictions from the Kalman Filter 199 | uref = get_parameter({}, 'uref') 200 | 201 | fig, axes = plt.subplots(3, 1, figsize=(10, 10)) 202 | axes[0].plot(t, y_meas[:, 0], "b", label='p_meas') 203 | axes[0].plot(t, y[:, 0], "k", label='p') 204 | axes[0].plot(t, y_ref[:, 0], "k--", label="p_ref") 205 | axes[0].set_title("Position (m)") 206 | 207 | axes[1].plot(t, y_meas[:, 1] * RAD_TO_DEG, "b", label='phi_meas') 208 | axes[1].plot(t, y[:, 1] * RAD_TO_DEG, 'k', label="phi") 209 | axes[1].plot(t, y_ref[:, 1] * RAD_TO_DEG, "k--", label="phi_ref") 210 | axes[1].set_title("Angle (deg)") 211 | 212 | axes[2].plot(t, u[:, 0], label="u") 213 | axes[2].plot(t, uref * np.ones(np.shape(t)), "r--", label="u_ref") 214 | axes[2].set_title("Force (N)") 215 | 216 | for ax in axes: 217 | ax.grid(True) 218 | ax.legend() 219 | 220 | default = get_default_parameters(simopt) 221 | 222 | t_calc = simout['t_calc'] 223 | fig, ax = plt.subplots(1, 1, figsize=(5, 5)) 224 | plt.hist(t_calc * 1e3) 225 | plt.title("Computation time (ms)") 226 | 227 | t_int = simout['t_int_fast'] 228 | t_fast = simout['t_fast'] 229 | u_fast = simout['u_fast'] 230 | 231 | # MPC time check 232 | # In[MPC computation time ] 233 | fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True) 234 | axes[0].plot(t, y_meas[:, 0], "b", label='p_meas') 235 | axes[0].plot(t_fast, x_fast[:, 0], "k", label='p') 236 | axes[0].step(t, y_ref[:, 0], "k--", where='post', label="p_ref") 237 | axes[0].set_ylim(-0.2, 1.0) 238 | axes[0].set_xlabel("Simulation time (s)") 239 | axes[0].set_ylabel("Position (m)") 240 | 241 | axes[1].step(t, t_calc[:, 0] * 1e3, "b", where='post', label='T_MPC') 242 | axes[1].set_xlabel("Simulation time (s)") 243 | axes[1].set_ylabel("MPC time (ms)") 244 | axes[1].set_ylim(0, 4) 245 | axes[2].step(t_fast[1:], t_int[1:, 0] * 1e3, "b", where='post', label='T_ODE') # why is 1st slow? check numba 246 | axes[2].set_xlabel("Simulation time (s)") 247 | axes[2].set_ylabel("ODE time (ms)") 248 | axes[2].set_ylim(0, 0.3) 249 | axes[3].step(t, u[:, 0], where='post', label="F") 250 | axes[3].step(t_fast, u_fast[:, 0], where='post', label="F_d") 251 | axes[3].set_xlabel("Simulation time (s)") 252 | axes[3].set_ylabel("Force (N)") 253 | 254 | for ax in axes: 255 | ax.grid(True) 256 | ax.legend() 257 | -------------------------------------------------------------------------------- /paper/fig/BEST_GLIS_PC.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/BEST_GLIS_PC.pdf -------------------------------------------------------------------------------- /paper/fig/BEST_GLIS_PI.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/BEST_GLIS_PI.pdf -------------------------------------------------------------------------------- /paper/fig/ITER_GLIS_PC.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/ITER_GLIS_PC.pdf -------------------------------------------------------------------------------- /paper/fig/ITER_GLIS_PC_500.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/ITER_GLIS_PC_500.pdf -------------------------------------------------------------------------------- /paper/fig/ITER_GLIS_PI_500.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/ITER_GLIS_PI_500.pdf -------------------------------------------------------------------------------- /paper/fig/MPC_CPUTIME_GLIS_PI.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/MPC_CPUTIME_GLIS_PI.pdf -------------------------------------------------------------------------------- /paper/fig/cart_pole.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/fig/cart_pole.pdf -------------------------------------------------------------------------------- /paper/fig/sym/F.svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/fig/sym/elle.svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/fig/sym/elleM.svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/fig/sym/m (Case Conflict).svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/fig/sym/m.svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/fig/sym/p.svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/fig/sym/preview.svg: -------------------------------------------------------------------------------- 1 | 2 | 16 | -------------------------------------------------------------------------------- /paper/ms.bib: -------------------------------------------------------------------------------- 1 | @article{Jon09, 2 | title={{DIRECT} global optimization algorithm}, 3 | author={D.R. Jones}, 4 | journal={Encyclopedia of Optimization}, 5 | pages={725--735}, 6 | year={2009}, 7 | } 8 | 9 | @article{HN99, 10 | title={Global optimization by multilevel coordinate search}, 11 | author={W. Huyer and A. Neumaier}, 12 | journal={Journal of Global Optimization}, 13 | volume={14}, 14 | number={4}, 15 | pages={331--355}, 16 | year={1999}, 17 | } 18 | 19 | 20 | @incollection{Han06, 21 | title={The {CMA} evolution strategy: a comparing review}, 22 | author={N. Hansen}, 23 | booktitle={Towards a new evolutionary computation}, 24 | pages={75--102}, 25 | year={2006}, 26 | } 27 | 28 | @inproceedings{EK95, 29 | title={A new optimizer using particle swarm theory}, 30 | author={R. Eberhart and J. Kennedy}, 31 | booktitle={Proceedings of the Sixth International Symposium on Micro Machine and Human Science}, 32 | pages={39--43}, 33 | year={1995}, 34 | address={Nagoya}, 35 | } 36 | 37 | @article{VV09, 38 | author={A.I.F. Vaz and L.N. Vicente}, 39 | title={{PSwarm}: A hybrid solver for linearly constrained global derivative-free optimization}, 40 | journal={Optimization Methods and Software}, 41 | volume=24, 42 | year=2009, 43 | pages={669--685}, 44 | note={\url{http://www.norg.uminho.pt/aivaz/pswarm/}}, 45 | } 46 | 47 | @article{Ken10, 48 | title={Particle swarm optimization}, 49 | author={J. Kennedy}, 50 | journal={Encyclopedia of Machine Learning}, 51 | pages={760--766}, 52 | year={2010}, 53 | } 54 | 55 | @article{SWMW89, 56 | title={Design and analysis of computer experiments}, 57 | author={J. Sacks and W.J. Welch and T.J. Mitchell and H.P. Wynn}, 58 | journal={Statistical Science}, 59 | pages={409--423}, 60 | year={1989}, 61 | } 62 | 63 | @article{CB17, 64 | author ={G. Cimini and A. Bemporad}, 65 | title ={Exact Complexity Certification of Active-Set 66 | Methods for Quadratic Programming}, 67 | journal =ieeetac, 68 | year = 2017, 69 | volume =62, 70 | number=12, 71 | pages = {6094--6109}, 72 | } 73 | @article{bemporad2002explicit, 74 | title={The explicit linear quadratic regulator for constrained systems}, 75 | author={Bemporad, Alberto and Morari, Manfred and Dua, Vivek and Pistikopoulos, Efstratios N}, 76 | journal={Automatica}, 77 | volume={38}, 78 | number={1}, 79 | pages={3--20}, 80 | year={2002}, 81 | publisher={Elsevier} 82 | } 83 | 84 | @inproceedings{bemporad2006model, 85 | title={Model predictive control design: New trends and tools}, 86 | author={Bemporad, Alberto}, 87 | booktitle={Proceedings of the 45th IEEE Conference on Decision and Control}, 88 | pages={6678--6683}, 89 | year={2006} 90 | } 91 | 92 | @book{borrelli2017predictive, 93 | title={Predictive control for linear and hybrid systems}, 94 | author={Borrelli, Francesco and Bemporad, Alberto and Morari, Manfred}, 95 | year={2017}, 96 | publisher={Cambridge University Press} 97 | } 98 | 99 | 100 | @article{FoPiToSa2016, 101 | year = {2016}, 102 | author = {Formentin, S. and Piga, D. and T\'oth, R. and Saveresi, S. M.}, 103 | title = {Direct learning of {LPV} controllers from data}, 104 | journal = {Automatica}, 105 | volume = {65}, 106 | pages = {98--110}, 107 | } 108 | 109 | @article{PiFoBe2017, 110 | author = {Piga, D. and Formentin, S. and Bemporad, A.}, 111 | title = {Direct data-driven control of constrained systems}, 112 | journal = {IEEE Transactions on Control Systems Technology}, 113 | volume={25}, 114 | number={4}, 115 | pages={331--351}, 116 | year={2018}, 117 | } 118 | 119 | @article{shahriari2016taking, 120 | title={Taking the human out of the loop: A review of bayesian optimization}, 121 | author={Shahriari, Bobak and Swersky, Kevin and Wang, Ziyu and Adams, Ryan P and De Freitas, Nando}, 122 | journal={Proceedings of the IEEE}, 123 | volume={104}, 124 | number={1}, 125 | pages={148--175}, 126 | year={2016}, 127 | publisher={IEEE} 128 | } 129 | 130 | @article{brochu2010tutorial, 131 | title={A tutorial on {Bayesian} optimization of expensive cost functions, with application to active user modeling and hierarchical reinforcement learning}, 132 | author={Brochu, E. and Cora, V. M. and {De Freitas}, N.}, 133 | journal={arXiv preprint arXiv:1012.2599}, 134 | year={2010} 135 | } 136 | 137 | 138 | @inproceedings{bansal2017goal, 139 | title={Goal-driven dynamics learning via bayesian optimization}, 140 | author={Bansal, Somil and Calandra, Roberto and Xiao, Ted and Levine, Sergey and Tomlin, Claire J}, 141 | booktitle={IEEE 56th Annual Conference on Decision and Control}, 142 | pages={5168--5173}, 143 | year={2017} 144 | } 145 | 146 | @article{piga2019performance, 147 | title={Performance-oriented model learning for data-driven {MPC} design}, 148 | author={Piga, Dario and Forgione, Marco and Formentin, Simone and Bemporad, Alberto}, 149 | journal={IEEE Control Systems Letters}, 150 | volume={3}, 151 | number={3}, 152 | pages={577--582}, 153 | year={2019}, 154 | publisher={IEEE} 155 | } 156 | 157 | @inproceedings{stellato2018osqp, 158 | title={{OSQP}: An operator splitting solver for quadratic programs}, 159 | author={Stellato, Bartolomeo and Banjac, Goran and Goulart, Paul and Bemporad, Alberto and Boyd, Stephen}, 160 | booktitle={2018 UKACC 12th International Conference on Control (CONTROL)}, 161 | note={\url{https://osqp.org}}, 162 | pages={339--339}, 163 | year={2018} 164 | } 165 | 166 | @article{bemporad2019global, 167 | title={Global optimization via inverse distance weighting}, 168 | author={Bemporad, Alberto}, 169 | journal={arXiv preprint arXiv:1906.06498}, 170 | note={\url{http://cse.lab.imtlucca.it/~bemporad/glis/}}, 171 | year={2019} 172 | } 173 | 174 | @article{bombois2006least, 175 | title={Least costly identification experiment for control}, 176 | author={Bombois, Xavier and Scorletti, G{\'e}rard and Gevers, Michel and Van den Hof, Paul MJ and Hildebrand, Roland}, 177 | journal={Automatica}, 178 | volume={42}, 179 | number={10}, 180 | pages={1651--1662}, 181 | year={2006}, 182 | publisher={Elsevier} 183 | } 184 | 185 | 186 | @inproceedings{roveda2019control, 187 | title={Control Parameters Tuning Based on Bayesian Optimization for Robot Trajectory Tracking}, 188 | author={Roveda, Loris and Forgione, Marco and Piga, Dario}, 189 | booktitle={2019 Workshop on Learning for Industry 4.0 (ICRA)}, 190 | pages={902--907}, 191 | year={2019} 192 | } 193 | 194 | @inproceedings{driess2017constrained, 195 | title={Constrained bayesian optimization of combined interaction force/task space controllers for manipulations}, 196 | author={Drie{\ss}, Danny and Englert, Peter and Toussaint, Marc}, 197 | booktitle={2017 IEEE International Conference on Robotics and Automation (ICRA)}, 198 | pages={902--907}, 199 | year={2017} 200 | } 201 | 202 | @inproceedings{koller2018learning, 203 | title={Learning-based Model Predictive Control for Safe Exploration}, 204 | author={Koller, Torsten and Berkenkamp, Felix and Turchetta, Matteo and Krause, Andreas}, 205 | booktitle={2018 IEEE Conference on Decision and Control (CDC)}, 206 | pages={6059--6066}, 207 | year={2018} 208 | } 209 | 210 | @article{mckay1979comparison, 211 | title={Comparison of three methods for selecting values of input variables in the analysis of output from a computer code}, 212 | author={McKay, Michael D and Beckman, Richard J and Conover, William J}, 213 | journal={Technometrics}, 214 | volume={21}, 215 | number={2}, 216 | pages={239--245}, 217 | year={1979}, 218 | publisher={Taylor \& Francis} 219 | } 220 | 221 | @inproceedings{forgione2020efficient, 222 | title={{E}fficient {C}alibration of {E}mbedded {MPC}}, 223 | author={Forgione, Marco and Piga, Dario and Bemporad, Alberto}, 224 | booktitle={Proc. of the 21st IFAC World Congress, Berlin, Germany}, 225 | year={2020} 226 | } 227 | -------------------------------------------------------------------------------- /paper/ms.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/paper/ms.pdf -------------------------------------------------------------------------------- /paper/ms.tex: -------------------------------------------------------------------------------- 1 | \documentclass{article} 2 | \usepackage{graphicx} % include this line if your document contains figures 3 | \usepackage[authoryear]{natbib} 4 | \bibliographystyle{abbrvnat} 5 | \setcitestyle{authoryear} 6 | %\usepackage{color} 7 | %\usepackage{algorithm} 8 | %\usepackage{multirow} 9 | \usepackage{amsmath} % assumes amsmath package installed 10 | \usepackage{amssymb} % assumes amsmath package installed 11 | \usepackage{url} 12 | \usepackage{authblk} 13 | 14 | \usepackage{algorithm} 15 | % Used for Algorithm: 16 | \let\labelindent\relax 17 | \usepackage{enumitem} 18 | \renewcommand{\theenumi}{\arabic{enumi}} 19 | \renewcommand{\labelenumi}{\theenumi.} 20 | \renewcommand{\theenumii}{\arabic{enumii}} 21 | \renewcommand{\labelenumii}{\theenumi.\theenumii.} 22 | \renewcommand{\theenumiii}{\arabic{enumiii}} 23 | \renewcommand{\labelenumiii}{\theenumi.\theenumii.\theenumiii.} 24 | \renewcommand{\theenumiv}{\arabic{enumiv}} 25 | \renewcommand{\labelenumiv}{\theenumi.\theenumii.\theenumiii.\theenumiv.} 26 | 27 | \usepackage{subcaption} 28 | %% New Variables %% 29 | \newcommand{\Texp}{T_{\mathrm{exp}}} 30 | \newcommand{\Tstop}{T_{\mathrm{stop}}} 31 | \newcommand{\TMPC}{T_{\mathrm{s}}^{\mathrm{MPC}}} 32 | \newcommand{\TCALCMPC}{T_{\mathrm{calc}}^{\mathrm{MPC}}} 33 | \newcommand{\Nu}{N_u} 34 | \newcommand{\Nc}{\Nu} 35 | \newcommand{\Np}{N_p} 36 | \newcommand{\Ts}{T_{\mathrm{s}}} 37 | \newcommand{\Tsmin}{T_{\mathrm{s,min}}} 38 | \newcommand{\JC}{{J^\mathrm{cl}}} 39 | \newcommand{\JCt}{{\tilde{J}^\mathrm{cl}}} 40 | \newcommand{\JCh}{{\hat{J}^\mathrm{cl}}} 41 | \newcommand{\diag}{\mathrm{diag}} 42 | \newcommand{\Nin}{n_{\mathrm{in}}} 43 | \newcommand{\ntheta}{n_\theta} 44 | 45 | \newcommand{\nin}{n_u} 46 | \newcommand{\nstate}{n_x} 47 | \newcommand{\nout}{n_y} 48 | 49 | \newtheorem{remark}{Remark}{\normalfont}{\normalfont} 50 | 51 | % For internal comments 52 | \usepackage{color} 53 | \newcommand{\DP}[1]{\textbf{\color{red}{[DP: #1]}}} 54 | \newcommand{\AB}[1]{\textbf{\color{green}{[AB: #1]}}} 55 | \newcommand{\MF}[1]{\textbf{\color{blue}{[MF: #1]}}} 56 | 57 | \title{Efficient Calibration of Embedded MPC} 58 | 59 | \author[1]{Marco Forgione} 60 | \author[1]{Dario Piga} 61 | \author[2]{Alberto Bemporad} 62 | \affil[1]{IDSIA Dalle Molle Institute for Artificial Intelligence SUPSI-USI, Manno, Switzerland} 63 | \affil[2]{IMT School for Advanced Studies Lucca, Lucca, Italy} 64 | 65 | 66 | \begin{document} 67 | \maketitle 68 | 69 | 70 | \begin{abstract} % Abstract of not more than 250 words. 71 | Model Predictive Control (MPC) is a powerful and flexible design tool of high-performance controllers for physical systems in 72 | the presence of input and output constraints. 73 | A challenge for the practitioner applying MPC is the need of tuning a large number of parameters such as prediction and control horizons, weight matrices of the MPC cost function, and observer gains, according to different trade-offs. The MPC design task is even more involved when the control law has to be deployed to an embedded hardware unit endowed with limited computational resources. In this case, real-time system requirements limit the complexity of the applicable MPC configuration, engendering additional design tradeoffs and requiring to tune further parameters, such as the sampling time and the tolerances used in the on-line numerical solver. 74 | To take into account closed-loop performance and real-time requirements, in this paper we tackle the embedded MPC design problem using a global, data-driven, optimization approach 75 | We showcase the potential of this approach by tuning an MPC controller on two hardware platforms characterized by largely different computational capabilities. 76 | \end{abstract} 77 | 78 | \vskip 1em 79 | %\begin{large} 80 | \noindent\rule{\textwidth}{1pt} 81 | To cite this work, please use the following bibtex entry: 82 | \begin{verbatim} 83 | @inproceedings{forgione2020efficient, 84 | title={{E}fficient {C}alibration of {E}mbedded {MPC}}, 85 | author={Forgione, Marco and Piga, Dario and Bemporad, Alberto}, 86 | booktitle={Proc. of the 21st IFAC World Congress, Berlin, Germany}, 87 | year={2020} 88 | } 89 | \end{verbatim} 90 | \vskip 1em 91 | Using the plain bibtex style, the bibliographic entry should look like:\\ \\ 92 | M. Forgione, D. Piga, and A. Bemporad. Efficient Calibration of Embedded {MPC}. In \textit{Proc. of the 21st IFAC World Congress, Berlin, Germany}, 2020. 93 | 94 | \noindent\rule{\textwidth}{1pt} 95 | %\end{large} 96 | 97 | 98 | \section{Introduction} 99 | %\citep{forgione2020efficient} % to test the entry 100 | Model Prediction Control (MPC) is an advanced control technology that is getting widely popular in different application 101 | domains \citep{borrelli2017predictive}. 102 | The main technical reason of its success is the ability to optimally coordinate inputs and outputs of multivariable systems in the presence of input/output constraints. 103 | %The main technical reason of its success is the ability to handle multivariable systems in the presence of input and output constraints. 104 | %The main technical reason of its success is the ability to optimally coordinate multiple inputs and outputs in the presence of input and output constraints. 105 | Besides, the intuitive interpretation of MPC as an optimal controller with respect to a given objective function makes it accessible even to practitioners with limited control background. 106 | 107 | Nonetheless, calibrating a high-performance MPC controller taking advantage of all the available tuning knobs may still require substantial effort. The challenges normally encountered are: (i) to choose parameters such as prediction and horizon, cost function weight matrices, and observer gains in order to meet desired closed-loop requirements; (ii) to implement the MPC control law on the target hardware platform, ensuring that all computations are performed in real time. 108 | 109 | Regarding challenge ($i$), the final MPC control law is determined by the prediction model, the specified cost function, and input/output constraints. 110 | %the designer has to specify input/output constraints, a model, and a cost function. 111 | Leaving aside the constraints, which may be considered direct problem specifications, the practitioner has yet to define the plant model for prediction and the cost function. The model is typically obtained from first-principle laws or estimated from measured data. However, when deriving such a model, a tradeoff emerges between accuracy and complexity, and, most of the times, it is difficult to decide \emph{a priori} how accurate the model should be in order to achieve satisfactory closed-loop performance~\citep{FoPiToSa2016,PiFoBe2017}. 112 | %This issue has been recognized as crucial in the Systems and Control community and is studied intensively in the so-called \emph{Identification for Control} (I4C) field. According to the I4C paradigm, the identification procedure should be performed keeping in mind the final control objective and thus capture those model dynamics that most affect the control performance \citep{bombois2006least}. 113 | % 114 | 115 | As for the MPC cost function, it should represent the underlying engineering or economic objective and --- in some cases --- it could also be a direct specification. However, the MPC cost function is often constrained to have a specific structure, typically a quadratic form of predicted input/output values, to allow the use of very efficient quadratic programming (QP) numerical optimization 116 | algorithms. Conversely, the true underlying objective resulting from a combination of time- or frequency-domain specifications (or economical considerations) may be formulated more naturally in a different form than a purely quadratic objective. In these cases, the MPC cost function has to be considered as a tuning knob available to achieve the actual closed-loop goal, rather than an exact quantification of the goal itself. 117 | 118 | Challenge ($ii$) is particularly relevant for fast systems controlled by embedded hardware platforms, such as mobile robots, automotive and aerospace systems, \emph{etc}. Indeed, in embedded systems the on-board computational power is usually limited, \emph{e.g.}, by cost, weight, power consumption, and battery life constraints. Therefore, real-time requirements must be taken into account, further complicating the overall MPC design task. For instance, due to throughput and memory limitations, the MPC sampling time cannot be chosen arbitrarily small and the control horizon cannot be chosen arbitrarily large. 119 | Moreover, the low-level design choices of the MPC implementation become crucial and should be taken into account in the design phase. For instance, the engineer should carefully decide whether to go for an explicit MPC approach where the solution is pre-computed offline for all states in a given range \citep{bemporad2002explicit}, or solve the MPC problem on-line by numerical optimization. The first approach is potentially very fast, but requires storing a large lookup table whose size increases with the MPC problem complexity. The applicability of this method is thus limited by the available system memory. Conversely, the second approach has generally a smaller memory footprint, but requires solving a numerical optimization problem on-line. Thus, in the latter case, the MPC problem complexity and the hardware's computational power limit the maximum controller update frequency. 120 | 121 | %Moreover, when the MPC control law is computed by numerical optimization --- for instance using a QP solver in the case of linear constrained MPC --- the optimizer type and all its hyperparameters may be considered as MPC tuning knobs. 122 | 123 | When the MPC law is computed by numerical optimization (a QP solver in case of linear MPC), the hyperparameters of the optimizer are also MPC tuning knobs, in that they affect solution accuracy and required computation time. 124 | %At this point, new tradeoffs arise: for instance, one could opt for a control law computed either with high accuracy and low updating frequency, or for another one computed with lower accuracy at a higher frequency. 125 | The overall design of a high-performance MPC must therefore take into account simultaneously high-level aspects related to control systems (model, weights, constraints, prediction horizon, sample time, \emph{etc.}) and low-level aspects of numerical optimization (problem size and solver-related hyperparametes). 126 | 127 | In recent years, data-driven approaches for solving complex engineering tuning problems based on derivative-free global optimization are gaining increasing attention \citep{shahriari2016taking}. 128 | The idea behind these approaches is rather intuitive: the user specifies a search space for the design parameters and the optimization algorithm, based on performance data previously observed, sequentially suggests the new configurations to be tested, aiming to optimize a user-given performance index. The procedure is iterated until a configuration achieving satisfactory performance is found or the maximum number of available tests has been reached. 129 | This approach has also been popularized as Design and Analysis of Computer Experiments (DACE)~\citep{SWMW89}. 130 | %Suitable optimization algorithms for this task should be able to apprach an optimal solution with a small number of possibly noisy function evaluations, and should not require derivative information. 131 | Specialized global optimization algorithms for this task such as Bayesian Optimization 132 | (BO) have been proposed \citep{brochu2010tutorial}. The key feature of these algorithms is their 133 | ability to optimize the objective function with a small number of (possibly noisy) evaluations, without relying on derivative information. 134 | %Suitable optimization algorithms should be able to cope with noisy observations, of. Furthermore, minimize evaluations. 135 | %Global gradient-free optimization algorithms requirin. Furthermore, 136 | %these algorithms should be very efficient in terms of number of function evaluations. 137 | Recently, optimization-based tuning has been successfully applied to control system design~\citep{roveda2019control, driess2017constrained} and to choose the MPC prediction model ~\citep{piga2019performance, bansal2017goal}. 138 | 139 | In this paper, we demonstrate the potential of the optimization-based data-driven approach for the joint tuning of high- and low-level MPC parameters in order to optimize a certain closed-loop performance objective, while ensuring that the control law can be computed in real-time on the hardware platform at hand. 140 | %ensure that a closed-loop performance objective is optimized and ($ii$) the control law can be computed in real-time on the hardware at hand. 141 | We apply a derivative-free global optimization algorithm recently developed by one of the authors~\citep{bemporad2019global}, which has been shown to be very efficient in terms of number of function evaluations required to solve the global optimization problem. 142 | We present the results of our MPC tuning procedure for a simulated cart-pole system on two hardware platforms with very different computational capability, namely a high-end x86-64 workstation and a low-performance ARM-based board (specifically, a Raspberry PI 3, Model B). We show that our tuning procedure can find an MPC configuration that squeezes the maximum performance out of the two architectures for the given control task. 143 | 144 | The rest of the paper is organized as follows. The MPC problem formulation and its design parameters are introduced in Section \ref{sec:problem}. Next, the data-driven MPC calibration strategy based on global optimization is described in Section \ref{sec:main} and numerical examples are presented in Section \ref{sec:example}. Finally, conclusions and directions for future research are discussed in Section \ref{sec:conclusions} 145 | 146 | \section{Problem formulation}\label{sec:problem} 147 | 148 | Let us consider the following nonlinear continuous-time multi-input multi-output dynamical system 149 | in state-space form 150 | \begin{subequations} \label{eqn:sys} 151 | \begin{align} 152 | \dot x &= f(x,u) \\ 153 | y &= g(x,u), 154 | \end{align}% 155 | \label{mathcalS}% 156 | \end{subequations} 157 | where $u\in \mathbb{R}^{\nin}$ is the vector of control inputs, $x\in \mathbb{R}^{\nstate}$ 158 | the state vector, $y\in \mathbb{R}^{\nout}$ the controlled outputs, $\dot x$ denotes the time derivative of the state $x$, and $f: \mathbb{R}^{\nstate+\nin} \to\mathbb{R}^{\nstate}$ and $g: \mathbb{R}^{\nstate+\nin} \to\mathbb{R}^{\nout}$ are the state and output mappings, respectively. 159 | Output variables can be collected and used for real-time control at a sampling time $\Ts \geq \Tsmin$, where $\Tsmin$ is the minimum sampling time achievable by the measurement system. 160 | 161 | %signals available at a regular time interval $\Ts$. 162 | We aim at synthesizing an MPC (with a state estimator) for~\eqref{eqn:sys} such that the resulting closed-loop system minimizes a certain closed-loop performance index $\JC$. This performance index is defined as a continuous-time functional $\JC(y_{[0,\;\Texp]}, u_{[0,\;\Texp]})$ in an experiment of fixed duration $\Texp$, where $y_{[0,\;\Texp]}$ (resp. $u_{[0,\;\Texp]}$) denotes the output signal $y(t)$ (resp. input signal $u(t)$) within the time interval $[0,\;\Texp]$. 163 | % 164 | %\begin{equation} 165 | % \JC = \JC(y_{[0\;\Texp]}, u_{[0\;\Texp]}). 166 | %\end{equation} 167 | %\MF{Improve notation for a functional cost on CT variables} 168 | %defined over the duration $T$ of an experiment. 169 | % 170 | As an additional requirement, we must ensure that the control law can be computed in real-time on a given hardware platform. This requirement is translated into the constraint $\TCALCMPC \leq \TMPC$, where $\TMPC$ denotes the MPC sampling time and $\TCALCMPC$ the worst-case time required to compute the MPC control law on the given platform. %In order to satisfy the real-time requirements, we need of course that for the chosen control law $\TCALCMPC < \TMPC$. 171 | %\begin{remark} 172 | It is worth remarking that in general the closed-loop performance index $\JC$ is a nonconvex function of the MPC design parameters, that we will defined later. For the sake of generality, $\JC$ has been denoted above as a continuous-time functional over the duration $\Texp$ of the experiment. Thus, $\JC$ does not necessarily correspond to the cost function minimized on-line by MPC. Indeed, the latter is generally defined as a discrete-time quadratic function over a prediction horizon generally shorter than $\Texp$. 173 | %(\DP{dire che la definiamo dopo}) 174 | %\end{remark} 175 | 176 | %\DP{Introdurre le sezioni sotto} 177 | In the following paragraphs, we define the MPC and state estimator design problem, 178 | along with their tuning parameters. 179 | %In the remainder of this section, we provide details on the MPC controller and the state estimator used in this paper, contextually introducing all their adjustable parameters available for design. 180 | %These free parameters will be tuned in Section \ref{sec:main} 181 | %that will be tuned according to the procedure described in Section \ref{sec:main}. 182 | 183 | \subsection{MPC controller} \label{Sec:outerMPC} 184 | We assume that a continuous-time (possibly parametrized) model $M(\theta^m)$ of~\eqref{mathcalS} is available, where $\theta^m$ represents a vector of adjustable model parameters. 185 | For a given choice of $\TMPC$, $M(\theta^m)$ can be linearized and discretized in time, yielding the discrete-time state-space model 186 | \begin{subequations} 187 | \label{eq:SITO} 188 | \begin{align} 189 | x_{t+1}&=A(\TMPC,\theta^m)x_t+B(\TMPC,\theta^m)u_t \label{eq:MPCsysstate}\\ 190 | y_t&=C(\theta^m)x_t+D(\theta^m)u_t \label{eq:MPCsysout} 191 | \end{align} 192 | \end{subequations} 193 | that is used as prediction model for MPC. 194 | 195 | At each time $t$ that is an integer multiple of the MPC sampling time $\TMPC$, MPC solves the minimization problem 196 | \begin{subequations} \label{eq:MPC} 197 | \begin{align} 198 | & \nonumber \min_{\left\{u_{t+k|t}\right\}_{k=0}^{\Nu-1}, \epsilon} \sum_{k=0}^{\Np-1} \left(y_{t+k|t}-y^{\rm{ref}}_{t+k}\right)^\top Q_y\left(y_{t+k|t}-y^{\rm{ref}}_{t+k}\right) +\\ 199 | & \nonumber \qquad \qquad+\sum_{k=0}^{\Np-1} \left(u_{t+k|t}-u^{\rm ref}_{t+k}\right)^\top Q_u \left(u_{t+k|t}-u^{\rm ref}_{t+k}\right) \!+\!\\ 200 | &\qquad \qquad + \sum_{k=0}^{\Np-1} \Delta u_{t+k|t}^\top Q_{\Delta u} \Delta u_{t+k|t} + Q_\epsilon \epsilon^2 \label{eq:MPCcost}\\%+ Q_{\rK} 201 | & \mathrm{s.t. \ } \text{model equations } \eqref{eq:MPCsysstate},\eqref{eq:MPCsysout} \\ 202 | & \quad y_{\mathrm{min}} \!-\! V_y\epsilon\leq y_{t+k|t} \leq y_{\mathrm{max}}+V_y\epsilon, \ k=1,\ldots,\Np \\ 203 | & \quad u_{\mathrm{min}}\!-\!V_u\epsilon \leq u_{t+k|t} \leq u_{\mathrm{max}}+V_u\epsilon, \ k=1,\ldots,\Np \\ 204 | & \quad \Delta u_{\mathrm{min}} -V_{\Delta u}\epsilon \leq \Delta u_{t+k|t}, \ \ k=1,\ldots,\Np \label{eq:MPC:Ducons1} \\ 205 | & \quad \Delta u_{t+k|t} \leq \Delta u_{\mathrm{max}}+V_{\Delta u}\epsilon, \ \ k=1,\ldots,\Np \label{eq:MPC:Ducons2}\\ %\label{eq:MPC:ucons} \\ 206 | & \quad u_{t+\Nu+j|t}= u_{t+\Nu|t}, \ \ j=1,\ldots, \Np-\Nu, \label{eq:MPCconst} 207 | \end{align} 208 | \end{subequations} 209 | where $\Delta u_{t+k|t} = u_{t+k|t}- u_{t+k-1|t}$; $\Np$ and $\Nu$ are the prediction and control horizon, respectively; $Q_{y}$, $Q_{u}$, and $Q_{\Delta u}$ are positive semidefinite weight matrices specifying the MPC cost function; $u_{\rm ref}$ and $y_{\rm ref}$ are the input and output references, respectively; $Q_{\epsilon}$, $V_y$, $V_u$, $V_{\Delta u}$ are positive constants used to soften the input and output constraints, ensuring that the optimization problem~\eqref{eq:MPC} is always feasible. 210 | An MPC calibrator would typically adjust $\Np,\Nc, Q_y, Q_u, Q_{\Delta u}$ using a mix of experience and trial-and-error until the desired closed-loop goals are achieved, 211 | fixing the remaining parameters to their default value. 212 | Such a process, in particular in the absence of a deep knowledge of MPC, can be very time consuming and therefore costly. 213 | 214 | Several parameterizations may be used to simplify the calibration task. For instance, weight matrices $Q_y$, $Q_u$, and $Q_{\Delta u}$ may be constrained to be diagonal (one of the weights may also be chosen equal to one without loss of generality). %[NOT DONE HERE, OMIT] 215 | For notation convenience, we denote by $\theta^c$ the set of all tuning parameters of the MPC problem introduced above.%\eqref{eq:MPC}. 216 | 217 | The solution of the QP problem \eqref{eq:MPC} is computed through numerical optimization. 218 | We denote by $\theta^s$ the QP solver settings, that we assume can also be adjusted by the calibrator. For instance, important solver parameters are the QP's relative and absolute 219 | feasibility/optimality tolerances for termination. Note that the parameters $\theta^s$ influence both the accuracy of the numerical solution (thus, the performance index $\JC$) and the computation time (thus, $\TCALCMPC$). 220 | %on the one hand the accuracy of the numerical solution and thus of performance $\JC$, on the other hand the time required to compute the solution and thus $\TCALCMPC$. 221 | 222 | \subsection{State estimator} 223 | An estimate of the system state $x_t$ is required to solve the MPC optimization problem~\eqref{eq:MPC}. In this paper, we use a Luenberger observer for state estimation: 224 | \begin{subequations} 225 | \begin{align} 226 | \hat x_{t+1|t} &= A \hat x_{t|t-1} + B u_t + L(y_t - C\hat x_{t|t-1})\\ 227 | \hat y_{t+1|t} &= C \hat x_{t+1|t}, 228 | \end{align} 229 | \end{subequations} 230 | where $\hat x_{t|t-1}$ is the state estimate at time $t$ based on observations up to time $t-1$. 231 | We compute the gain $L$ as the standard stationary Kalman filter gain, based on the (linearized) model \eqref{eq:SITO}, assuming positive definite covariance matrices $W_w$ and $W_v$ for the additive process and measurement noise, respectively. 232 | As for the MPC tuning weights, different parametrizations/structures may be used to 233 | define such covariance matrices. The corresponding parameters are the tuning knobs of the state estimator and are denoted as $\theta^f$. 234 | %Let us introduce parameters $\theta_F$ describing the positive-definite matrices $W_v$ and $W_w$: we have $L = L(\TMPC, \theta_M, \theta_F)$. 235 | %As for the MPC weight matrices, different parametrizations may be used. 236 | %\begin{remark} 237 | %The Kalman filter may run at a sample time different than $\TMPC$. 238 | %This could make sense for a standard Kalman filter whose computational complexity is %way lower than the one of constrained MPC. 239 | %However, we avoid this further complication in this paper. 240 | %\end{remark} 241 | 242 | \section{Performance-driven parameter tuning}\label{sec:main} 243 | 244 | For notation convenience, the design parameters $\theta^m$, $\theta^c$, $\theta^s$, $\theta^f$ introduced in the previous section are collected in the single vector $\theta \in \mathbb{R}^{\ntheta}$. 245 | %Note that the closed-loop performance cost is a function of $\theta$. 246 | 247 | %In this section, we describe the experiment-driven MPC calibration procedure that optimize the closed-loop performance index $\JC(y_{[0,\Texp]},u_{[0,\Texp]}; \theta)$, under the real-time constraint $\TCALCMPC(\theta) \leq \TMPC$. 248 | In this section, we describe how to tune $\theta$ through an experiment-driven approach in order to optimize the closed-loop performance index $\JC(y_{[0,\Texp]},u_{[0,\Texp]}; \theta)$, under the real-time constraint $\TCALCMPC(\theta) \leq \TMPC$. 249 | The overall MPC design task can be formalized as the following constrained global 250 | optimization problem 251 | \begin{subequations} 252 | \label{eq:GOP} 253 | \begin{align} 254 | & \min_{\theta \in \Theta}{\JC(y_{[0,\Texp]},u_{[0,\Texp]}; \theta)} \label{eq:TCALCcst} \\ 255 | & \mathrm{s.t. \ } \TCALCMPC(\theta) \leq \eta \TMPC. \label{eq:TCALCcnst} 256 | \end{align} 257 | \end{subequations} 258 | In~\eqref{eq:TCALCcst}, $\Theta \subseteq \mathbb{R}^{\ntheta}$ is the set of admissible values of the design vector $\theta$. Specifically, in this work, $\Theta$ is a box-shaped region delimited by lower and upper bounds for each individual parameter. 259 | The constant $\eta$, $0<\eta<1$ in~\eqref{eq:TCALCcnst} takes into account that, in a practical implementation, a fraction of the controller's computation time should be left available for other tasks. %Furthermore, it is a safety margin. 260 | 261 | It is important to stress that neither for $\JC(\theta)$ nor for $\TCALCMPC(\theta)$ a closed-form expression is available. 262 | Nevertheless, these functions can be evaluated through real experiments or simulations\footnote{In order to evaluate the worst-case computation time $\TCALCMPC$ for a given parameter $\theta$, the control law should be computed on the target hardware directly. In the absence of a platform emulator, this can be done, for instance, with an \emph{hardware-in-the-loop} setup where the target hardware closes the control loop on a simulated system.}. 263 | In the following, we describe the optimization algorithm used to solve problem \eqref{eq:GOP} based on function evaluations of $\JC$ and $\TCALCMPC$. 264 | One of main strengths of this algorithm is its efficiency in terms of number of required 265 | function evaluations. 266 | %ad 267 | 268 | %The optimization algorithm used in this paper is briefly discussed in the remainder of this section. 269 | 270 | %It is important to stress that neither the performance $\JC$ nor the MPC computation time $\TCALCMPC$ are known functions of $\theta$. 271 | 272 | \subsection{Global optimization for parameter selection} \label{sec:GLIS} 273 | First, the constrained optimization problem~\eqref{eq:GOP} is approximated by the following box-constrained problem 274 | \begin{subequations} \label{eqr:probConst} 275 | \begin{align} 276 | & \min_{\theta \in \Theta}{\JCt(\theta)}, 277 | \end{align} 278 | where $\JCt(\theta)$ is defined as the original cost $\JC( \theta)$ plus a continuous barrier function $\ell: \mathbb{R} \rightarrow \mathbb{R}^+$ penalizing the violation of the constraint $\TCALCMPC(\theta) \leq \eta \TMPC$, \emph{i.e.}, 279 | \begin{align} \label{eqn:Jtilde} 280 | & \JCt(\theta) = \JC( \theta) + \ell\left(\TCALCMPC(\theta) - \eta \TMPC\right). 281 | \end{align} 282 | \end{subequations} 283 | 284 | 285 | We solve the optimization problem~\eqref{eqr:probConst} by using the approach described 286 | in~\citep{bemporad2019global}, called GLIS (GLobal optimization based on Inverse distance weighting and radial basis function Surrogates).%, available for download at \url{http://cse.lab.imtlucca.it/~bemporad/glis}. 287 | 288 | The algorithm first runs $\Nin \geq 1$ closed-loop experiments 289 | for $\Nin$ different values of the vector of controller parameters $\theta_n$, $n=1,\ldots,\Nin$, 290 | generated randomly using Latin Hypercube Sampling (LHS)~\citep{mckay1979comparison} 291 | within $\Theta$, measuring the corresponding performance index $\JCt_n$. 292 | Next, at iteration $n \geq \Nin$ a radial basis function $\hat f_{\rm RBF}$ is fit to the available samples $\{(\theta_1,\JCt_1),\dots, (\theta_n,\JCt_n)\}$. This function $\hat f_{\rm RBF}$ is a surrogate of the (non-quantified) performance index $\JCt$. 293 | Another function $z:\Theta\to\mathbb{R}$, that promotes the exploration of the set $\Theta$ in areas that 294 | have not been sampled yet and where the empirical variance of $\JCt-\hat f_{\rm RBF}$ 295 | is large, is summed to $\hat f_{\rm RBF}$ to define an acquisition function $a:\Theta\to\mathbb{R}$. 296 | The function $a$ (which is very easy to evaluate and even differentiate) is then minimized over $\Theta$ by using a global optimization algorithm, 297 | such as Particle Swarm Optimization (PSO)~\citep{EK95,VV09}, 298 | to get a new configuration $\theta_{n+1}$ 299 | of MPC parameters to test. A new closed-loop experiment is performed with controller parameters $\theta = \theta_{n+1}$ and the performance index $\JCt_{n+1}$ is measured. 300 | The algorithm iterates until a stopping criterion is met or a maximum number $n_{\mathrm{max}}$ of iterations is reached. 301 | 302 | The main advantage of using the global optimization algorithm GLIS described above for solving the calibration problem is twofold. First, it is a derivative-free algorithm, and thus it is particularly convenient since a closed-form expression of the cost $\JCt$ as a function of the design parameters $\theta$ is not available. Second, it allows us to tune the controller parameters $\theta$ with a smaller number of experiments compared to other existing global optimization methods, 303 | such as PSO, DIRECT (DIvide a hyper-RECTangle)~\citep{Jon09}, 304 | Multilevel Coordinate Search (MCS)~\citep{HN99}, Genetic Algorithms (GA)~\citep{Han06}, 305 | and usually even than BO as reported in~\citep{bemporad2019global}. 306 | 307 | % The latter point is crucial, since each evaluation can be costly and time-consuming, as it requires a closed-loop experiment. 308 | 309 | %\subsection{Bayesian optimization for parameter selection} 310 | 311 | \section{Numerical Example} 312 | \label{sec:example} 313 | \begin{figure} 314 | \centering 315 | \includegraphics[width=.4\textwidth]{fig/cart_pole.pdf} 316 | \caption{Schematics of the cart-pole system.} 317 | \label{fig:pendulum_cart} 318 | \end{figure} 319 | 320 | As a case study, we consider the problem of controlling the cart-pole system depicted in Fig.~\ref{fig:pendulum_cart}. 321 | We aim at designing an MPC controller that minimizes a given closed-loop performance index $\JC$, while satisfying the constraint~\eqref{eq:TCALCcnst} on the worst-case MPC execution time $\TCALCMPC$ for real-time implementation. 322 | The MPC law is computed using a custom-made Python library that transforms problem~\eqref{eq:MPC} into a standard QP form, which is subsequently solved using the ADMM-based QP solver OSQP \citep{stellato2018osqp}. 323 | The MPC parameters are tuned via global optimization using the solver GLIS 324 | recalled in Section~\ref{sec:GLIS} and retrieved from \url{http://cse.lab.imtlucca.it/~bemporad/glis}. In particular, package version 1.1 was used with default settings. 325 | 326 | The source code generating the results in this paper can be found at \url{https://github.com/forgi86/efficient-calibration-embedded-MPC}. 327 | A standalone installation version of the MPC library is also available at \url{https://github.com/forgi86/pyMPC} for convenient integration in other projects. 328 | 329 | The MPC tuning is performed for two different hardware platforms with remarkably different speed performance: 330 | \begin{itemize} 331 | \item an x86-64 PC equipped with an Intel i5-7300U 2.60~GHz CPU and 32 GB of RAM; 332 | \item a Raspberry PI 3 rev. B board equipped with a 1.2~GHz ARM Cortex-A53 CPU and 1~GB of RAM. 333 | \end{itemize} 334 | 335 | The Raspberry PI 3 is roughly 10 times slower than the PC in computing the MPC law. This leads to different constraints on the maximum controller complexity and thus on the achievable closed-loop performance. 336 | 337 | \subsection{System description} 338 | The cart-pole dynamics are governed by the following continuous-time differential equations which are used to simulate the behavior of the system: 339 | %\begin{subequations*} \label{eqn:pend} 340 | \begin{align*} 341 | (M+m)\ddot p + mL\ddot\phi \cos\phi - mL \dot \phi ^2 \sin \phi + b\dot p &= F\\ 342 | L \ddot \phi + \ddot p \cos \phi - g \sin\phi + f_\phi\dot \phi &=0 , 343 | \end{align*} 344 | %\end{subequations} 345 | where $\phi\,\mathrm{(rad)}$ is the angle of the pendulum with respect to the upright vertical position, $p\,\mathrm{(m)}$ is the cart position, and $F\,\mathrm{(N)}$ is the input force on the cart. The following values of the physical parameters are used: $M=0.5\,\mathrm{kg}$ (cart mass), $m=0.2\,\mathrm{kg}$ (pendulum mass), $L=0.3\,\mathrm{m}$ (rod length), 346 | $g=9.81\,\mathrm{m/s^{2}}$ (gravitational acceleration), $b=0.1\,\mathrm{N/m/s}$, and $f_\phi=0.1\,\mathrm{m/s}$ (friction terms). 347 | Measurements of $p$ and $\phi$ 348 | are supposed to be collected at a minimum sampling time $\Tsmin=1\,\mathrm{ms}$, and are corrupted by additive zero-mean white Gaussian noise sources with standard deviation $0.02\,\mathrm{m}$ and $0.01\,\mathrm{rad}$, respectively. The input force $F$ is perturbed by an additive zero-mean colored Gaussian noise with standard deviation $0.1\,\mathrm{N}$ and bandwidth $5\,\mathrm{rad/s}$. 349 | The force $F$ is bounded to the interval $[-10,\; 10]\, \mathrm{N}$, which is due to actuator saturation, while the cart position $p$ is limited to the interval $[-1.2,\; 1.2]\,\mathrm{m}$, representing the length of the track where the cart moves. 350 | 351 | The system is initialized at $[p(0) \; \dot{p}(0)\; \phi(0)\;\dot{\phi}(0)] = [0 \; 0\; \frac{\pi}{18}\;0]$. In the MPC calibration experiment, the objective is to track a piecewise linear position reference $p^{\mathrm{ref}}$ for the cart passing through the time-position points $\{(0,0)$, $(5,0)$, $(10,0.8)$, $(20,0.8)$, $(25,0)$, $(30,0)$, $(40,0.8)\}$ over an experiment of duration $\Texp=40$ s, while stabilizing the angle $\phi$ to the upright vertical position, \emph{i.e.,} at $\phi=0$. 352 | %The MPC is configured with fixed constraints $F_{\max}\!=\!-F_{\min}\!=\!10~\mathrm{N}$ and $p_{\max}\!=\!-p_{\min}\!=\!1~\mathrm{m}$ on the input force $F$ and the output position $p$, respectively. 353 | %Other MPC settings are free tunable parameters, as discussed later. 354 | %\footnote{Note that the position constraint of MPC is slightly tighter than the physical limit.} 355 | 356 | 357 | The controller is disabled at time $\Tstop < \Texp$ whenever one of the following early termination condition occurs: 358 | \begin{itemize} 359 | \item pendulum falling ($|\phi| > \frac{\pi}{6})$ 360 | \item cart approaching end of the track ($|p| \geq 1.1~\mathrm{m}$) 361 | \item numerical errors in the MPC law computation 362 | \end{itemize} 363 | Similar conditions may be required to ensure safety in the case of real experiments performed on a physical setup. 364 | In our simulation setting, they are still useful to reduce the computational time as they speed up the test of configurations that are definitely not optimal. 365 | Furthermore, early termination is explicitly penalized in our closed-loop performance index (see next paragraph), and thus provides useful information for MPC calibration to the global optimization algorithm. 366 | 367 | \subsection{Performance index} 368 | The closed-loop performance index $\JCt$ is defined as 369 | \begin{multline} 370 | \label{eq:JCL_example} 371 | \JCt = \ln \left ( \int_{t=0}^{\Texp}10 |p^{\mathrm{ref}}(t) - p(t)| + 30 |\phi(t)|\;dt \right )+ \\ 372 | + \ell\left(\TCALCMPC - \eta \TMPC\right) + \ell'(\Texp - \Tstop), 373 | \end{multline} 374 | where the penalty term $\ell$ for real-time implementation of the control law is 375 | \begin{equation} 376 | \label{eq:penalty} 377 | \ell\!=\! \begin{cases} 378 | \!\ln \left(1+ 10^3\frac{\TCALCMPC\!-\!\eta \TMPC}{\eta \TMPC}\right)&\text{if } \TCALCMPC > \eta \TMPC\\ 379 | 0 &\text{otherwise}, 380 | \end{cases} 381 | \end{equation} 382 | with $\eta = 0.8$. Another term $\ell'$ is used to penalize early termination conditions, as previously discussed: 383 | \begin{equation} 384 | \label{eq:penalty_time} 385 | \ell'\!=\! \begin{cases} 386 | \!\ln \left(1+ 10^3\frac{\Texp\!-\!\Tstop}{\Texp}\right)&\text{if } \Tstop < \Texp\\ 387 | 0 & \text{otherwise}. 388 | \end{cases} 389 | \end{equation} 390 | The integral in~\eqref{eq:JCL_example} is approximated using samples collected at the fastest sampling time $\Tsmin$. 391 | %Conversely, the MPC sample $\TMPC$ is one of the MPC design parameters. 392 | 393 | \subsection{Control design parameters} 394 | We have different MPC design parameters to tune in order to minimize the performance-driven 395 | objective~\eqref{eq:JCL_example}. 396 | 397 | As for the MPC cost function, the positive definite weight matrix $Q_y$ is diagonally parameterized as 398 | $\bigl( \begin{smallmatrix} 399 | q_{y_{11}}&0\\ 0&q_{y_{22}} 400 | \end{smallmatrix} \bigr)$, 401 | where $q_{y_{11}}$, and $q_{y_{22}}$ are design parameters taking real values in the interval $[10^{-16},\ 1]$. Since in the example $n_u=1$, the weights $Q_u$ and $Q_{\Delta u}$ are scalars. $Q_{\Delta u}$ is taken as a decision variable and bounded in the range $[10^{-16} ,\ 1]$, while $Q_u$ is set to 0. The prediction horizon $\Np$ is an integer parameter in the range $[5,\ 300]$, while the control horizon $\Nc$ is a fraction $\epsilon_c$ of $\Np$ rounded to the closest integer, with design parameter $\epsilon_c$ restricted to the range $[0.3,\ 1]$. Lastly, the MPC sampling time $\TMPC$ is a design parameter restricted to the range $[1,\ 50]\,\mathrm{ms}$. 402 | 403 | In the QP solver, the relative and absolute feasibility/optimality tolerances are tuned. Specifically, the log of two tolerances are parameters in the range $[-7,\ -1]$. 404 | 405 | As for the state estimator, the 4x4 process noise covariance matrix $W_w$ and the 2x2 output noise covariance matrix $W_v$ are diagonally parametrized, similarly to $Q_y$. 406 | 407 | The system dynamics are assumed to be known. Therefore, there is no tunable model parameter in our design problem. 408 | 409 | Finally, MPC is configured with fixed constraints $F_{\max}\!=\!-F_{\min}\!=\!10~\mathrm{N}$ and $p_{\max}\!=\!-p_{\min}\!=\!1~\mathrm{m}$ on the input force $F$ and on the output position $p$, respectively, while standard values are used for all other MPC settings in \eqref{eq:MPC} not mentioned here. 410 | %\footnote{Note that the position constraint of MPC is slightly tighter than the physical limit.} 411 | 412 | The design parameter $\theta$ has thus dimension $\ntheta=14$. 413 | %Overall, the design parameter $\theta$ consists of $\ntheta=14$ variables. 414 | 415 | \subsection{Results} 416 | 417 | 418 | \begin{figure} 419 | \centering 420 | \begin{subfigure}[b]{\textwidth} 421 | \includegraphics[width=1\linewidth]{fig/ITER_GLIS_PC_500.pdf} 422 | %\caption{} 423 | %\label{fig:Ng1} 424 | \end{subfigure} 425 | 426 | \begin{subfigure}[b]{\textwidth} 427 | \includegraphics[width=1\linewidth]{fig/ITER_GLIS_PI_500.pdf} 428 | %\caption{} 429 | %\label{fig:Ng2} 430 | \end{subfigure} 431 | \caption{Performance cost $\JCt$ vs. iteration index $n$ of GLIS for experiments run on the PC (top) and on the Raspberry PI (bottom).} 432 | \label{fig:iterations} 433 | \end{figure} 434 | 435 | %\end{figure} 436 | 437 | \begin{figure} 438 | \centering 439 | \begin{subfigure}[b]{\textwidth} 440 | \includegraphics[width=1\linewidth]{fig/BEST_GLIS_PC.pdf} 441 | %\caption{} 442 | %\label{fig:Ng1} 443 | \end{subfigure} 444 | 445 | \begin{subfigure}[b]{\textwidth} 446 | \includegraphics[width=1\linewidth]{fig/BEST_GLIS_PI.pdf} 447 | %\caption{} 448 | %\label{fig:Ng2} 449 | \end{subfigure} 450 | \caption{Performance achieved by the designed MPC for experiments run on the PC (top panel) and on the Raspberry PI (bottom panel).} 451 | \label{fig:timetraces} 452 | \end{figure} 453 | 454 | The global optimizer GLIS is run for $n_{\mathrm{max}} = 500$ iterations. The performance cost $\JCt$~\eqref{eq:JCL_example} \emph{vs.} the iteration index $n$ is shown in Fig.~\ref{fig:iterations} for the PC (top) and the Raspberry PI 3 (bottom). It can be observed that, after about $100$ iterations, the majority of the controller parameter configurations proposed by the optimization algorithm are concentrated in regions with low cost $\JCt$. 455 | This is more evident on the Raspberry PI 3 platform, where the set of parameters satisfying the real-time constraint~\eqref{eq:TCALCcnst} is smaller. 456 | Persistent high values $\JCt$ are due (correctly) to the exploration of the parameter space $\Theta$ promoted by the GLIS algorithm. 457 | 458 | 459 | The obtained optimal performance index $\JCt$ after 500 iterations is slightly better on the PC (-0.44) than on the Raspberry PI (0.02), as expected. Indeed, certain MPC configurations characterized, \emph{e.g.}, by small sampling time and long prediction horizon may be feasible on the PC, but not on the Raspberry PI. 460 | 461 | Fig.~\ref{fig:timetraces} shows the time trajectories of position $p$, angle $\phi$, and force $F$ for the optimal MPC controller on the PC (top panel) and on the Raspberry PI (bottom panel), over a validation reference trajectory different from the one used for calibration. A slightly better performance for the MPC implementation on the PC can be appreciated both in terms of a tighter cart position tracking and a lower variance in the angle signal. %throughout the whole experiment. 462 | 463 | Analyzing the two final MPC designs, we noticed that on the PC we have $\TMPC= 6~\mathrm{ms}$, while on the Raspberry PI we have $\TMPC= 22~\mathrm{ms}$. The optimal solution found for the PC platform allows a faster loop update, and thus achieves superior trajectory tracking and noise rejection capabilities. 464 | On the other hand, a larger MPC time is required on the Raspberry PI to guarantee real-time implementation. %, whose violation would otherwise result in a large cost due to the penalty term $\ell(\TCALCMPC)$ in $\JCt$. 465 | % Conversely, the penalty term $\ell(\TCALCMPC)$ is zero for most of the MPC configuration in the parameter space on the PC, where the control law can be computed much faster. Thus, on the PC, a larger portion of the MPC design parameter space is feasible. 466 | 467 | 468 | 469 | \section{Conclusions and follow-up} 470 | \label{sec:conclusions} 471 | We have presented an automated method to calibrate MPC parameters with a limited number of experiments. Real-time implementation constraints are explicitly taken into account in the design in order to allow embedded implementation of the resulting controller. 472 | 473 | Future research will be devoted to find a parameterized solution of the optimal MPC tuning knobs with respect to the reference trajectories, and to the analysis of the generalization properties of the designed controllers against control objectives not considered in the calibration phase. 474 | 475 | 476 | %\begin{ack} 477 | %Place acknowledgments here. 478 | %\end{ack} 479 | 480 | \bibliography{ms} % bib file to produce the bibliography 481 | % with bibtex (preferred) 482 | 483 | \end{document} 484 | -------------------------------------------------------------------------------- /pendulum_MPC_sim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.sparse as sparse 3 | from pyMPC.mpc import MPCController 4 | from kalman import kalman_design_simple, LinearStateEstimator 5 | from pendulum_model import * 6 | from scipy.integrate import ode 7 | from scipy.interpolate import interp1d 8 | import time 9 | import control 10 | import control.matlab 11 | import numpy.random 12 | 13 | Ts_fast = 1e-3 14 | 15 | Ac_def = np.array([[0, 1, 0, 0], 16 | [0, -b / M, -(g * m) / M, (ftheta * m) / M], 17 | [0, 0, 0, 1], 18 | [0, b / (M * l), (M * g + g * m) / (M * l), -(M * ftheta + ftheta * m) / (M * l)]]) 19 | 20 | Bc_def = np.array([ 21 | [0.0], 22 | [1.0 / M], 23 | [0.0], 24 | [-1 / (M * l)] 25 | ]) 26 | 27 | # Reference input and states 28 | #t_ref_vec = np.array([0.0, 10.0, 20.0, 30.0, 40.0]) 29 | #p_ref_vec = np.array([0.0, 0.8, 0.8, 0.0, 0.0]) 30 | #rp_fun = interp1d(t_ref_vec, p_ref_vec, kind='zero') 31 | t_ref_vec = np.array([0.0, 5.0, 10.0, 20.0, 25.0, 30.0, 40.0, 100.0]) 32 | p_ref_vec = np.array([0.0, 0.0, 0.8, 0.8, 0.0, 0.0, 0.8, 0.8]) 33 | rp_fun = interp1d(t_ref_vec, p_ref_vec, kind='linear') 34 | 35 | 36 | def xref_fun_def(t): 37 | return np.array([rp_fun(t), 0.0, 0.0, 0.0]) 38 | 39 | 40 | #Qx_def = 0.9 * sparse.diags([0.1, 0, 0.9, 0]) # Quadratic cost for states x0, x1, ..., x_N-1 41 | #QxN_def = Qx_def 42 | #Qu_def = 0.0 * sparse.eye(1) # Quadratic cost for u0, u1, ...., u_N-1 43 | #QDu_def = 0.01 * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 44 | 45 | Ts_MPC_def = 10e-3 46 | Qx_def = 1.0 * sparse.diags([1.0, 0, 5.0, 0]) # Quadratic cost for states x0, x1, ..., x_N-1 47 | QxN_def = Qx_def 48 | 49 | Qu_def = 0.0 * sparse.eye(1) # Quadratic cost for u0, u1, ...., u_N-1 50 | QDu_def = 1e-5/(Ts_MPC_def**2) * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 51 | 52 | DEFAULTS_PENDULUM_MPC = { 53 | 'xref_fun': xref_fun_def, 54 | 'uref': np.array([0.0]), # N 55 | 'std_npos': 0.02, # m 56 | 'std_nphi': 0.01, # rad 57 | 'std_dF': 0.1, # N 58 | 'w_F': 5, # rad/s 59 | 'len_sim': 40, #s 60 | 61 | 'Ac': Ac_def, 62 | 'Bc': Bc_def, 63 | 'Ts_MPC': Ts_MPC_def, 64 | 'Q_kal': np.diag([0.1, 10, 0.1, 10]), 65 | 'R_kal': 1*np.eye(2), 66 | 67 | 'Np': 100, 68 | 'Nc': 50, 69 | 'Qx': Qx_def, 70 | 'QxN': QxN_def, 71 | 'Qu': Qu_def, 72 | 'QDu': QDu_def, 73 | 'QP_eps_abs': 1e-3, 74 | 'QP_eps_rel': 1e-3, 75 | 'seed_val': None 76 | 77 | } 78 | 79 | 80 | def get_parameter(sim_options, par_name): 81 | return sim_options.get(par_name, DEFAULTS_PENDULUM_MPC[par_name]) 82 | 83 | 84 | def get_default_parameters(sim_options): 85 | """ Which parameters are left to default ??""" 86 | default_keys = [key for key in DEFAULTS_PENDULUM_MPC if key not in sim_options] 87 | return default_keys 88 | 89 | 90 | def simulate_pendulum_MPC(sim_options): 91 | 92 | seed_val = get_parameter(sim_options,'seed_val') 93 | if seed_val is not None: 94 | np.random.seed(seed_val) 95 | 96 | Ac = get_parameter(sim_options, 'Ac') 97 | Bc = get_parameter(sim_options, 'Bc') 98 | 99 | Cc = np.array([[1., 0., 0., 0.], 100 | [0., 0., 1., 0.]]) 101 | 102 | Dc = np.zeros((2, 1)) 103 | 104 | [nx, nu] = Bc.shape # number of states and number or inputs 105 | ny = np.shape(Cc)[0] 106 | 107 | Ts_MPC = get_parameter(sim_options, 'Ts_MPC') 108 | ratio_Ts = int(Ts_MPC // Ts_fast) 109 | Ts_MPC = ((Ts_MPC // Ts_fast)) * Ts_fast # make Ts_MPC an integer multiple of Ts_fast 110 | 111 | # Brutal forward euler discretization 112 | Ad = np.eye(nx) + Ac*Ts_MPC 113 | Bd = Bc*Ts_MPC 114 | Cd = Cc 115 | Dd = Dc 116 | 117 | # Standard deviation of the measurement noise on position and angle 118 | 119 | std_npos = get_parameter(sim_options, 'std_npos') 120 | std_nphi = get_parameter(sim_options, 'std_nphi') 121 | 122 | # Force disturbance 123 | std_dF = get_parameter(sim_options, 'std_dF') 124 | 125 | # disturbance power spectrum 126 | w_F = get_parameter(sim_options, 'w_F') # bandwidth of the force disturbance 127 | tau_F = 1 / w_F 128 | Hu = control.TransferFunction([1], [1 / w_F, 1]) 129 | Hu = Hu * Hu 130 | Hud = control.matlab.c2d(Hu, Ts_fast) 131 | N_sim_imp = tau_F / Ts_fast * 20 132 | t_imp = np.arange(N_sim_imp) * Ts_fast 133 | t, y = control.impulse_response(Hud, t_imp) 134 | #y = y[0] 135 | std_tmp = np.sqrt(np.sum(y ** 2)) # np.sqrt(trapz(y**2,t)) 136 | Hu = Hu / (std_tmp) * std_dF 137 | 138 | 139 | N_skip = int(20 * tau_F // Ts_fast) # skip initial samples to get a regime sample of d 140 | t_sim_d = get_parameter(sim_options, 'len_sim') # simulation length (s) 141 | N_sim_d = int(t_sim_d // Ts_fast) 142 | N_sim_d = N_sim_d + N_skip 143 | e = np.random.randn(N_sim_d) 144 | te = np.arange(N_sim_d) * Ts_fast 145 | _, d = control.forced_response(Hu, te, e, return_x=False) 146 | d_fast = d[N_skip:] 147 | #td = np.arange(len(d)) * Ts_fast 148 | 149 | 150 | Np = get_parameter(sim_options, 'Np') 151 | Nc = get_parameter(sim_options, 'Nc') 152 | 153 | Qx = get_parameter(sim_options, 'Qx') 154 | QxN = get_parameter(sim_options, 'QxN') 155 | Qu = get_parameter(sim_options, 'Qu') 156 | QDu = get_parameter(sim_options, 'QDu') 157 | 158 | # Reference input and states 159 | xref_fun = get_parameter(sim_options, 'xref_fun') # reference state 160 | xref_fun_v = np.vectorize(xref_fun, signature='()->(n)') 161 | 162 | t0 = 0 163 | xref_MPC = xref_fun(t0) 164 | uref = get_parameter(sim_options, 'uref') 165 | uminus1 = np.array([0.0]) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref. 166 | 167 | # Constraints 168 | xmin = np.array([-1.0, -np.inf, -np.inf, -np.inf]) 169 | xmax = np.array([1.0, np.inf, np.inf, np.inf]) 170 | 171 | umin = np.array([-10]) 172 | umax = np.array([10]) 173 | 174 | Dumin = np.array([-100*Ts_MPC]) 175 | Dumax = np.array([100*Ts_MPC]) 176 | 177 | # Initialize simulation system 178 | phi0 = 10*2*np.pi/360 179 | x0 = np.array([0, 0, phi0, 0]) # initial state 180 | system_dyn = ode(f_ODE_wrapped).set_integrator('vode', method='bdf') # dopri5 181 | # system_dyn = ode(f_ODE_wrapped).set_integrator('dopri5') 182 | system_dyn.set_initial_value(x0, t0) 183 | system_dyn.set_f_params(0.0) 184 | 185 | QP_eps_rel = get_parameter(sim_options, 'QP_eps_rel') 186 | QP_eps_abs = get_parameter(sim_options, 'QP_eps_abs') 187 | 188 | # Emergency exit conditions 189 | 190 | EMERGENCY_STOP = False 191 | EMERGENCY_POS = 1.1 192 | EMERGENCY_ANGLE = 30*DEG_TO_RAD 193 | 194 | K = MPCController(Ad,Bd,Np=Np,Nc=Nc,x0=x0,xref=xref_MPC,uminus1=uminus1, 195 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 196 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax, 197 | eps_feas = 1e3, eps_rel= QP_eps_rel, eps_abs=QP_eps_abs) 198 | 199 | try: 200 | K.setup(solve=True) # setup initial problem and also solve it 201 | except: 202 | EMERGENCY_STOP = True 203 | 204 | if not EMERGENCY_STOP: 205 | if K.res.info.status != 'solved': 206 | EMERGENCY_STOP = True 207 | 208 | # Basic Kalman filter design 209 | Q_kal = get_parameter(sim_options, 'Q_kal') 210 | R_kal = get_parameter(sim_options, 'R_kal') 211 | try: 212 | L, P, W = kalman_design_simple(Ad, Bd, Cd, Dd, Q_kal, R_kal, type='predictor') 213 | except: 214 | EMERGENCY_STOP = True 215 | 216 | x0_est = x0 217 | KF = LinearStateEstimator(x0_est, Ad, Bd, Cd, Dd, L) 218 | 219 | # Simulate in closed loop 220 | len_sim = get_parameter(sim_options, 'len_sim') # simulation length (s) 221 | nsim = int(np.ceil(len_sim / Ts_MPC)) # simulation length(timesteps) # watch out! +1 added, is it correct? 222 | t_vec = np.zeros((nsim, 1)) 223 | t_calc_vec = np.zeros((nsim,1)) # computational time to get MPC solution (+ estimator) 224 | status_vec = np.zeros((nsim,1)) 225 | x_vec = np.zeros((nsim, nx)) 226 | x_ref_vec = np.zeros((nsim, nx)) 227 | y_vec = np.zeros((nsim, ny)) 228 | y_meas_vec = np.zeros((nsim, ny)) 229 | y_est_vec = np.zeros((nsim, ny)) 230 | x_est_vec = np.zeros((nsim, nx)) 231 | u_vec = np.zeros((nsim, nu)) 232 | x_MPC_pred = np.zeros((nsim, Np+1, nx)) # on-line predictions from the Kalman Filter 233 | 234 | nsim_fast = int(len_sim // Ts_fast) 235 | t_vec_fast = np.zeros((nsim_fast, 1)) 236 | x_vec_fast = np.zeros((nsim_fast, nx)) # finer integration grid for performance evaluation 237 | x_ref_vec_fast = np.zeros((nsim_fast, nx)) # finer integration grid for performance evaluatio 238 | u_vec_fast = np.zeros((nsim_fast, nu)) # finer integration grid for performance evaluatio 239 | Fd_vec_fast = np.zeros((nsim_fast, nu)) # 240 | t_int_vec_fast = np.zeros((nsim_fast, 1)) 241 | emergency_vec_fast = np.zeros((nsim_fast, 1)) # 242 | 243 | t_pred_all = t0 + np.arange(nsim + Np + 1) * Ts_MPC 244 | Xref_MPC_all = xref_fun_v(t_pred_all) 245 | 246 | t_step = t0 247 | x_step = x0 248 | u_MPC = None 249 | for idx_fast in range(nsim_fast): 250 | 251 | ## Determine step type: fast simulation only or MPC step 252 | idx_MPC = idx_fast // ratio_Ts 253 | run_MPC = (idx_fast % ratio_Ts) == 0 254 | 255 | # Output for step i 256 | # Ts_MPC outputs 257 | if run_MPC: # it is also a step of the simulation at rate Ts_MPC 258 | t_vec[idx_MPC, :] = t_step 259 | x_vec[idx_MPC, :] = x_step#system_dyn.y 260 | xref_MPC = xref_fun(t_step) # reference state 261 | t_pred = t_step + np.arange(Np + 1) * Ts_MPC 262 | x_ref_vec[idx_MPC,:] = xref_MPC 263 | 264 | if not EMERGENCY_STOP: 265 | # u_MPC, info_MPC = K.output(return_x_seq=True, return_status=True) # u[i] = k(\hat x[i]) possibly computed at time instant -1 266 | u_MPC, info_MPC = K.output(return_status=True) # u[i] = k(\hat x[i]) possibly computed at time instant -1 267 | else: 268 | u_MPC = np.zeros(nu) 269 | 270 | if not EMERGENCY_STOP: 271 | if info_MPC['status'] != 'solved': 272 | EMERGENCY_STOP = True 273 | 274 | if not EMERGENCY_STOP: 275 | pass 276 | #x_MPC_pred[idx_MPC, :, :] = info_MPC['x_seq'] # x_MPC_pred[i,i+1,...| possibly computed at time instant -1] 277 | u_vec[idx_MPC, :] = u_MPC 278 | 279 | y_step = Cd.dot(x_step) # y[i] from the system 280 | ymeas_step = np.copy(y_step) 281 | ymeas_step[0] += std_npos * np.random.randn() 282 | ymeas_step[1] += std_nphi * np.random.randn() 283 | y_vec[idx_MPC,:] = y_step 284 | y_meas_vec[idx_MPC,:] = ymeas_step 285 | if not EMERGENCY_STOP: 286 | status_vec[idx_MPC,:] = (info_MPC['status'] != 'solved') 287 | 288 | if np.abs(ymeas_step[0]) > EMERGENCY_POS or np.abs(ymeas_step[1]) > EMERGENCY_ANGLE: 289 | EMERGENCY_STOP = True 290 | 291 | 292 | # Ts_fast outputs 293 | t_vec_fast[idx_fast,:] = t_step 294 | x_vec_fast[idx_fast, :] = x_step #system_dyn.y 295 | x_ref_vec_fast[idx_fast, :] = xref_MPC 296 | u_fast = u_MPC + d_fast[idx_fast] 297 | u_vec_fast[idx_fast,:] = u_fast 298 | Fd_vec_fast[idx_fast,:] = d_fast[idx_fast] 299 | emergency_vec_fast[idx_fast,:] = EMERGENCY_STOP 300 | 301 | ## Update to step i+1 302 | 303 | # Controller simulation step at rate Ts_MPC 304 | if run_MPC: 305 | time_calc_start = time.perf_counter() 306 | # Kalman filter: update and predict 307 | #KF.update(ymeas_step) # \hat x[i|i] 308 | #KF.predict(u_MPC) # \hat x[i+1|i] 309 | KF.predict_update(u_MPC, ymeas_step) 310 | # MPC update 311 | if not EMERGENCY_STOP: 312 | Xref_MPC = Xref_MPC_all[idx_MPC:idx_MPC + Np + 1] 313 | K.update(KF.x, u_MPC, xref=Xref_MPC) # update with measurement and reference 314 | #K.update(KF.x, u_MPC, xref=xref_MPC) # update with measurement and reference 315 | t_calc_vec[idx_MPC,:] = time.perf_counter() - time_calc_start 316 | if t_calc_vec[idx_MPC,:] > 2 * Ts_MPC: 317 | EMERGENCY_STOP = True 318 | 319 | # System simulation step at rate Ts_fast 320 | time_integrate_start = time.perf_counter() 321 | system_dyn.set_f_params(u_fast) 322 | system_dyn.integrate(t_step + Ts_fast) 323 | x_step = system_dyn.y 324 | #x_step = x_step + f_ODE_jit(t_step, x_step, u_fast)*Ts_fast 325 | #x_step = x_step + f_ODE(0.0, x_step, u_fast) * Ts_fast 326 | t_int_vec_fast[idx_fast,:] = time.perf_counter() - time_integrate_start 327 | 328 | # Time update 329 | t_step += Ts_fast 330 | 331 | simout = {'t': t_vec, 'x': x_vec, 'u': u_vec, 'y': y_vec, 'y_meas': y_meas_vec, 'x_ref': x_ref_vec, 'x_MPC_pred': x_MPC_pred, 'status': status_vec, 'Fd_fast': Fd_vec_fast, 332 | 't_fast': t_vec_fast, 'x_fast': x_vec_fast, 'x_ref_fast': x_ref_vec_fast, 'u_fast': u_vec_fast, 'emergency_fast': emergency_vec_fast, 333 | 'KF': KF, 'K': K, 'nsim': nsim, 'Ts_MPC': Ts_MPC, 't_calc': t_calc_vec, 334 | 't_int_fast': t_int_vec_fast 335 | } 336 | 337 | return simout 338 | 339 | 340 | if __name__ == '__main__': 341 | 342 | import matplotlib.pyplot as plt 343 | import matplotlib 344 | 345 | 346 | plt.close('all') 347 | 348 | simopt = DEFAULTS_PENDULUM_MPC 349 | 350 | time_sim_start = time.perf_counter() 351 | simout = simulate_pendulum_MPC(simopt) 352 | time_sim = time.perf_counter() - time_sim_start 353 | 354 | t = simout['t'] 355 | x = simout['x'] 356 | u = simout['u'] 357 | y = simout['y'] 358 | y_meas = simout['y_meas'] 359 | x_ref = simout['x_ref'] 360 | x_MPC_pred = simout['x_MPC_pred'] 361 | x_fast = simout['x_fast'] 362 | u_fast = simout['u_fast'] 363 | 364 | t_fast = simout['t_fast'] 365 | x_ref_fast = simout['x_ref_fast'] 366 | Fd_fast = simout['Fd_fast'] 367 | KF = simout['KF'] 368 | status = simout['status'] 369 | 370 | uref = get_parameter(simopt, 'uref') 371 | Np = get_parameter(simopt, 'Np') 372 | nsim = len(t) 373 | nx = x.shape[1] 374 | ny = y.shape[1] 375 | 376 | y_ref = x_ref[:, [0, 2]] 377 | y_OL_pred = np.zeros((nsim-Np-1, Np+1, ny)) # on-line predictions from the Kalman Filter 378 | y_MPC_pred = x_MPC_pred[:, :, [0, 2]] # how to vectorize C * x_MPC_pred?? 379 | y_MPC_err = np.zeros(np.shape(y_OL_pred)) 380 | y_OL_err = np.zeros(np.shape(y_OL_pred)) 381 | for i in range(nsim-Np-1): 382 | u_init = u[i:i+Np+1, :] 383 | x_init = x[i,:] 384 | y_OL_pred[i,:,:] = KF.sim(u_init,x_init) 385 | y_OL_err[i, :, :] = y_OL_pred[i, :, :] - y_meas[i:i + Np + 1] 386 | y_MPC_err[i, :, :] = y_MPC_pred[i, :, :] - y_meas[i:i + Np + 1] 387 | 388 | fig,axes = plt.subplots(3,1, figsize=(10,10)) 389 | axes[0].plot(t, y_meas[:, 0], "b", label='p_meas') 390 | axes[0].plot(t_fast, x_fast[:, 0], "k", label='p') 391 | axes[0].plot(t, y_ref[:,0], "k--", label="p_ref") 392 | idx_pred = 0 393 | axes[0].plot(t[idx_pred:idx_pred+Np+1], y_OL_pred[0, :, 0], 'r', label='Off-line k-step prediction') 394 | axes[0].plot(t[idx_pred:idx_pred+Np+1], y_MPC_pred[0, :, 0], 'c', label='MPC k-step prediction' ) 395 | axes[0].plot(t[idx_pred:idx_pred+Np+1], y_OL_err[0, :, 0], 'r--', label='Off-line prediction error') 396 | axes[0].plot(t[idx_pred:idx_pred+Np+1], y_MPC_err[0, :, 0], 'c--', label='MPC prediction error') 397 | axes[0].set_ylim(-0.2,1.0) 398 | axes[0].set_title("Position (m)") 399 | 400 | 401 | axes[1].plot(t, y_meas[:, 1]*RAD_TO_DEG, "b", label='phi_meas') 402 | axes[1].plot(t_fast, x_fast[:, 2]*RAD_TO_DEG, 'k', label="phi") 403 | idx_pred = 0 404 | axes[1].plot(t[idx_pred:idx_pred+Np+1], y_OL_pred[0, :, 1]*RAD_TO_DEG, 'r', label='Off-line k-step prediction') 405 | axes[1].plot(t[idx_pred:idx_pred+Np+1], y_MPC_pred[0, :, 1]*RAD_TO_DEG, 'c',label='MPC k-step prediction' ) 406 | axes[1].plot(t[idx_pred:idx_pred+Np+1], y_OL_err[0, :, 1]*RAD_TO_DEG, 'r--', label='Off-line prediction error' ) 407 | axes[1].plot(t[idx_pred:idx_pred+Np+1], y_MPC_err[0, :, 1]*RAD_TO_DEG, 'c--', label='MPC prediction error') 408 | axes[1].set_ylim(-20,20) 409 | axes[1].set_title("Angle (deg)") 410 | 411 | axes[2].plot(t, u[:,0], label="u") 412 | axes[2].plot(t, uref*np.ones(np.shape(t)), "r--", label="u_ref") 413 | axes[2].set_ylim(-8,8) 414 | axes[2].set_title("Force (N)") 415 | 416 | for ax in axes: 417 | ax.grid(True) 418 | ax.legend() 419 | 420 | # In[MPC computational time] 421 | t_calc = simout['t_calc'] 422 | fig, ax = plt.subplots(1, 1, figsize=(5, 5)) 423 | ax.hist(t_calc * 1000, bins=100) 424 | ax.set_title('MPC computation time)') 425 | ax.grid(True) 426 | ax.set_xlabel('Time (ms)') 427 | 428 | # In[ODE integration time] 429 | t_int = simout['t_int_fast'] 430 | fig, ax = plt.subplots(1, 1, figsize=(5, 5)) 431 | ax.hist(t_int * 1e3, bins=100) 432 | ax.set_title('ODE integration time)') 433 | ax.grid(True) 434 | ax.set_xlabel('Time (ms)') 435 | 436 | # In[MPC computation time ] 437 | fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True) 438 | axes[0].plot(t, y_meas[:, 0], "b", label='p_meas') 439 | axes[0].plot(t_fast, x_fast[:, 0], "k", label='p') 440 | axes[0].step(t, y_ref[:, 0], "k--", where='post', label="p_ref") 441 | axes[0].set_ylim(-0.2, 1.0) 442 | axes[0].set_xlabel("Simulation time (s)") 443 | axes[0].set_ylabel("Position (m)") 444 | 445 | axes[1].step(t, t_calc[:, 0] * 1e3, "b", where='post', label='T_MPC') 446 | axes[1].set_xlabel("Simulation time (s)") 447 | axes[1].set_ylabel("MPC time (ms)") 448 | axes[1].set_ylim(0, 4.0) 449 | axes[2].step(t_fast[1:], t_int[1:, 0] * 1e3, "b", where='post', label='T_ODE') # why is 1st slow? check numba 450 | axes[2].set_xlabel("Simulation time (s)") 451 | axes[2].set_ylabel("ODE time (ms)") 452 | axes[2].set_ylim(0, 0.3) 453 | axes[3].step(t, u[:, 0], where='post', label="F") 454 | axes[3].step(t_fast, u_fast[:, 0], where='post', label="F_d") 455 | axes[3].set_xlabel("Simulation time (s)") 456 | axes[3].set_ylabel("Force (N)") 457 | 458 | for ax in axes: 459 | ax.grid(True) 460 | ax.legend() 461 | 462 | time_integrate = np.sum(simout['t_int_fast']) 463 | time_MPC = np.sum(simout['t_calc']) 464 | print(f'Total time: {time_sim:.2f}, Integration time: {time_integrate:.2f}, MPC time:{time_MPC:.2f}') 465 | 466 | Ts_MPC = simout['Ts_MPC'] 467 | -------------------------------------------------------------------------------- /pendulum_model.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # Constants # 4 | M = 0.5 5 | m = 0.2 6 | b = 0.1 7 | ftheta = 0.1 8 | l = 0.3 9 | g = 9.81 10 | RAD_TO_DEG = 360.0/2.0/np.pi 11 | DEG_TO_RAD = 1.0/RAD_TO_DEG 12 | 13 | # Nonlinear dynamics ODE: 14 | # \dot x = f_ODE(x,u) 15 | from numba import jit 16 | 17 | @jit("float64[:](float64,float64[:],float64[:])", nopython=True, cache=True) 18 | def f_ODE_jit(t, x, u): 19 | F = u[0] 20 | v = x[1] 21 | theta = x[2] 22 | omega = x[3] 23 | der = np.zeros(x.shape) 24 | der[0] = v 25 | der[1] = (m * l * np.sin(theta) * omega ** 2 - m * g * np.sin(theta) * np.cos(theta) + m * ftheta * np.cos( 26 | theta) * omega + F - b * v) / (M + m * (1 - np.cos(theta) ** 2)) 27 | der[2] = omega 28 | der[3] = ((M + m) * (g * np.sin(theta) - ftheta * omega) - m * l * omega ** 2 * np.sin(theta) * np.cos( 29 | theta) - ( 30 | F - b * v) * np.cos(theta)) / (l * (M + m * (1 - np.cos(theta) ** 2))) 31 | return der 32 | 33 | 34 | def f_ODE_wrapped(t,x,u): 35 | return f_ODE_jit(t,x,u) 36 | 37 | def f_ODE(t, x, u): 38 | F = u 39 | v = x[1] 40 | theta = x[2] 41 | omega = x[3] 42 | der = np.zeros(x.shape) 43 | der[0] = v 44 | der[1] = (m * l * np.sin(theta) * omega ** 2 - m * g * np.sin(theta) * np.cos(theta) + m * ftheta * np.cos( 45 | theta) * omega + F - b * v) / (M + m * (1 - np.cos(theta) ** 2)) 46 | der[2] = omega 47 | der[3] = ((M + m) * (g * np.sin(theta) - ftheta * omega) - m * l * omega ** 2 * np.sin(theta) * np.cos( 48 | theta) - ( 49 | F - b * v) * np.cos(theta)) / (l * (M + m * (1 - np.cos(theta) ** 2))) 50 | return der 51 | 52 | 53 | if __name__ == '__main__': 54 | f_ODE_jit(0.1, np.array([0.3, 0.2, 0.1, 0.4]), 3.1) 55 | -------------------------------------------------------------------------------- /pyMPC/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/pyMPC/__init__.py -------------------------------------------------------------------------------- /pyMPC/mpc.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy as sp 3 | import scipy.sparse as sparse 4 | import osqp 5 | import warnings 6 | 7 | 8 | def __is_vector__(vec): 9 | if vec.ndim == 1: 10 | return True 11 | else: 12 | if vec.ndim == 2: 13 | if vec.shape[0] == 1 or vec.shape[1] == 0: 14 | return True 15 | else: 16 | return False 17 | return False 18 | 19 | 20 | def __is_matrix__(mat): 21 | if mat.ndim == 2: 22 | return True 23 | else: 24 | return False 25 | 26 | class MPCController: 27 | """ This class implements an MPC controller 28 | 29 | Attributes 30 | ---------- 31 | Ad : 2D array_like. Size: (nx, nx) 32 | Discrete-time system matrix Ad. 33 | Bd : 2D array-like. Size: (nx, nu) 34 | Discrete-time system matrix Bd. 35 | Np : int 36 | Prediction horizon. Default value: 20. 37 | Nc : int 38 | Control horizon. It must be lower or equal to Np. If None, it is set equal to Np. 39 | x0 : 1D array_like. Size: (nx,) 40 | System state at time instant 0. If None, it is set to np.zeros(nx) 41 | xref : 1D array-like. Size: (nx,) 42 | System state reference (aka target, set-point). 43 | uref : 1D array-like. Size: (nu, ). 44 | System input reference. If None, it is set to np.zeros(nx) 45 | uminus1 : 1D array_like 46 | Input value assumed at time instant -1. If None, it is set to uref. 47 | Qx : 2D array_like 48 | State weight matrix. If None, it is set to eye(nx). 49 | Qu : 2D array_like 50 | Input weight matrix. If None, it is set to zeros((nu,nu)). 51 | QDu : 2D array_like 52 | Input delta weight matrix. If None, it is set to zeros((nu,nu)). 53 | xmin : 1D array_like 54 | State minimum value. If None, it is set to -np.inf*ones(nx). 55 | xmax : 1D array_like 56 | State maximum value. If None, it is set to np.inf*ones(nx). 57 | umin : 1D array_like 58 | Input minimum value. If None, it is set to -np.inf*ones(nx). 59 | umax : 1D array_like 60 | Input maximum value. If None, it is set to np.inf*ones(nx). 61 | Dumin : 1D array_like 62 | Input variation minimum value. If None, it is set to np.inf*ones(nx). 63 | Dumax : 1D array_like 64 | Input variation maximum value. If None, it is set to np.inf*ones(nx). 65 | eps_feas : float 66 | Scale factor for the matrix Q_eps. Q_eps = eps_feas*eye(nx). 67 | eps_rel : float 68 | Relative tolerance of the QP solver. Default value: 1e-3. 69 | eps_abs : float 70 | Absolute tolerance of the QP solver. Default value: 1e-3. 71 | """ 72 | 73 | def __init__(self, Ad, Bd, Np=20, Nc=None, 74 | x0=None, xref=None, uref=None, uminus1=None, 75 | Qx=None, QxN=None, Qu=None, QDu=None, 76 | xmin=None, xmax=None, umin=None,umax=None,Dumin=None,Dumax=None, 77 | eps_feas=1e6, eps_rel=1e-3, eps_abs=1e-3): 78 | 79 | if __is_matrix__(Ad) and (Ad.shape[0] == Ad.shape[1]): 80 | self.Ad = Ad 81 | self.nx = Ad.shape[0] # number of states 82 | else: 83 | raise ValueError("Ad should be a square matrix of dimension (nx,nx)!") 84 | 85 | if __is_matrix__(Bd) and Bd.shape[0] == self.nx: 86 | self.Bd = Bd 87 | self.nu = Bd.shape[1] # number of inputs 88 | else: 89 | raise ValueError("Bd should be a matrix of dimension (nx, nu)!") 90 | 91 | if Np > 1: 92 | self.Np = Np # assert 93 | else: 94 | raise ValueError("Np should be > 1!") 95 | 96 | if Nc is not None: 97 | if Nc <= Np: 98 | self.Nc = Nc 99 | else: 100 | raise ValueError("Nc should be <= Np!") 101 | else: 102 | self.Nc = self.Np 103 | 104 | # x0 handling 105 | if x0 is not None: 106 | if __is_vector__(x0) and x0.size == self.nx: 107 | self.x0 = x0.ravel() 108 | else: 109 | raise ValueError("nx should be an array of dimension (nx,)!") 110 | else: 111 | self.x0 = np.zeros(self.nx) 112 | 113 | # reference handing 114 | if xref is not None: 115 | if __is_vector__(xref) and xref.size == self.nx: 116 | self.xref = xref.ravel() 117 | elif __is_matrix__(xref) and xref.shape[1] == self.nx and xref.shape[0] >= self.Np: 118 | self.xref = xref 119 | else: 120 | raise ValueError("xref should be either a vector of shape (nx,) or a matrix of shape (Np+1, nx)!") 121 | else: 122 | self.xref = np.zeros(self.nx) 123 | 124 | if uref is not None: 125 | if __is_vector__(uref) and uref.size == self.nu: 126 | self.uref = uref.ravel() # assert... 127 | else: 128 | raise ValueError("uref should be a vector of shape (nu,)!") 129 | else: 130 | self.uref = np.zeros(self.nu) 131 | 132 | if uminus1 is not None: 133 | if __is_vector__(uminus1) and uminus1.size == self.nu: 134 | self.uminus1 = uminus1 135 | else: 136 | raise ValueError("uminus1 should be a vector of shape (nu,)!") 137 | else: 138 | self.uminus1 = self.uref 139 | 140 | # weights handling 141 | if Qx is not None: 142 | if __is_matrix__(Qx) and Qx.shape[0] == self.nx and Qx.shape[1] == self.nx: 143 | self.Qx = Qx 144 | else: 145 | raise ValueError("Qx should be a matrix of shape (nx,nx)!") 146 | else: 147 | self.Qx = np.zeros((self.nx, self.nx)) # sparse 148 | 149 | if QxN is not None: 150 | if __is_matrix__(QxN) and QxN.shape[0] == self.nx and Qx.shape[1] == self.nx: 151 | self.QxN = QxN 152 | else: 153 | raise ValueError("QxN should be a square matrix of shape (nx,nx)!") 154 | else: 155 | self.QxN = self.Qx # sparse 156 | 157 | if Qu is not None: 158 | if __is_matrix__(Qu) and Qu.shape[0] == self.nu and Qu.shape[1] == self.nu: 159 | self.Qu = Qu 160 | else: 161 | raise ValueError("Qu should be a square matrix of shape (nu,nu)!") 162 | else: 163 | self.Qu = np.zeros((self.nu, self.nu)) 164 | 165 | if QDu is not None: 166 | if __is_matrix__(QDu) and QDu.shape[0] == self.nu and QDu.shape[1] == self.nu: 167 | self.QDu = QDu 168 | else: 169 | raise ValueError("QDu should be a square matrix of shape (nu, nu)!") 170 | else: 171 | self.QDu = np.zeros((self.nu, self.nu)) 172 | 173 | # constraints handling 174 | if xmin is not None: 175 | if __is_vector__(xmin) and xmin.size == self.nx: 176 | self.xmin = xmin.ravel() # assert... 177 | else: 178 | raise ValueError("xmin should be a vector of shape (nx,)!") 179 | else: 180 | self.xmin = -np.ones(self.nx)*np.inf 181 | 182 | if xmax is not None: 183 | if __is_vector__(xmax) and xmax.size == self.nx: 184 | self.xmax = xmax # assert... 185 | else: 186 | raise ValueError("xmax should be a vector of shape (nx,)!") 187 | else: 188 | self.xmax = np.ones(self.nx)*np.inf 189 | 190 | if umin is not None: 191 | if __is_vector__(umin) and umin.size == self.nu: 192 | self.umin = umin # assert... 193 | else: 194 | raise ValueError("umin should be a vector of shape (nu,)!") 195 | else: 196 | self.umin = -np.ones(self.nu)*np.inf 197 | 198 | if umax is not None: 199 | if __is_vector__(umax) and umax.size == self.nu: 200 | self.umax = umax # assert... 201 | else: 202 | raise ValueError("umax should be a vector of shape (nu,)!") 203 | else: 204 | self.umax = np.ones(self.nu)*np.inf 205 | 206 | if Dumin is not None: 207 | if __is_vector__(Dumin) and Dumin.size == self.nu: 208 | self.Dumin = Dumin # assert... 209 | else: 210 | raise ValueError("Dumin should be a vector of shape (nu,)!") 211 | else: 212 | self.Dumin = -np.ones(self.nu)*np.inf 213 | 214 | if Dumax is not None: 215 | if __is_vector__(Dumax) and Dumax.size == self.nu: 216 | self.Dumax = Dumax # assert... 217 | else: 218 | raise ValueError("Dumax should be a vector of shape (nu,)!") 219 | else: 220 | self.Dumax = np.ones(self.nu)*np.inf 221 | 222 | self.eps_feas = eps_feas 223 | self.Qeps = eps_feas * sparse.eye(self.nx) 224 | 225 | self.eps_rel = eps_rel 226 | self.eps_abs = eps_abs 227 | 228 | self.u_failure = self.uref # value provided when the MPC solver fails. 229 | 230 | self.raise_error = False # Raise an error when MPC optimization fails 231 | self.JX_ON = True # Cost function terms in X active 232 | self.JU_ON = True # Cost function terms in U active 233 | self.JDU_ON = True # Cost function terms in Delta U active 234 | self.SOFT_ON = True # Soft constraints active 235 | 236 | self.prob = osqp.OSQP() 237 | 238 | self.res = None 239 | self.P = None 240 | self.q = None 241 | self.A = None 242 | self.l = None 243 | self.u = None 244 | self.x0_rh = None 245 | self.uminus1_rh = None 246 | self.J_CNST = None # Constant term of the cost function 247 | 248 | def setup(self, solve=True): 249 | """ Set-up the QP problem. 250 | 251 | Parameters 252 | ---------- 253 | solve : bool 254 | If True, also solve the QP problem. 255 | 256 | """ 257 | self.x0_rh = np.copy(self.x0) 258 | self.uminus1_rh = np.copy(self.uminus1) 259 | self._compute_QP_matrices_() 260 | self.prob.setup(self.P, self.q, self.A, self.l, self.u, warm_start=True, verbose=False, eps_abs=self.eps_rel, eps_rel=self.eps_abs) 261 | 262 | if solve: 263 | self.solve() 264 | 265 | def output(self, return_x_seq=False, return_u_seq=False, return_eps_seq=False, return_status=False, return_obj_val=False): 266 | """ Return the MPC controller output uMPC, i.e., the first element of the optimal input sequence and assign is to self.uminus1_rh. 267 | 268 | 269 | Parameters 270 | ---------- 271 | return_x_seq : bool 272 | If true, the method also returns the optimal sequence of states 273 | 274 | Returns 275 | ------- 276 | array_like (nu,) 277 | The first element of the optimal input sequence uMPC to be applied to the system. 278 | """ 279 | Nc = self.Nc 280 | Np = self.Np 281 | nx = self.nx 282 | nu = self.nu 283 | 284 | # Extract first control input to the plant 285 | if self.res.info.status == 'solved': 286 | uMPC = self.res.x[(Np+1)*nx:(Np+1)*nx + nu] 287 | else: 288 | uMPC = self.u_failure 289 | 290 | # Return additional info? 291 | info = {} 292 | if return_x_seq: 293 | seq_X = self.res.x[0:(Np+1)*nx] 294 | seq_X = seq_X.reshape(-1,nx) 295 | info['x_seq'] = seq_X 296 | 297 | if return_u_seq: 298 | seq_U = self.res.x[(Np+1)*nx:(Np+1)*nx + Nc*nu] 299 | seq_U = seq_U.reshape(-1,nu) 300 | info['u_seq'] = seq_U 301 | 302 | if return_eps_seq: 303 | seq_eps = self.res.x[(Np+1)*nx + Nc*nu : (Np+1)*nx + Nc*nu + (Np+1)*nx ] 304 | seq_eps = seq_eps.reshape(-1,nx) 305 | info['eps_seq'] = seq_eps 306 | 307 | if return_status: 308 | info['status'] = self.res.info.status 309 | 310 | if return_obj_val: 311 | obj_val = self.res.info.obj_val + self.J_CNST # constant of the objective value 312 | info['obj_val'] = obj_val 313 | 314 | self.uminus1_rh = uMPC 315 | 316 | if len(info) == 0: 317 | return uMPC 318 | 319 | else: 320 | return uMPC, info 321 | 322 | def update(self,x,u=None,xref=None,solve=True): 323 | """ Update the QP problem. 324 | 325 | Parameters 326 | ---------- 327 | x : array_like. Size: (nx,) 328 | The new value of x0. 329 | 330 | u : array_like. Size: (nu,) 331 | The new value of uminus1. If none, it is set to the previously computed u. 332 | 333 | xref : array_like. Size: (nx,) 334 | The new value of xref. If none, it is not changed 335 | 336 | solve : bool 337 | If True, also solve the QP problem. 338 | 339 | """ 340 | 341 | self.x0_rh = x # previous x0 342 | if u is not None: 343 | self.uminus1_rh = u # otherwise it is just the uMPC updated from the previous step() call 344 | if xref is not None: 345 | self.xref = xref # TODO: check that new reference is != old reference, do a minimal update of the QP matrices to improve speed 346 | self._update_QP_matrices_() 347 | if solve: 348 | self.solve() 349 | 350 | def solve(self): 351 | """ Solve the QP problem. """ 352 | 353 | self.res = self.prob.solve() 354 | 355 | # Check solver status 356 | if self.res.info.status != 'solved': 357 | warnings.warn('OSQP did not solve the problem!') 358 | if self.raise_error: 359 | raise ValueError('OSQP did not solve the problem!') 360 | 361 | def __controller_function__(self, x, u, xref=None): 362 | """ This function is meant to be used for debug only. 363 | """ 364 | 365 | self.update(x,u,xref=xref,solve=True) 366 | uMPC = self.output() 367 | 368 | return uMPC 369 | 370 | 371 | def _update_QP_matrices_(self): 372 | x0_rh = self.x0_rh 373 | uminus1_rh = self.uminus1_rh 374 | Np = self.Np 375 | Nc = self.Nc 376 | nx = self.nx 377 | nu = self.nu 378 | Dumin = self.Dumin 379 | Dumax = self.Dumax 380 | QDu = self.QDu 381 | uref = self.uref 382 | Qeps = self.Qeps 383 | Qx = self.Qx 384 | QxN = self.QxN 385 | Qu = self.Qu 386 | xref = self.xref 387 | P_X = self.P_X 388 | 389 | self.l[:nx] = -x0_rh 390 | self.u[:nx] = -x0_rh 391 | 392 | self.l[(Np+1)*nx + (Np+1)*nx + (Nc)*nu:(Np+1)*nx + (Np+1)*nx + (Nc)*nu + nu] = Dumin + uminus1_rh[0:nu] # update constraint on \Delta u0: Dumin <= u0 - u_{-1} 393 | self.u[(Np+1)*nx + (Np+1)*nx + (Nc)*nu:(Np+1)*nx + (Np+1)*nx + (Nc)*nu + nu] = Dumax + uminus1_rh[0:nu] # update constraint on \Delta u0: u0 - u_{-1} <= Dumax 394 | 395 | # Update the linear term q. This part could be further optimized in case of constant xref... 396 | q_X = np.zeros((Np + 1) * nx) # x_N 397 | self.J_CNST = 0.0 398 | if self.JX_ON: 399 | # self.J_CNST += 1/2*Np*(xref.dot(QxN.dot(xref))) + 1/2*xref.dot(QxN.dot(xref)) # TODO adjust for non-constant xref 400 | 401 | if xref.ndim == 2 and xref.shape[0] >= Np + 1: # xref is a vector per time-instant! experimental feature 402 | #for idx_ref in range(Np): 403 | # q_X[idx_ref * nx:(idx_ref + 1) * nx] += -Qx.dot(xref[idx_ref, :]) 404 | #q_X[Np * nx:(Np + 1) * nx] += -QxN.dot(xref[Np, :]) 405 | q_X += (-xref.reshape(1, -1) @ (P_X)).ravel() # way faster implementation of the same formula commented above 406 | else: 407 | q_X += np.hstack([np.kron(np.ones(Np), -Qx.dot(xref)), # x0... x_N-1 408 | -QxN.dot(xref)]) # x_N 409 | else: 410 | pass 411 | 412 | q_U = np.zeros(Nc*nu) 413 | if self.JU_ON: 414 | self.J_CNST += 1/2* Np * (uref.dot(Qu.dot(uref))) 415 | if self.Nc == self.Np: 416 | q_U += np.kron(np.ones(Nc), -Qu.dot(uref)) 417 | else: # Nc < Np. This formula is more general and could handle the case Nc = Np either. TODO: test 418 | iU = np.ones(Nc) 419 | iU[Nc-1] = (Np - Nc + 1) 420 | q_U += np.kron(iU, -Qu.dot(uref)) 421 | 422 | # Filling P and q for J_DU 423 | if self.JDU_ON: 424 | self.J_CNST += 1/2*uminus1_rh.dot((QDu).dot(uminus1_rh)) 425 | q_U += np.hstack([-QDu.dot(uminus1_rh), # u0 426 | np.zeros((Nc - 1) * nu)]) # u1..uN-1 427 | else: 428 | pass 429 | 430 | if self.SOFT_ON: 431 | q_eps = np.zeros((Np+1)*nx) 432 | self.q = np.hstack([q_X, q_U, q_eps]) 433 | else: 434 | self.q = np.hstack([q_X, q_U]) 435 | 436 | self.prob.update(l=self.l, u=self.u, q=self.q) 437 | 438 | def _compute_QP_matrices_(self): 439 | Np = self.Np 440 | Nc = self.Nc 441 | nx = self.nx 442 | nu = self.nu 443 | Qx = self.Qx 444 | QxN = self.QxN 445 | Qu = self.Qu 446 | QDu = self.QDu 447 | xref = self.xref 448 | uref = self.uref 449 | uminus1 = self.uminus1 450 | Ad = self.Ad 451 | Bd = self.Bd 452 | x0 = self.x0 453 | xmin = self.xmin 454 | xmax = self.xmax 455 | umin = self.umin 456 | umax = self.umax 457 | Dumin = self.Dumin 458 | Dumax = self.Dumax 459 | Qeps = self.Qeps 460 | 461 | # Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1)) 462 | # - quadratic objective 463 | 464 | P_X = sparse.csc_matrix(((Np+1)*nx, (Np+1)*nx)) 465 | q_X = np.zeros((Np+1)*nx) # x_N 466 | self.J_CNST = 0.0 467 | if self.JX_ON: 468 | P_X += sparse.block_diag([sparse.kron(sparse.eye(Np), Qx), # x0...x_N-1 469 | QxN]) # xN 470 | 471 | if xref.ndim == 2 and xref.shape[0] >= Np + 1: # xref is a vector per time-instant! experimental feature 472 | #for idx_ref in range(Np): 473 | # q_X[idx_ref * nx:(idx_ref + 1) * nx] += -Qx.dot(xref[idx_ref, :]) 474 | #q_X[Np * nx:(Np + 1) * nx] += -QxN.dot(xref[Np, :]) 475 | q_X += (-xref.reshape(1, -1) @ (P_X)).ravel() 476 | else: 477 | q_X += np.hstack([np.kron(np.ones(Np), -Qx.dot(xref)), # x0... x_N-1 478 | -QxN.dot(xref)]) # x_N 479 | 480 | # self.J_CNST += 1/2*Np*(xref.dot(QxN.dot(xref))) + 1/2*xref.dot(QxN.dot(xref)) # TODO adapt for non-constant xref 481 | else: 482 | pass 483 | 484 | # Filling P and q for J_U 485 | P_U = sparse.csc_matrix((Nc*nu, Nc*nu)) 486 | q_U = np.zeros(Nc*nu) 487 | if self.JU_ON: 488 | self.J_CNST += 1/2*Np*(uref.dot(Qu.dot(uref))) 489 | 490 | if self.Nc == self.Np: 491 | P_U += sparse.kron(sparse.eye(Nc), Qu) 492 | q_U += np.kron(np.ones(Nc), -Qu.dot(uref)) 493 | else: # Nc < Np. This formula is more general and could handle the case Nc = Np either. TODO: test 494 | iU = np.ones(Nc) 495 | iU[Nc-1] = (Np - Nc + 1) 496 | P_U += sparse.kron(sparse.diags(iU), Qu) 497 | q_U += np.kron(iU, -Qu.dot(uref)) 498 | 499 | # Filling P and q for J_DU 500 | if self.JDU_ON: 501 | self.J_CNST += 1/2*uminus1.dot((QDu).dot(uminus1)) 502 | iDu = 2 * np.eye(Nc) - np.eye(Nc, k=1) - np.eye(Nc, k=-1) 503 | iDu[Nc - 1, Nc - 1] = 1 504 | P_U += sparse.kron(iDu, QDu) 505 | q_U += np.hstack([-QDu.dot(uminus1), # u0 506 | np.zeros((Nc - 1) * nu)]) # u1..uN-1 507 | else: 508 | pass 509 | 510 | if self.SOFT_ON: 511 | P_eps = sparse.kron(np.eye((Np+1)), Qeps) 512 | q_eps = np.zeros((Np+1)*nx) 513 | 514 | # Linear constraints 515 | 516 | # - linear dynamics x_k+1 = Ax_k + Bu_k 517 | Ax = sparse.kron(sparse.eye(Np + 1), -sparse.eye(nx)) + sparse.kron(sparse.eye(Np + 1, k=-1), Ad) 518 | iBu = sparse.vstack([sparse.csc_matrix((1, Nc)), 519 | sparse.eye(Nc)]) 520 | if self.Nc < self.Np: # expand A matrix if Nc < Nu (see notes) 521 | iBu = sparse.vstack([iBu, 522 | sparse.hstack([sparse.csc_matrix((Np - Nc, Nc - 1)), np.ones((Np - Nc, 1))]) 523 | ]) 524 | Bu = sparse.kron(iBu, Bd) 525 | 526 | n_eps = (Np + 1) * nx 527 | Aeq_dyn = sparse.hstack([Ax, Bu]) 528 | if self.SOFT_ON: 529 | Aeq_dyn = sparse.hstack([Aeq_dyn, sparse.csc_matrix((Aeq_dyn.shape[0], n_eps))]) # For soft constraints slack variables 530 | 531 | leq_dyn = np.hstack([-x0, np.zeros(Np * nx)]) 532 | ueq_dyn = leq_dyn # for equality constraints -> upper bound = lower bound! 533 | 534 | # - bounds on x 535 | Aineq_x = sparse.hstack([sparse.eye((Np + 1) * nx), sparse.csc_matrix(((Np+1)*nx, Nc*nu))]) 536 | if self.SOFT_ON: 537 | Aineq_x = sparse.hstack([Aineq_x, sparse.eye(n_eps)]) # For soft constraints slack variables 538 | lineq_x = np.kron(np.ones(Np + 1), xmin) # lower bound of inequalities 539 | uineq_x = np.kron(np.ones(Np + 1), xmax) # upper bound of inequalities 540 | 541 | Aineq_u = sparse.hstack([sparse.csc_matrix((Nc*nu, (Np+1)*nx)), sparse.eye(Nc * nu)]) 542 | if self.SOFT_ON: 543 | Aineq_u = sparse.hstack([Aineq_u, sparse.csc_matrix((Aineq_u.shape[0], n_eps))]) # For soft constraints slack variables 544 | lineq_u = np.kron(np.ones(Nc), umin) # lower bound of inequalities 545 | uineq_u = np.kron(np.ones(Nc), umax) # upper bound of inequalities 546 | 547 | 548 | # - bounds on \Delta u 549 | Aineq_du = sparse.vstack([sparse.hstack([np.zeros((Np + 1) * nx), np.ones(nu), np.zeros((Nc - 1) * nu)]), # for u0 - u-1 550 | sparse.hstack([np.zeros((Nc * nu, (Np+1) * nx)), -sparse.eye(Nc * nu) + sparse.eye(Nc * nu, k=1)]) # for uk - uk-1, k=1...Np 551 | ] 552 | ) 553 | if self.SOFT_ON: 554 | Aineq_du = sparse.hstack([Aineq_du, sparse.csc_matrix((Aineq_du.shape[0], n_eps))]) 555 | 556 | uineq_du = np.ones((Nc+1) * nu)*Dumax 557 | uineq_du[0:nu] += self.uminus1[0:nu] 558 | 559 | lineq_du = np.ones((Nc+1) * nu)*Dumin 560 | lineq_du[0:nu] += self.uminus1[0:nu] # works for nonscalar u? 561 | 562 | # Positivity of slack variables (not necessary!) 563 | #Aineq_eps_pos = sparse.hstack([sparse.coo_matrix((n_eps,(Np+1)*nx)), sparse.coo_matrix((n_eps, Np*nu)), sparse.eye(n_eps)]) 564 | #lineq_eps_pos = np.zeros(n_eps) 565 | #uineq_eps_pos = np.ones(n_eps)*np.inf 566 | 567 | # - OSQP constraints 568 | #A = sparse.vstack([Aeq_dyn, Aineq_x, Aineq_u, Aineq_du, Aineq_eps_pos]).tocsc() 569 | #l = np.hstack([leq_dyn, lineq_x, lineq_u, lineq_du, lineq_eps_pos]) 570 | #u = np.hstack([ueq_dyn, uineq_x, uineq_u, uineq_du, uineq_eps_pos]) 571 | 572 | A = sparse.vstack([Aeq_dyn, Aineq_x, Aineq_u, Aineq_du]).tocsc() 573 | l = np.hstack([leq_dyn, lineq_x, lineq_u, lineq_du]) 574 | u = np.hstack([ueq_dyn, uineq_x, uineq_u, uineq_du]) 575 | 576 | # assign all 577 | if self.SOFT_ON: 578 | self.P = sparse.block_diag([P_X, P_U, P_eps], format='csc') 579 | self.q = np.hstack([q_X, q_U, q_eps]) 580 | else: 581 | self.P = sparse.block_diag([P_X, P_U],format='csc') 582 | self.q = np.hstack([q_X, q_U]) 583 | 584 | self.A = A 585 | self.l = l 586 | self.u = u 587 | 588 | self.P_X = P_X 589 | # Debug assignments 590 | 591 | # self.P_x = P_X 592 | # self.P_U = P_U 593 | # self.P_eps = P_eps 594 | #self.Aineq_du = Aineq_du 595 | #self.leq_dyn = leq_dyn 596 | #self.lineq_du = lineq_du 597 | 598 | if __name__ == '__main__': 599 | import time 600 | import matplotlib.pyplot as plt 601 | # Constants # 602 | Ts = 0.2 # sampling time (s) 603 | M = 2 # mass (Kg) 604 | b = 0.3 # friction coefficient (N*s/m) 605 | 606 | Ad = sparse.csc_matrix([ 607 | [1.0, Ts], 608 | [0, 1.0 -b/M*Ts] 609 | ]) 610 | Bd = sparse.csc_matrix([ 611 | [0.0], 612 | [Ts/M]]) 613 | 614 | # Continous-time matrices (just for reference) 615 | Ac = np.array([ 616 | [0.0, 1.0], 617 | [0, -b/M]] 618 | ) 619 | Bc = np.array([ 620 | [0.0], 621 | [1/M] 622 | ]) 623 | 624 | # Reference input and states 625 | pref = 7.0 626 | vref = 0.0 627 | xref = np.array([pref, vref]) # reference state 628 | uref = np.array([0.0]) # reference input 629 | uminus1 = np.array([0.0]) # input at time step negative one - used to penalize the first delta u at time instant 0. Could be the same as uref. 630 | 631 | # Constraints 632 | xmin = np.array([-10, -10.0]) 633 | xmax = np.array([7.0, 10.0]) 634 | 635 | umin = np.array([-1.2]) 636 | umax = np.array([1.2]) 637 | 638 | Dumin = np.array([-2e-1]) 639 | Dumax = np.array([2e-1]) 640 | 641 | # Objective function 642 | Qx = sparse.diags([0.5, 0.1]) # Quadratic cost for states x0, x1, ..., x_N-1 643 | QxN = sparse.diags([0.5, 0.1]) # Quadratic cost for xN 644 | Qu = 2.0 * sparse.eye(1) # Quadratic cost for u0, u1, ...., u_N-1 645 | QDu = 10.0 * sparse.eye(1) # Quadratic cost for Du0, Du1, ...., Du_N-1 646 | 647 | # Initial state 648 | x0 = np.array([0.1, 0.2]) # initial state 649 | 650 | # Prediction horizon 651 | Np = 25 652 | Nc = 10 653 | 654 | Xref = np.kron(np.ones((Np + 1,1)), xref) 655 | K = MPCController(Ad,Bd,Np=Np,Nc=Nc,x0=x0,xref=Xref,uminus1=uminus1, 656 | Qx=Qx, QxN=QxN, Qu=Qu,QDu=QDu, 657 | xmin=xmin,xmax=xmax,umin=umin,umax=umax,Dumin=Dumin,Dumax=Dumax) 658 | K.setup() 659 | 660 | # Simulate in closed loop 661 | [nx, nu] = Bd.shape # number of states and number or inputs 662 | len_sim = 40 # simulation length (s) 663 | nsim = int(len_sim/Ts) # simulation length(timesteps) 664 | xsim = np.zeros((nsim,nx)) 665 | usim = np.zeros((nsim,nu)) 666 | tsim = np.arange(0,nsim)*Ts 667 | 668 | time_start = time.time() 669 | xstep = x0 670 | for i in range(nsim): 671 | uMPC, info = K.output(return_u_seq=True, return_x_seq=True, return_eps_seq=True, return_status=True) 672 | xstep = Ad.dot(xstep) + Bd.dot(uMPC) # system step 673 | K.update(xstep, xref=Xref) # update with measurement 674 | K.solve() 675 | xsim[i,:] = xstep 676 | usim[i,:] = uMPC 677 | 678 | time_sim = time.time() - time_start 679 | 680 | #K.__controller_function__(np.array([0,0]), np.array([0])) 681 | 682 | fig,axes = plt.subplots(3,1, figsize=(10,10)) 683 | axes[0].plot(tsim, xsim[:,0], "k", label='p') 684 | axes[0].plot(tsim, xref[0]*np.ones(np.shape(tsim)), "r--", label="pref") 685 | axes[0].set_title("Position (m)") 686 | 687 | axes[1].plot(tsim, xsim[:,1], label="v") 688 | axes[1].plot(tsim, xref[1]*np.ones(np.shape(tsim)), "r--", label="vref") 689 | axes[1].set_title("Velocity (m/s)") 690 | 691 | axes[2].plot(tsim, usim[:,0], label="u") 692 | axes[2].plot(tsim, uref*np.ones(np.shape(tsim)), "r--", label="uref") 693 | axes[2].set_title("Force (N)") 694 | 695 | for ax in axes: 696 | ax.grid(True) 697 | ax.legend() 698 | -------------------------------------------------------------------------------- /slides/biblio.bib: -------------------------------------------------------------------------------- 1 | @article{bombois2006least, 2 | title={Least costly identification experiment for control}, 3 | author={Bombois, X. and Scorletti, G. and Gevers, M. and Van den Hof, PMJ and Hildebrand, R.}, 4 | journal={Automatica}, 5 | volume={42}, 6 | number={10}, 7 | pages={1651--1662}, 8 | year={2006}, 9 | publisher={Elsevier} 10 | } -------------------------------------------------------------------------------- /slides/img/GLIS/GLIS_06.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/GLIS/GLIS_06.pdf -------------------------------------------------------------------------------- /slides/img/GLIS/GLIS_07.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/GLIS/GLIS_07.pdf -------------------------------------------------------------------------------- /slides/img/GLIS/GLIS_08.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/GLIS/GLIS_08.pdf -------------------------------------------------------------------------------- /slides/img/GLIS/acquisition.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/GLIS/acquisition.png -------------------------------------------------------------------------------- /slides/img/GLIS/surrogate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/GLIS/surrogate.png -------------------------------------------------------------------------------- /slides/img/MPCloop.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/MPCloop.pdf -------------------------------------------------------------------------------- /slides/img/MPCloop.svg: -------------------------------------------------------------------------------- 1 | 2 | 297 | -------------------------------------------------------------------------------- /slides/img/example/BEST_GLIS_PC.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/BEST_GLIS_PC.pdf -------------------------------------------------------------------------------- /slides/img/example/BEST_GLIS_PI.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/BEST_GLIS_PI.pdf -------------------------------------------------------------------------------- /slides/img/example/COMPUTATION_BO_PI.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/COMPUTATION_BO_PI.pdf -------------------------------------------------------------------------------- /slides/img/example/COMPUTATION_GLIS_PI.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/COMPUTATION_GLIS_PI.pdf -------------------------------------------------------------------------------- /slides/img/example/HIL_PC.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/HIL_PC.pdf -------------------------------------------------------------------------------- /slides/img/example/HIL_PI.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/HIL_PI.pdf -------------------------------------------------------------------------------- /slides/img/example/ITER_GLIS_PC_500.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/ITER_GLIS_PC_500.pdf -------------------------------------------------------------------------------- /slides/img/example/ITER_GLIS_PI_500.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/ITER_GLIS_PI_500.pdf -------------------------------------------------------------------------------- /slides/img/example/cart_pole.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/example/cart_pole.pdf -------------------------------------------------------------------------------- /slides/img/teaser.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/img/teaser.pdf -------------------------------------------------------------------------------- /slides/preamble.tex: -------------------------------------------------------------------------------- 1 | % does not look nice, try deleting the line with the fontenc. 2 | \usepackage[english]{babel} 3 | \usepackage{amsmath} 4 | \usepackage[latin1]{inputenc} 5 | \usepackage{units} 6 | \usepackage{colortbl} 7 | \usepackage{multimedia} 8 | \usepackage{bm} 9 | 10 | \mode 11 | { 12 | \usetheme{Boadilla} 13 | \useoutertheme{infolines} 14 | \setbeamercovered{transparent} 15 | } 16 | 17 | 18 | \title[Efficient Calibration of Embedded MPC]{\textsc{Efficient Calibration of Embedded MPC}} 19 | 20 | %\subtitle{Industrial-scale experimental results\\} % (optional) 21 | 22 | \author[]{Marco Forgione\inst{1}, Dario Piga\inst{1}, 23 | and Alberto Bemporad \inst{2}} 24 | 25 | \institute[IDSIA]{ 26 | \inst{1}IDSIA Dalle Molle Institute for Artificial Intelligence SUPSI-USI, Lugano, Switzerland \and 27 | \inst{2}IMT School for Advanced Studies Lucca, Lucca, Italy 28 | } 29 | 30 | 31 | \date[IFAC 2020]{IFAC 2020 World Congress\\ Berlin, Germany} 32 | 33 | 34 | \subject{Performance-oriented MPC tuning} 35 | 36 | 37 | %% MATH DEFINITIONS %% 38 | %\newcommand{\R}{\mathcal{R}} 39 | %\newcommand{\du}{\delta u} 40 | %\newcommand{\dy}{\delta y} 41 | %\newcommand{\DU}{\Delta U} 42 | %\newcommand{\DY}{\Delta Y} 43 | %\newcommand{\abs}[1]{\left | #1 \right |} 44 | \newcommand{\norm}[1]{\left \lVert #1 \right \rVert} 45 | %\newcommand{\relphantom}[1]{\mathrel{\phantom{#1}}} 46 | %\newenvironment{matrixc}{\begin{array}{c}\left[}{\end{array}\right]} 47 | \DeclareMathOperator*\argmin{arg \, min} 48 | %\DeclareMathOperator*\argmax{arg \, max} 49 | %\DeclareMathOperator*\fit{fit} 50 | %\DeclareMathOperator*\RMSE{RMSE} 51 | %\DeclareMathOperator*\diag{diag} 52 | %\DeclareMathOperator*\diet{diet} 53 | %\DeclareMathOperator*\Risk{Risk} 54 | %\DeclareMathOperator*\Num{Num} 55 | %\DeclareMathOperator*\Den{Den} 56 | %\DeclareMathOperator*\Rat{Rat} 57 | \DeclareMathOperator*\cov{cov} 58 | %\DeclareMathOperator*\Var{Var} 59 | %\DeclareMathOperator*\SSR{SSR} 60 | %\setcounter{MaxMatrixCols}{20} 61 | %\newcommand{\pdiff}[2]{\frac{\partial #1}{\partial #2}} 62 | %\definecolor{mypink1}{rgb}{0.858, 0.188, 0.478} 63 | %\definecolor{olive}{RGB}{85, 107, 47} 64 | \definecolor{orange}{RGB}{204, 85, 0} 65 | 66 | %\definecolor{mypink3}{cmyk}{0, 0.7808, 0.4429, 0.1412} 67 | %\definecolor{mygray}{gray}{0.6} 68 | %\definecolor{olivegreen}[RGB]{85, 107, 47} 69 | 70 | \newcommand{\K}{K} 71 | \newcommand{\M}{M} 72 | \newcommand{\Mo}{M_o} 73 | \newcommand{\So}{S_o} 74 | \newcommand{\Smod}{S} 75 | \newcommand{\parcolor}[1]{{\color{orange}#1}} 76 | \newcommand{\Ts}{T_s^{\rm MPC}} 77 | \newcommand{\cites}[1]{\begin{small}(#1)\end{small}} 78 | %\newcommand{\Ts}{T_s} 79 | -------------------------------------------------------------------------------- /slides/presentation_main.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/forgi86/efficient-calibration-embedded-MPC/ac42a7ebe9e394e35deda7e72d856c2033dff434/slides/presentation_main.pdf -------------------------------------------------------------------------------- /slides/presentation_main.tex: -------------------------------------------------------------------------------- 1 | \documentclass{beamer} 2 | \usefonttheme[onlymath]{serif} 3 | \input{preamble} 4 | \definecolor{darkgreen}{RGB}{20,150,50} 5 | \definecolor{acq}{RGB}{119, 172, 48} 6 | \definecolor{true}{RGB}{162, 20, 47} 7 | \definecolor{surr}{RGB}{0, 114, 189} 8 | 9 | \begin{document} 10 | 11 | \begin{frame} 12 | \titlepage 13 | \end{frame} 14 | 15 | \begin{frame}{Motivations} 16 | Calibration of \structure{model predictive controllers} is \alert{costly} and \alert{time-consuming}.\\ 17 | One has to choose model, cost function weights, prediction/control horizon, design a state estimator, etc. 18 | %Typically, models are obtained through Physical modeling or Identification 19 | \vskip 1em 20 | The design is even more involved for \structure{embedded applications}, where the hardware is also a limit. Need to choose carefully: 21 | \begin{itemize} 22 | \item Solution strategy (QP solver?) 23 | \item Low-level solver settings (e.g., tolerances) 24 | \item A \structure{feasible} MPC sampling time 25 | \end{itemize} 26 | %\vskip 1em 27 | %Requires \structure{knowledge} in control systems, numerical math, computer engineering + \structure{hands-on experience}. 28 | \vskip 1em 29 | In this paper, we introduce an approach to ease embedded MPC calibration based on \structure{global optimization} and repeated experiements. 30 | \end{frame} 31 | 32 | 33 | \begin{frame}{Control architecture}{Model Predictive Controller} 34 | \begin{figure} 35 | \centering 36 | \includegraphics[width=.3\textwidth]{img/MPCloop.pdf} 37 | \end{figure} 38 | 39 | Linear constrained MPC for a (non-linear) plant with model 40 | \begin{columns} 41 | \column{.3\textwidth} 42 | \begin{small} 43 | \begin{equation*} 44 | \begin{split} 45 | \dot x(t) &= f(x(t), u(t);\; \parcolor{\beta})\\ 46 | y(t) &=g(x(t); \; \parcolor{\beta}) %+\parcolor{D}u_t, 47 | \end{split} 48 | \end{equation*} 49 | \end{small} 50 | \column{.2\textwidth} 51 | $$\Rightarrow$$ 52 | \column{.5\textwidth} 53 | \begin{small} 54 | \begin{equation*} 55 | \begin{split} 56 | x_{t+1}&={A}(\parcolor{\Ts}, \parcolor{\beta})x_t+{B}(\parcolor{\Ts}, \parcolor{\beta})u_t\\ 57 | y_t &=C(\parcolor{\beta})x_t %+\parcolor{D}u_t, 58 | \end{split} 59 | \end{equation*} 60 | \end{small} 61 | \end{columns} 62 | input $u$ chosen on-line as the solution (applied in 63 | {receding horizon}) of: 64 | \vskip -1.5em 65 | \begin{small} 66 | \begin{align*} 67 | &\min_{\left\{u_{t+k|t}\right\}_{k=1}^{\parcolor{N_c}}} \;\;\sum_{k=1}^{\parcolor{N_p}} \norm{\hat y_{t+k|t}-\overline{y}_{t+k}}^2_{\parcolor{Q_y}} 68 | + \norm{u_{t+k|k}\!-\!u_{t+k-1|t}}^2_{\parcolor{Q_{\Delta u}}} \\ 69 | & \text{s.t. model equations + constraints on $y$, $u$, $\Delta u$} 70 | \end{align*} 71 | \end{small} 72 | %\vskip -1.5em 73 | %MPC control law computed on-line using a QP solver.% (OSQP, ADMM-based). 74 | \end{frame} 75 | 76 | \begin{frame}{Control architecture}{Design variables} 77 | The MPC design variables to be tuned are 78 | \begin{columns}[t] 79 | \column{.6\textwidth} 80 | \begin{itemize} 81 | \item Model parameters: $\parcolor{\beta}$ 82 | \item MPC sampling time: $\parcolor{\Ts}$ 83 | \item MPC cost function: $\parcolor{Q_y}, \parcolor{Q_{\Delta u}}, \parcolor{N_c}, \parcolor{N_p}$ 84 | \end{itemize} 85 | \column{.4\textwidth} 86 | \vskip -1em 87 | \begin{figure} 88 | \centering 89 | \includegraphics[width=.75\textwidth]{img/MPCloop.pdf} 90 | \end{figure} 91 | \end{columns} 92 | \begin{itemize} 93 | \item MPC implementation: QP solver tolerances $\parcolor{\epsilon_{\rm abs}}, \parcolor{\epsilon_{\rm rel}}$ 94 | \item State estimator: Kalman covariances $\parcolor{W_v}, \parcolor{W_w}$ 95 | \end{itemize} 96 | %\vskip 1em 97 | %in order to: 98 | %\begin{enumerate} 99 | %\item maximize closed-loop performance %objective $J$ 100 | %\item ensure that the implemented MPC runs in real-time 101 | %\end{enumerate} 102 | %\vskip 1em 103 | %Note: closed-loop performance is not the MPC cost function! 104 | \end{frame} 105 | 106 | 107 | 108 | \begin{frame}{MPC calibration procedure}{Overview} 109 | In our optimization-based MPC calibration, we define 110 | \begin{itemize} 111 | \item Tunable \structure{design parameters} collected in a \parcolor{design vector $\theta$} $\in \Theta$. 112 | \item A closed-loop \structure{performance index $J$} defined in terms of measured input/outputs during the {calibration experiment}: $J = J(y_{1:T},u_{1:T};\;\parcolor{\theta})$ 113 | \item An \structure{procedure} to perform \structure{calibration experiments} (or SIL simulations) representative of the intended closed-loop operation 114 | \end{itemize} 115 | \pause 116 | MPC calibration is seen as a \structure{global optimization} problem: 117 | \begin{align*} 118 | &\theta^{\rm opt} = \argmin_{\theta \in \Theta} J(y_{1:T},u_{1:T};\;{\theta})\\ 119 | & \text{s.t. } T_{\rm calc}^{\rm MPC}({\theta}) \leq {\Ts} 120 | \end{align*} 121 | each (noisy) \structure{function evaluation} correspond to a \structure{calibration experiment}.\\ 122 | Both $J$ and $T_{\rm calc}^{\rm MPC}$ (worst-case MPC computation time) are observed! 123 | %\vskip 1em 124 | %\pause 125 | %Problem is tackled using \structure{efficient} global optimization algorithms. 126 | \end{frame} 127 | 128 | \begin{frame}{Global Optimization Algorithm}{Overview} 129 | Several \structure{derivative-free} global optimization algorithms exist: 130 | \begin{itemize} 131 | \item Response surface methods 132 | \item Bayesian Optimization% (BO) 133 | \item Genetic algorithms 134 | \item Particle Swarm Optimization 135 | \item \dots%Lipshitzian-based partitioning techniques 136 | \end{itemize} 137 | % \end{itemize} 138 | \vskip .5em 139 | \pause 140 | Here, we adopt \structure{GLIS}: a method recently introduced in \cites{Bemporad, 2019}. 141 | \begin{itemize} 142 | \item Radial basis function \structure{surrogate} + inverse distance weighting (IDW) for \structure{exploration} 143 | \item Performs better than BO on standard benchmarks, at a lower computational cost 144 | %\item Matlab- and Python-based implementation of GLIS available 145 | \end{itemize} 146 | \end{frame} 147 | 148 | \begin{frame}{GLIS Algorithm}{Surrogate function} 149 | 150 | \begin{columns}[t] 151 | \column{.55\textwidth} 152 | Goal: solve \structure{global optimization} problem: 153 | % \begin{align*} 154 | $$ \min_{\theta \in \Theta} J(\theta)$$ 155 | %\\ 156 | %& \text{s.t. } \theta \in \Theta 157 | %\end{align*} 158 | %\vskip -.5em 159 | \begin{enumerate} 160 | \item Collect ${n_{\rm in}}$ initial observations $\{(\theta_1, J_1), (\theta_2, J_2), \dots, (\theta_{n_{\rm in}}, J_{n_{\rm in}})\}$ 161 | \item Build a \structure{surrogate function} 162 | \vskip -0.5em 163 | $$\hat J(\theta) = \sum_{i=1}^{n} \alpha_i \phi(\norm{\theta - \theta_i}_2 )$$ 164 | \end{enumerate} 165 | \column{.45\textwidth} 166 | %\vskip -1.5em 167 | \centering 168 | True {\color{true} $J(\theta)$}, surrogate {\color{surr} $\hat{J}(\theta)$} 169 | \begin{figure} 170 | \includegraphics[width=.7\textwidth]{img/GLIS/surrogate.png} 171 | \end{figure} 172 | $ \phi = $ \text{radial basis function}\\ 173 | \vskip .5em 174 | Example: $\phi(d) = \frac{1}{1 + (\epsilon d)^2}$ 175 | \end{columns} 176 | \vskip 1em 177 | %Vector $\alpha$ solves $\hat J(\theta_i) = J_i,\; i=1,\dots,n\;\;$ (= linear system) 178 | \pause 179 | \vskip 1em 180 | Minimizing $\hat J(\theta)$ to find $\theta_{n+1}$ may easily miss the global optimum! 181 | \end{frame} 182 | 183 | \begin{frame}{GLIS Algorithm}{Exploration vs. Exploitation} 184 | \begin{columns}[t] 185 | \column{.57\textwidth} 186 | \begin{enumerate} 187 | \setcounter{enumi}{2} 188 | \item Construct the IDW \structure{exploration function} 189 | \begin{equation*} 190 | z(\theta) = \frac{2}{\pi}\Delta F \tan^{-1}\left(\frac{1}{\sum_{i=1}^n w_i(\theta)} \right) 191 | \end{equation*} 192 | where $w_i(\theta) = \frac{e^{-\norm{\theta - \theta_i}^2}}{\norm{\theta - \theta_i}^2}$%{-\norm{x - x_i}}^2%}{1}$ 193 | \item Optimize the \structure{acquisition function}: 194 | $$\theta_{n+1} = \arg \min_{\theta \in \Theta} \hat J(\theta) - \alert{\delta} z(\theta)$$ 195 | \end{enumerate} 196 | \column{.45\textwidth} 197 | \centering 198 | \begin{figure} 199 | Exploration function {\color{acq}$z(\theta)$} 200 | \includegraphics[width=.7\textwidth]{img/GLIS/acquisition.png} 201 | \end{figure} 202 | $\alert{\delta}$: exploitation vs.\\ exploration trade-off 203 | \end{columns} 204 | \vskip 1em 205 | to get the \structure{query point} $\theta_{n+1}$. Execute experiment with $\theta_{n+1}$, measure $J_{n+1}$. 206 | \pause 207 | \vskip 1em 208 | Iterate the procedure for $n+2, n+3 \dots$. 209 | \end{frame} 210 | 211 | \begin{frame}{MPC Calibration Example}%{Cart-pole system} 212 | \begin{columns} 213 | \column{.48\textwidth} 214 | \centering 215 | Cart-pole system 216 | \begin{figure} 217 | \includegraphics[width=.45\textwidth]{img/example/cart_pole.pdf} 218 | \end{figure} 219 | \begin{itemize} 220 | \item Input: force $F$ with \structure{fast disturbance} ($5~\text{rad/sec}$) 221 | \item Output: noisy position $p$%$ with white measurement noise 222 | \item Objective: follow trajectory for $p$, keep $\phi$ small. 223 | \end{itemize} 224 | \column{.54\textwidth} 225 | Optimization-based calibration of 226 | \begin{enumerate} 227 | \item MPC sample time $T_{s}^{\rm MPC}$ 228 | \item MPC weights $Q_y$ and $Q_{\Delta u}$ 229 | \item Prediction and control horizon 230 | \item Kalman filter covariance matrices 231 | \item QP solver's abs. and rel. tolerances 232 | \end{enumerate} 233 | 234 | \vskip 1em 235 | $n_\theta=14$ design parameters optimized using $n=500$ iterations of GLIS 236 | 237 | \begin{small} 238 | \begin{equation*} 239 | J = \int_0^{T_{\rm exp}} |\overline{p}(t) - p(t)| + 30 |\phi(t)|\;dt 240 | \end{equation*} 241 | \end{small} 242 | % \footnotesize 243 | % \url{https://github.com/forgi86/efficient-calibration-embedded-MPC.git} 244 | \end{columns} 245 | %\pause 246 | %\vskip 1em 247 | \pause 248 | Python codes on-line, $100\%$ \structure{open source} (including all dependencies).\\ 249 | \small 250 | \url{https://github.com/forgi86/efficient-calibration-embedded-MPC} 251 | \end{frame} 252 | 253 | \begin{frame}{MPC Calibration Example}%{Cart-pole system} 254 | \begin{columns} 255 | \column{.3\textwidth} 256 | \begin{figure} 257 | \centering 258 | %GLIS iterations on the PC 259 | \includegraphics[width=.99\textwidth]{img/example/HIL_PC.pdf} 260 | \end{figure} 261 | \column{.2\textwidth} 262 | \centering 263 | \huge vs. 264 | \column{.3\textwidth} 265 | \begin{figure} 266 | %GLIS iterations on the Raspberry PI 267 | \centering 268 | \includegraphics[width=.99\textwidth]{img/example/HIL_PI.pdf} 269 | \end{figure} 270 | \end{columns} 271 | \vskip 1em 272 | \begin{itemize} 273 | \item An Intel i5 PC (left) vs. an ARM-based Raspberry PI 3 (right) 274 | %\item On the same control problem, running the same Python code 275 | \item PI is about 10 time slower than the PC for MPC computations 276 | %\item Cart-pole simulated in the same Python code for simplicity \dots 277 | %\item Could be used in a true HIL setup or a real system 278 | \end{itemize} 279 | \end{frame} 280 | 281 | 282 | \begin{frame}{MPC Calibration Example}%{Cart-pole system} 283 | \begin{columns} 284 | \column{.5\textwidth} 285 | \begin{figure} 286 | \centering 287 | Optimized MPC on the PC 288 | \includegraphics[width=.99\textwidth]{img/example/BEST_GLIS_PC.pdf} 289 | $T_s^{\rm MPC} = 6$~ms 290 | \end{figure} 291 | \column{.5\textwidth} 292 | \begin{figure} 293 | Optimized MPC on the Raspberry PI 294 | \centering 295 | \includegraphics[width=.99\textwidth]{img/example/BEST_GLIS_PI.pdf} 296 | $T_s^{\rm MPC} = 22$~ms 297 | \end{figure} 298 | \end{columns} 299 | \vskip 1em 300 | \begin{itemize} 301 | \item Position and angle control tighter on the PC 302 | \item Faster loop update on the PC $\Rightarrow$ more effective disturbance rejection 303 | \item Calibration squeezes max performance out of the hardware! 304 | \end{itemize} 305 | \end{frame} 306 | 307 | \begin{frame}{MPC Calibration Example}%{Cart-pole system} 308 | \begin{columns} 309 | \column{.5\textwidth} 310 | \begin{figure} 311 | \centering 312 | GLIS iterations on the PC 313 | \includegraphics[width=.99\textwidth]{img/example/ITER_GLIS_PC_500.pdf} 314 | \end{figure} 315 | \column{.5\textwidth} 316 | \begin{figure} 317 | GLIS iterations on the Raspberry PI 318 | \centering 319 | \includegraphics[width=.99\textwidth]{img/example/ITER_GLIS_PI_500.pdf} 320 | \end{figure} 321 | \end{columns} 322 | \vskip 1em 323 | \begin{itemize} 324 | \item High cost for interrupted tests/unfeasible design 325 | \item For increasing $n$, more points have ``low'' cost 326 | \item Experiments with high cost still appear for large $n$ (exploration) 327 | %\item Optimum on PC is slightly better than on the PI. Some configurations are only feasible on PC! 328 | \end{itemize} 329 | \end{frame} 330 | 331 | \begin{frame}{Simulation Example}{Cart-pole system} 332 | Similar calibration results are obtained using Bayesian Optimization (BO).\\ 333 | However, BO is a much heavier algorithm! 334 | \vskip -.5em 335 | \begin{columns} 336 | \column{.5\textwidth} 337 | \begin{figure} 338 | \centering 339 | GLIS iterations on the PI 340 | \includegraphics[width=.99\textwidth]{img/example/COMPUTATION_GLIS_PI.pdf} 341 | \end{figure} 342 | \column{.5\textwidth} 343 | \begin{figure} 344 | BO iterations on the PI 345 | \centering 346 | \includegraphics[width=1\textwidth]{img/example/COMPUTATION_BO_PI.pdf} 347 | \end{figure} 348 | \end{columns} 349 | \begin{itemize} 350 | \item With GLIS, the overall computation time is dominated by function evaluations (= running MPC and simulating the system). 351 | %\item A lightweight algorithm like GLIS could be used for self-tuning on an embedded system! 352 | \end{itemize} 353 | \end{frame} 354 | 355 | \begin{frame}{Conclusions} 356 | An approach for embedded MPC calibration approach based on global optimization and repeated experiments 357 | \begin{itemize} 358 | \item Higher- and lower-level tuning knobs jointly optimized for performance, while keeping the design feasible for real-time implementation. 359 | \item Tested in simulation on two hardware platforms 360 | \end{itemize} 361 | \vskip 1em 362 | \pause 363 | Current/future works 364 | \begin{itemize} 365 | \item Application to {robotic systems} 366 | \item Preference-based MPC calibration 367 | \end{itemize} 368 | \end{frame} 369 | 370 | \begin{frame}{}{} 371 | \begin{center} 372 | \huge{\structure{Thank you.\\ Questions?}}\\ 373 | \vskip 1em 374 | \begin{small} 375 | \texttt{marco.forgione@idsia.ch} 376 | \end{small} 377 | \end{center} 378 | \end{frame} 379 | 380 | %\appendix 381 | %\newcounter{finalframe} 382 | %\setcounter{finalframe}{\value{framenumber}} 383 | %\setcounter{framenumber}{\value{finalframe}} 384 | 385 | \end{document} 386 | --------------------------------------------------------------------------------