├── .gitignore ├── Control_Calc.py ├── Default_Values.py ├── Estimator.py ├── Ex_ENMPC.py ├── Ex_LMPC_CSTR.py ├── Ex_LMPC_WB.py ├── Ex_LMPC_nlplant.py ├── Ex_LMPCxp_nlplant.py ├── Ex_NMPC.py ├── Ex_NMPC_dis.py ├── LICENSE ├── MPC_code.py ├── SS_JAC_ID.py ├── Target_Calc.py ├── User_Guide.pdf └── Utilities.py /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | pdt_otto_mol.py 3 | Empc_otto_2mod_off_adap_5s.py 4 | Empc_otto2me123offlinadaposci.py 5 | Empc_otto2me123offlinadaposci.py 6 | Empc_otto2me123offlinadaposci.py 7 | Empc_otto2me123offlinadaposci.py 8 | Empc_otto2me123offlinadaposci.py 9 | Empc_otto2me123offlinadaposci.py 10 | Empc_otto2me123offlinadaposci.py 11 | Empc_otto2me123offlinadaposci.py 12 | -------------------------------------------------------------------------------- /Control_Calc.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on December 3, 2015 4 | 5 | @author: Marco, Mirco, Gabriele 6 | 7 | MPC control calculation 8 | """ 9 | 10 | from builtins import range 11 | from casadi import * 12 | from casadi.tools import * 13 | from matplotlib import pylab as plt 14 | import math 15 | import scipy.linalg as scla 16 | import numpy as np 17 | from Utilities import* 18 | 19 | 20 | def opt_dyn(xSX , uSX, ySX, dSX, tSX, pxSX, pySX, n, m, p, nd, npx, npy, ng_v, nh_v, Fx_model, Fy_model, F_obj, Vfin, N, QForm, DUForm, DUFormEcon, ContForm, TermCons, slacks, slacksG, slacksH, nw, sol_opts, G_ineq, H_eq, umin = None, umax = None, W = None, Z = None, ymin = None, ymax = None, xmin = None, xmax = None, Dumin = None, Dumax = None, h = None, fx = None, xstat = None, ustat = None, Ws = None): 21 | """ 22 | SUMMARY: 23 | It builds the dynamic optimization problem 24 | """ 25 | # Extract dimensions 26 | nxu = n+m 27 | nxuy = nxu + p 28 | ns = nw - (nxu*N+n) 29 | 30 | # Define symbolic optimization variables 31 | w = MX.sym("w",nw) # w = [x[0],u[0], ... ,u[N-1],x[N]] , w = [x[0],u[0], ... ,u[N-1],x[N],Sl] 32 | 33 | # Get states 34 | X = [w[nxu*k : nxu*k+n] for k in range(N+1)] 35 | 36 | # Get controls 37 | U = [w[nxu*k+n : nxu*k + nxu] for k in range(N)] 38 | 39 | if slacks == True: 40 | Sl = w[nw-ns:nw] # 2*ny (+ng) (+nh) 41 | 42 | # Define parameters 43 | par = MX.sym("par", 2*nxu+nd+1+p*m+npx*N+npy*N) 44 | x0 = par[0:n] 45 | xs = par[n:2*n] 46 | us = par[2*n:n+nxu] 47 | d = par[n+nxu:n+nxu+nd] 48 | um1 = par[n+nxu+nd:2*nxu+nd] 49 | t = par[2*nxu+nd:2*nxu+nd+1] 50 | lambdayT_r = par[2*nxu+nd+1:2*nxu+nd+1+p*m] 51 | par_xmk_r = par[2*nxu+nd+1+p*m:2*nxu+nd+1+p*m+npx*N] 52 | par_ymk_r = par[2*nxu+nd+1+p*m+npx*N:2*nxu+nd+1+p*m+npx*N+npy*N] 53 | 54 | lambdayT = lambdayT_r.reshape((p,m)) #shaping lambda_r vector in order to reconstruct the matrix 55 | 56 | par_xmk = reshape(par_xmk_r,npx,N) #shaping par_xmk_r vector in order to reconstruct the matrix 57 | par_ymk = reshape(par_ymk_r,npy,N) #shaping par_xmk_r vector in order to reconstruct the matrix 58 | 59 | # Defining bound constraint 60 | if ymin is None and ymax is None: 61 | yFree = True 62 | else: 63 | yFree = False 64 | if ymin is None: 65 | if slacks == False: 66 | ymin = -DM.inf(p) 67 | else: 68 | ymin = -1e12*DM.ones(p) 69 | if ymax is None: 70 | if slacks == False: 71 | ymax = DM.inf(p) 72 | else: 73 | ymax = 1e12*DM.ones(p) 74 | if xmin is None: 75 | xmin = -DM.inf(n) 76 | if xmax is None: 77 | xmax = DM.inf(n) 78 | if umin is None: 79 | umin = -DM.inf(m) 80 | if umax is None: 81 | umax = DM.inf(m) 82 | if Dumin is None and Dumax is None: 83 | DuFree = True 84 | else: 85 | DuFree = False 86 | if Dumin is None: 87 | Dumin = -DM.inf(m) 88 | if Dumax is None: 89 | Dumax = DM.inf(m) 90 | 91 | if h is None: 92 | h = .1 #Defining integrating step if not provided from the user 93 | 94 | if G_ineq != None: 95 | g_ineq = G_ineq(xSX,uSX,ySX,dSX,tSX,pxSX,pySX) 96 | G_ineqSX = Function('G_ineqSX', [xSX,uSX,ySX,dSX,tSX,pxSX,pySX], [g_ineq]) 97 | 98 | if H_eq != None: 99 | h_eq = H_eq(xSX,uSX,ySX,dSX,tSX,pxSX,pySX) 100 | H_eqSX = Function('H_eqSX', [xSX,uSX,ySX,dSX,tSX,pxSX,pySX], [h_eq]) 101 | 102 | if ContForm is True: 103 | xdot = fx(xSX,uSX,dSX,tSX,pxSX) + pxSX 104 | y = Fy_model( xSX,uSX, dSX, tSX, pySX) 105 | ystat = Fy_model( xstat, ustat, dSX, tSX, pySX) 106 | F_obj1 = F_obj(xSX, uSX, y, xstat, ustat, ystat) 107 | 108 | # Create an integrator 109 | dae = {'x':xSX, 'p':vertcat(uSX,dSX,tSX,xstat,ustat,pxSX,pySX), 'ode':xdot, 'quad':F_obj1} 110 | opts = {'tf':h, 't0':0.0} # final time 111 | F = integrator('F', 'idas', dae, opts) 112 | 113 | # Initializing constraints vectors and obj fun 114 | g = [] 115 | g1 = [] # Costraint vector for y bounds 116 | g2 = [] # Costraint vector for Du bounds 117 | g4 = [] # User defined inequality constraints 118 | g5 = [] # User defined equality constraints 119 | f_obj = 0.0; 120 | sl_ub = [] 121 | sl_lb = [] 122 | 123 | 124 | ys = Fy_model( xs, us, d, t, par_ymk[:,0]) #Calculating steady-state output if necessary 125 | 126 | g.append(x0 - X[0]) #adding initial contraint to the current xhat_k|k 127 | 128 | for k in range(N): 129 | # Correction for dynamic KKT matching 130 | Y_k = Fy_model( X[k], U[k], d, t, par_ymk[:,k]) + mtimes(lambdayT,(U[k] - us)) 131 | 132 | if G_ineq != None: 133 | if slacks == True and slacksG == True: 134 | G_k = G_ineqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) - Sl[2*p:2*p+ng_v] 135 | else: 136 | G_k = G_ineqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) 137 | else: 138 | G_k = [] 139 | if H_eq != None: 140 | if slacks == True and slacksH == True: 141 | H_k = H_eqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) - Sl[2*p+ng_v:2*p+ng_v+nh_v] 142 | else: 143 | H_k = H_eqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) 144 | else: 145 | H_k = [] 146 | 147 | g4.append(G_k) 148 | g5.append(H_k) 149 | 150 | if yFree is False: 151 | g1.append(Y_k) #bound constraint on Y_k 152 | 153 | if ContForm is True: 154 | Fk = F(x0=X[k], p=vertcat(U[k],d,t, xs, us, par_xmk[:,k], par_ymk[:,k])) 155 | g.append(X[k+1] - Fk['xf']) 156 | 157 | # Add contribution to the objective 158 | f_obj += Fk['qf'] 159 | 160 | else: 161 | X_next = Fx_model( X[k], U[k], h, d, t, par_xmk[:,k]) 162 | 163 | if k == 0: 164 | DU_k = U[k] - um1 165 | else: 166 | DU_k = U[k] - U[k-1] 167 | 168 | if DuFree is False: 169 | g2.append(DU_k) #bound constraint on DU_k 170 | 171 | g.append(X_next - X[k+1]) 172 | # Defining variable entering the objective function 173 | dx = X[k] 174 | du = U[k] 175 | dy = Y_k 176 | if QForm is True: #Checking if the OF is quadratic 177 | dx = dx - xs 178 | du = du - us 179 | dy = dy - ys 180 | if DUForm is True: #Checking if the OF requires DU instead of u 181 | du = DU_k 182 | # if DUFormEcon is True: #Checking if the OF requires DU instead of u 183 | us_obj = DU_k if DUFormEcon is True else us 184 | 185 | f_obj_new = F_obj( dx, du, dy, xs, us_obj, ys) 186 | if slacks == True: 187 | f_obj_new = F_obj( dx, du, dy, xs, us_obj, ys) + mtimes(Sl.T,mtimes(Ws,Sl)) 188 | f_obj += f_obj_new 189 | 190 | if slacks == True: 191 | sl_ub.append(Sl[0:p]) 192 | sl_lb.append(Sl[p:2*p]) 193 | 194 | dx = X[N] 195 | if QForm is True: #Checking if the OF is quadratic 196 | dx = dx - xs 197 | if TermCons is True: #Adding the terminal constraint 198 | g.append(dx) 199 | 200 | g = vertcat(*g) 201 | g1 = vertcat(*g1) #bound constraint on Y_k 202 | g2 = vertcat(*g2) #bound constraint on Du_k 203 | g4 = vertcat(*g4) 204 | g5 = vertcat(*g5) 205 | if slacks == True: 206 | sl_ub = vertcat(*sl_ub) 207 | sl_lb = vertcat(*sl_lb) 208 | 209 | vfin = Vfin(dx,xs) 210 | f_obj += vfin #adding the final weight 211 | 212 | #Defining bound constraint 213 | w_lb = -DM.inf(nw) 214 | w_ub = DM.inf(nw) 215 | w_lb[0:n] = xmin 216 | w_ub[0:n] = xmax 217 | w_lb[nw-ns:nw] = DM.zeros(ns) # sl > 0 218 | 219 | ng = g.size1() 220 | ng1 = g1.size1() 221 | ng2 = g2.size1() 222 | ng4 = g4.size1() 223 | ng5 = g5.size1() 224 | g_lb = DM.zeros(ng+ng1+ng2+ng4+ng5,1) 225 | g_ub = DM.zeros(ng+ng1+ng2+ng4+ng5,1) 226 | 227 | if ng1 != 0: 228 | if slacks == False: 229 | g_lb[ng:ng+ng1] = mtimes(ymin,DM.ones(N).T).reshape((ng1,1)) 230 | g_ub[ng:ng+ng1] = mtimes(ymax,DM.ones(N).T).reshape((ng1,1)) 231 | else: 232 | g_lb = DM.zeros(ng+2*ng1+ng2+ng4+ng5,1) 233 | g_ub = DM.zeros(ng+2*ng1+ng2+ng4+ng5,1) 234 | g1_old = g1 235 | g1 = MX.zeros(2*ng1) 236 | g1[0:ng1] = mtimes(ymin,DM.ones(N).T).reshape((ng1,1)) - g1_old - sl_lb 237 | g1[ng1:2*ng1] = -mtimes(ymax,DM.ones(N).T).reshape((ng1,1)) + g1_old - sl_ub 238 | g_lb[ng:ng+2*ng1] = -DM.inf(2*ng1) 239 | ng1 = g1.size1() 240 | 241 | if ng2 != 0: 242 | g_lb[ng+ng1:ng+ng1+ng2] = mtimes(Dumin,DM.ones(N).T).reshape((ng2,1)) 243 | g_ub[ng+ng1:ng+ng1+ng2] = mtimes(Dumax,DM.ones(N).T).reshape((ng2,1)) 244 | 245 | if ng4 != 0: 246 | g_lb[ng+ng1+ng2:ng+ng1+ng2+ng4] = -DM.inf(ng4) 247 | 248 | for k in range(1,N+1,1): 249 | w_lb[k*nxu:k*nxu+n] = xmin 250 | w_ub[k*nxu:k*nxu+n] = xmax 251 | w_lb[k*nxu-m:k*nxu] = umin 252 | w_ub[k*nxu-m:k*nxu] = umax 253 | 254 | g = vertcat(g, g1, g2, g4, g5) 255 | 256 | nlp = {'x':w, 'p':par, 'f':f_obj, 'g':g} 257 | 258 | solver = nlpsol('solver', 'ipopt', nlp, sol_opts) 259 | 260 | return [solver, w_lb, w_ub, g_lb, g_ub] 261 | 262 | 263 | 264 | def opt_dyn_CM(xSX , uSX, ySX, dSX, tSX, pxSX, pySX, n, m, p, nd, npx, npy, ng_v, nh_v, Fx_model, Fy_model, F_obj, Vfin, N, QForm, DUForm, DUFormEcon, ContForm, TermCons, slacks, slacksG, slacksH, nw, sol_opts, G_ineq , H_eq, umin = None, umax = None, W = None, Z = None, ymin = None, ymax = None, xmin = None, xmax = None, Dumin = None, Dumax = None, h = None, fx = None, xstat = None, ustat = None, Mx = None, Ws = None): 265 | 266 | """ 267 | SUMMARY: 268 | It builds the dynamic optimization problem 269 | """ 270 | # Extract dimensions 271 | nxu = n+m 272 | nxuk = 3*n+m 273 | nxuy = nxu + p 274 | ns = nw - (nxu*N+n) 275 | 276 | nw = nw + 2*n*N #number of optimization variables for collocation methods 277 | 278 | # Define symbolic optimization variables 279 | w = MX.sym("w",nw) # w = [x[0],s1[0],s2[0],u[0], ... ,u[N-1],x[N],d,xs,us] 280 | 281 | 282 | # Get states 283 | X = [w[nxuk*k : nxuk*k+n] for k in range(N+1)] 284 | # Get internal states 285 | S1 = [w[nxuk*k+n : nxuk*k+2*n] for k in range(N)] 286 | S2 = [w[nxuk*k+2*n : nxuk*k+3*n] for k in range(N)] 287 | # Get controls 288 | U = [w[nxuk*k+3*n : nxuk*k +3*n+m] for k in range(N)] 289 | 290 | if slacks == True: 291 | Sl = w[nw-ns:nw] # 2*ny (+ng) (+nh) 292 | 293 | # Define parameters 294 | par = MX.sym("par", 2*nxu+nd+1+p*m+npx*N+npy*N) 295 | x0 = par[0:n] 296 | xs = par[n:2*n] 297 | us = par[2*n:n+nxu] 298 | d = par[n+nxu:n+nxu+nd] 299 | um1 = par[n+nxu+nd:2*nxu+nd] 300 | t = par[2*nxu+nd:2*nxu+nd+1] 301 | lambdayT_r = par[2*nxu+nd+1:2*nxu+nd+1+p*m] 302 | par_xmk_r = par[2*nxu+nd+1+p*m:2*nxu+nd+1+p*m+npx*N] 303 | par_ymk_r = par[2*nxu+nd+1+p*m+npx*N:2*nxu+nd+1+p*m+npx*N+npy*N] 304 | 305 | lambdayT = lambdayT_r.reshape((p,m)) #shaping lambda_r vector in order to reconstruct the matrix 306 | 307 | par_xmk = reshape(par_xmk_r,npx,N) #shaping par_xmk_r vector in order to reconstruct the matrix 308 | par_ymk = reshape(par_ymk_r,npy,N) #shaping par_xmk_r vector in order to reconstruct the matrix 309 | 310 | # Defining bound constraint 311 | if ymin is None and ymax is None: 312 | yFree = True 313 | else: 314 | yFree = False 315 | if ymin is None: 316 | if slacks == False: 317 | ymin = -DM.inf(p) 318 | else: 319 | ymin = -1e12*DM.ones(p) 320 | if ymax is None: 321 | if slacks == False: 322 | ymax = DM.inf(p) 323 | else: 324 | ymax = 1e12*DM.ones(p) 325 | if xmin is None: 326 | xmin = -DM.inf(n) 327 | if xmax is None: 328 | xmax = DM.inf(n) 329 | if umin is None: 330 | umin = -DM.inf(m) 331 | if umax is None: 332 | umax = DM.inf(m) 333 | if Dumin is None and Dumax is None: 334 | DuFree = True 335 | else: 336 | DuFree = False 337 | if Dumin is None: 338 | Dumin = -DM.inf(m) 339 | if Dumax is None: 340 | Dumax = DM.inf(m) 341 | 342 | if h is None: 343 | h = .1 #Defining integrating step if not provided from the user 344 | 345 | hSX = SX.sym("h_SX", 1) 346 | 347 | if G_ineq != None: 348 | g_ineq = G_ineq(xSX,uSX,ySX,dSX,tSX,pxSX,pySX) 349 | G_ineqSX = Function('G_ineqSX', [xSX,uSX,ySX,dSX,tSX,pxSX,pySX], [g_ineq]) 350 | 351 | if H_eq != None: 352 | h_eq = H_eq(xSX,uSX,ySX,dSX,tSX,pxSX,pySX) 353 | H_eqSX = Function('H_eqSX', [xSX,uSX,ySX,dSX,tSX,pxSX,pySX], [h_eq]) 354 | 355 | 356 | fx_SX = fx(xSX,uSX,dSX,tSX,pxSX) 357 | Fx_SX = Function('Fx_SX', [xSX,uSX,hSX,dSX,tSX,pxSX], [fx_SX]) 358 | 359 | if ContForm is True: 360 | xdot = fx(xSX,uSX,dSX,tSX) + pxSX 361 | y = Fy_model( xSX, uSX, dSX, tSX, pySX) 362 | ystat = Fy_model( xstat, ustat, dSX, tSX, pySX) 363 | F_obj1 = F_obj(xSX, uSX, y, xstat, ustat, ystat) 364 | 365 | # Create an integrator 366 | dae = {'x':xSX, 'p':vertcat(uSX,dSX,tSX,xstat,ustat,pxSX,pySX), 'ode':xdot, 'quad':F_obj1} 367 | opts = {'tf':h, 't0':0.0} # final time 368 | F = integrator('F', 'idas', dae, opts) 369 | 370 | ### INTEGRATION WITH COLLOCATION #### 371 | #coefficient of Butcher tableau with Gauss-Legendre method 372 | c1 = 1/2-(3**0.5)/6 ; c2 = 1/2+(3**0.5)/6 373 | a11 = 1/4 ; a12 = 1/4-(3**0.5)/6 374 | a21 = 1/4+(3**0.5)/6; a22 = 1/4 375 | b1 = 1/2 ; b2 = 1/2 376 | b = [b1,b2] 377 | 378 | A = [[a11,a12],[a21,a22]] 379 | D = np.linalg.inv(A) 380 | D11 = D[0,0] ; D12 = D[0,1] 381 | D21 = D[1,0] ; D22 = D[1,1] 382 | b_t = np.dot(D.T,b) 383 | b1_t = b_t[0] ; b2_t = b_t[1] 384 | 385 | # Initializing constraints vectors and obj fun 386 | g = [] 387 | g1 = [] # Costraint vector for y bounds 388 | g2 = [] # Costraint vector for Du bounds 389 | g3 = [] # Constraint vector for S internal states 390 | g4 = [] # User defined inequality constraints 391 | g5 = [] # User defined equality constraints 392 | f_obj = 0.0; 393 | sl_ub = [] 394 | sl_lb = [] 395 | 396 | 397 | 398 | ys = Fy_model( xs, us, d, t, par_ymk[:,0]) #Calculating steady-state output if necessary 399 | 400 | g.append(x0 - X[0]) #adding initial contraint to the current xhat_k|k 401 | 402 | for k in range(N): 403 | 404 | # Correction for dynamic KKT matching 405 | Y_k = Fy_model( X[k], U[k], d, t, par_ymk[:,k]) + mtimes(lambdayT,(U[k] - us)) 406 | 407 | if G_ineq != None: 408 | if slacks == True and slacksG == True: 409 | G_k = G_ineqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) - Sl[2*p:2*p+ng_v] 410 | else: 411 | G_k = G_ineqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) 412 | else: 413 | G_k = [] 414 | if H_eq != None: 415 | if slacks == True and slacksH == True: 416 | H_k = H_eqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) - Sl[2*p+ng_v:2*p+ng_v+nh_v] 417 | else: 418 | H_k = H_eqSX(X[k], U[k], Y_k, d, t, par_xmk[:,k], par_ymk[:,k]) 419 | else: 420 | H_k = [] 421 | 422 | g4.append(G_k) 423 | g5.append(H_k) 424 | 425 | if yFree is False: 426 | g1.append(Y_k) #bound constraint on Y_k 427 | 428 | if ContForm is True: 429 | Fk = F(x0=X[k], p=vertcat(U[k],d,t, xs, us, par_xmk[:,k], par_ymk[:,k])) 430 | g.append(X[k+1] - Fk['xf']) 431 | 432 | # Add contribution to the objective 433 | f_obj += Fk['qf'] 434 | 435 | 436 | else: 437 | X_next = X[k] + b1_t*(S1[k] - X[k]) + b2_t*(S2[k] - X[k])#transition to the next state wìfor collocation method in state representation 438 | #X_next = X[k]+h*(b1*S1[k]+b2*S2[k]) #transition to the next state for collocation method in derivative state 439 | 440 | 441 | if k == 0: 442 | DU_k = U[k] - um1 443 | else: 444 | DU_k = U[k] - U[k-1] 445 | 446 | if DuFree is False: 447 | g2.append(DU_k) #bound constraint on DU_k 448 | 449 | g.append(X_next - X[k+1]) 450 | # Defining variable entering the objective function 451 | dx = X[k] 452 | du = U[k] 453 | dy = Y_k 454 | ds1 = S1[k] 455 | ds2 = S2[k] 456 | ds = vertcat(ds1,ds2) 457 | 458 | if QForm is True: #Checking if the OF is quadratic 459 | dx = dx - xs 460 | du = du - us 461 | dy = dy - ys 462 | ds1 = ds1 - xs 463 | ds2 = ds2 - xs 464 | ds = vertcat(ds1,ds2) 465 | 466 | 467 | if DUForm is True: #Checking if the OF requires DU instead of u 468 | du = DU_k 469 | # if DUFormEcon is True: #Checking if the OF requires DU instead of u 470 | us_obj = DU_k if DUFormEcon is True else us 471 | 472 | #equality constrain in state rappresentation 473 | rg1 = 1/h*(D11*(S1[k]-X[k])+D12*(S2[k]-X[k]))-Fx_SX(S1[k],U[k],h,d,t,par_xmk[:,0]) 474 | rg2 = 1/h*(D21*(S1[k]-X[k])+D22*(S2[k]-X[k]))-Fx_SX(S2[k],U[k],h,d,t,par_xmk[:,0]) 475 | 476 | #equality constrain in derivative states S==K 477 | #rg1 = S1[k] - fx(X[k]+h*(a11*S1[k]+a12*S2[k]),U[k],h,d,t,dxm) 478 | #rg2 = S2[k] - fx(X[k]+h*(a21*S1[k]+a22*S2[k]),U[k],h,d,t,dxm) 479 | 480 | g3.append(rg1) #internal states 481 | g3.append(rg2) 482 | 483 | f_obj_new = F_obj( dx, du, dy, xs, us_obj, ys, ds) 484 | if slacks == True: 485 | f_obj_new = F_obj( dx, du, dy, xs, us_obj, ys, ds) + mtimes(Sl.T,mtimes(Ws,Sl)) 486 | f_obj += f_obj_new 487 | 488 | if slacks == True: 489 | sl_ub.append(Sl[0:p]) 490 | sl_lb.append(Sl[p:2*p]) 491 | 492 | dx = X[N] 493 | if QForm is True: #Checking if the OF is quadratic 494 | dx = dx - xs 495 | if TermCons is True: #Adding the terminal constraint 496 | g.append(dx) 497 | 498 | g = vertcat(*g) 499 | g1 = vertcat(*g1) #bound constraint on Y_k 500 | g2 = vertcat(*g2) #bound constraint on Du_k 501 | g3 = vertcat(*g3) #bound constraint on S_k 502 | g4 = vertcat(*g4) 503 | g5 = vertcat(*g5) 504 | if slacks == True: 505 | sl_ub = vertcat(*sl_ub) 506 | sl_lb = vertcat(*sl_lb) 507 | 508 | 509 | vfin = Vfin(dx,xs) 510 | f_obj += vfin #adding the final weight 511 | 512 | #Defining bound constraint 513 | w_lb = -DM.inf(nw) 514 | w_ub = DM.inf(nw) 515 | w_lb[0:n] = xmin 516 | w_ub[0:n] = xmax 517 | w_lb[nw-ns:nw] = DM.zeros(ns) # sl > 0 518 | 519 | ng = g.size1() #x 520 | ng1 = g1.size1() #y 521 | ng2 = g2.size1() #u 522 | ng3 = g3.size1() 523 | ng4 = g4.size1() 524 | ng5 = g5.size1() 525 | g_lb = DM.zeros(ng+ng1+ng2+ng3+ng4+ng5,1) 526 | g_ub = DM.zeros(ng+ng1+ng2+ng3+ng4+ng5,1) 527 | 528 | 529 | if ng1 != 0: # yFree = False 530 | if slacks == False: 531 | g_lb[ng:ng+ng1] = mtimes(ymin,DM.ones(N).T).reshape((ng1,1)) 532 | g_ub[ng:ng+ng1] = mtimes(ymax,DM.ones(N).T).reshape((ng1,1)) 533 | else: 534 | # Ridefine g1 (- inf < g1 < 0 ) 535 | g_lb = DM.zeros(ng+2*ng1+ng2+ng3+ng4+ng5,1) 536 | g_ub = DM.zeros(ng+2*ng1+ng2+ng3+ng4+ng5,1) 537 | g1_old = g1 538 | g1 = MX.zeros(2*ng1) 539 | g1[0:ng1] = mtimes(ymin,DM.ones(N).T).reshape((ng1,1)) - g1_old - sl_lb 540 | g1[ng1:2*ng1] = -mtimes(ymax,DM.ones(N).T).reshape((ng1,1)) + g1_old - sl_ub 541 | g_lb[ng:ng+2*ng1] = -DM.inf(2*ng1) 542 | ng1 = g1.size1() 543 | 544 | if ng2 != 0: 545 | g_lb[ng+ng1:ng+ng1+ng2] = mtimes(Dumin,DM.ones(N).T).reshape((ng2,1)) 546 | g_ub[ng+ng1:ng+ng1+ng2] = mtimes(Dumax,DM.ones(N).T).reshape((ng2,1)) 547 | 548 | if ng4 != 0: 549 | g_lb[ng+ng1+ng2+ng3:ng+ng1+ng2+ng3+ng4] = -DM.inf(ng4) 550 | 551 | 552 | for k in range(1,N+1,1): 553 | w_lb[k*nxuk:k*nxuk+n] = xmin 554 | w_ub[k*nxuk:k*nxuk+n] = xmax 555 | w_lb[k*nxuk-m-2*n:k*nxuk-m] = vertcat(xmin,xmin) 556 | w_ub[k*nxuk-m-2*n:k*nxuk-m] = vertcat(xmax,xmax) 557 | w_lb[k*nxuk-m:k*nxuk] = umin 558 | w_ub[k*nxuk-m:k*nxuk] = umax 559 | 560 | 561 | g = vertcat(g, g1, g2, g3, g4, g5) 562 | 563 | nlp = {'x':w, 'p':par, 'f':f_obj, 'g':g} 564 | 565 | solver = nlpsol('solver', 'ipopt', nlp, sol_opts) 566 | 567 | return [solver, w_lb, w_ub, g_lb, g_ub] 568 | -------------------------------------------------------------------------------- /Default_Values.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Wed Apr 13 11:33:23 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | 8 | from casadi import * 9 | from casadi.tools import * 10 | from matplotlib import pylab as plt 11 | import math 12 | import scipy.linalg as scla 13 | import numpy as np 14 | 15 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 16 | estimating = False # Set to True if you want to do only the estimation problem 17 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 18 | ssjacid = False # Set to True if you want a linearization of the process in ss point 19 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 20 | StateFeedback = False # Set to True if you have all the states measured 21 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 22 | Fp_nominal = False # Define the nominal case: fp = fx_model e hp = fy_model 23 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 24 | offree = 'no' # Set to 'lin'/'nl' to have a linear/non linear disturbance model 25 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 26 | 27 | ## BOUND CONSTRAINT 28 | 29 | # Input bounds 30 | umin = None 31 | umax = None 32 | 33 | # State bounds 34 | xmin = None 35 | xmax = None 36 | 37 | # Output bounds 38 | ymin = None 39 | ymax = None 40 | 41 | # Input bounds on the target problem 42 | umin_ss = None 43 | umax_ss = None 44 | 45 | # State bounds on the target problem 46 | xmin_ss = None 47 | xmax_ss = None 48 | 49 | # Output bounds on the target problem 50 | ymin_ss = None 51 | ymax_ss = None 52 | 53 | # Input bounds on the dynamic problem 54 | umin_dyn = None 55 | umax_dyn = None 56 | 57 | # State bounds on the dynamic problem 58 | xmin_dyn = None 59 | xmax_dyn = None 60 | 61 | # Output bounds on the dynamic problem 62 | ymin_dyn = None 63 | ymax_dyn = None 64 | 65 | # Disturbance bounds 66 | dmin = None 67 | dmax = None 68 | 69 | # DeltaInput bounds 70 | Dumin = None 71 | Dumax = None 72 | 73 | # State noise bounds 74 | wmin = None 75 | wmax = None 76 | 77 | # Ouput noise bounds 78 | vmin = None 79 | vmax = None 80 | 81 | ## OBJECTIVE FUNCTION 82 | 83 | ## Steady-state optimization 84 | QForm_ss = False # Set true if you have setpoint and you want y-y_sp and u-u_sp as optimization variables 85 | 86 | DUssForm = False # Set true if you want DUss = u_s[k]-u_s[k-1] as optimization variables rather than u_s[k] 87 | 88 | Adaptation = False # Set true to have the modifiers-adaption method 89 | 90 | ## Dynamic optimization 91 | ContForm = False # Set true if you want to integrate the objective function in continuous form 92 | 93 | TermCons = False # Set true if you want the terminal constraint 94 | 95 | QForm = False # Set true if you want x[k]-xs and u[k]-us as optimization variables 96 | 97 | DUForm = False # Set true if you want DU = u[k]-u[k-1] as optimization variables rather than u[k] 98 | 99 | DUFormEcon = False # Set true if you want DU = u[k]-u[k-1] as optimization variables added to u[k] in economic MPC 100 | 101 | ## Solver 102 | Sol_itmax = 100 103 | Sol_Hess_constss = 'no' # Set True if you want to calculate Hessian matrices every iteration 104 | Sol_Hess_constdyn = 'no' # Set True if you want to calculate Hessian matrices every iteration 105 | Sol_Hess_constmhe = 'no' 106 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 107 | ## Estimator 108 | #### Steady-state Kalman filter tuning params ################################# 109 | kalss = False # Set True if you want the Steady-state Kalman filter 110 | 111 | #### Luemberger observer tuning params ###################################### 112 | lue = False # Set True if you want the Luemberger observer 113 | 114 | #### Kalman filter tuning params ############################################# 115 | kal = False # Set True if you want the Kalman filter 116 | 117 | ############################################################################# 118 | #### Extended Kalman filter tuning params ################################### 119 | ekf = False # Set True if you want the Kalman filter 120 | 121 | #### Moving Horizon Estimation params ################################### 122 | mhe = False # Set True if you want the MHE 123 | 124 | Collocation = False # Set True if want the Collocation Method 125 | 126 | LinPar = True 127 | 128 | slacks = False 129 | 130 | slacksG = True 131 | slacksH = True 132 | -------------------------------------------------------------------------------- /Estimator.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on December 3, 2015 4 | 5 | @author: Marco, Mirco, Gabriele 6 | 7 | Estimator envelope file 8 | """ 9 | from __future__ import division 10 | 11 | from builtins import range 12 | from past.utils import old_div 13 | from casadi import * 14 | from casadi.tools import * 15 | from matplotlib import pylab as plt 16 | import math 17 | import scipy.linalg as scla 18 | import numpy as np 19 | from Utilities import* 20 | 21 | def defEstimator(Fx,Fy,y_k,u_k, estype,xhat_min, t_k, p_x, p_y, **kwargsin): 22 | if estype == 'kal': 23 | Q = kwargsin['Q'] 24 | R = kwargsin['R'] 25 | P_min = kwargsin['P_min'] 26 | [P_plus, P_corr, xhat_corr] = kalman(Fx,Fy,y_k,u_k,Q,R,P_min,xhat_min,t_k,p_y) 27 | kwargsout = {"P_plus": P_plus, "P_corr": P_corr} 28 | elif estype == 'ekf': 29 | Q = kwargsin['Q'] 30 | R = kwargsin['R'] 31 | P_min = kwargsin['P_min'] 32 | ts = kwargsin['ts'] 33 | [P_plus, P_corr, xhat_corr] = ekf(Fx,Fy,y_k,u_k,Q,R,P_min,xhat_min,ts,t_k,p_y,p_x) 34 | kwargsout = {"P_plus": P_plus, "P_corr": P_corr} 35 | elif estype == 'kalss': 36 | K = kwargsin['K'] 37 | [xhat_corr] = kalss(Fx,Fy,y_k,u_k,K,xhat_min,t_k,p_y) 38 | kwargsout = {} 39 | elif estype == 'mhe': 40 | Fobj = kwargsin['Fobj'] 41 | P_k = kwargsin['P_min'] 42 | 43 | sol = kwargsin['sol'] 44 | solwlb = kwargsin['solwlb'] 45 | solwub = kwargsin['solwub'] 46 | solglb = kwargsin['solglb'] 47 | solgub = kwargsin['solgub'] 48 | N = kwargsin['N'] 49 | ts = kwargsin['ts'] 50 | v_k = kwargsin['vk'] 51 | w_k = kwargsin['wk'] 52 | U = kwargsin['U'] 53 | X = kwargsin['X'] 54 | Xm = kwargsin['Xm'] 55 | Y = kwargsin['Y'] 56 | T = kwargsin['T'] 57 | V = kwargsin['V'] 58 | W = kwargsin['W'] 59 | xb = kwargsin['xb'] 60 | up = kwargsin['up'] 61 | Nmhe = kwargsin['Nmhe'] 62 | C = kwargsin['C'] 63 | G = kwargsin['G'] 64 | A = kwargsin['A'] 65 | B = kwargsin['B'] 66 | f = kwargsin['f'] 67 | h = kwargsin['h'] 68 | Qk = kwargsin['Qk'] 69 | Rk = kwargsin['Rk'] 70 | Sk = kwargsin['Sk'] 71 | Q = kwargsin['Q'] 72 | bU = kwargsin['bU'] 73 | P = kwargsin['P'] 74 | Pc = kwargsin['Pc'] 75 | P_kal = kwargsin['P_kal'] 76 | P_c_kal = kwargsin['P_c_kal'] 77 | pH = kwargsin['pH'] 78 | pO = kwargsin['pO'] 79 | pPyx = kwargsin['pPyx'] 80 | xm_kal = kwargsin['xm_kal'] 81 | PX = kwargsin['PX'] 82 | PY = kwargsin['PY'] 83 | nd = kwargsin['nd'] 84 | 85 | 86 | 87 | [P_k, xhat_corr, w_k,v_k,U,Y,T,Xm,X,V,W,xb,C,G, A, B,\ 88 | f, h, Qk, Rk, Sk, Q, bU,P, Pc, P_kal, P_c_kal, pH,pO,pPyx, xm_kal, xc_kal, PX, PY] = \ 89 | mhe(Fx,Fy,y_k,u_k,P_k,\ 90 | xhat_min,Fobj,ts,t_k,p_x,p_y,U,Y,T,Xm,X,V,W,w_k,v_k,xb,N,up,Nmhe,sol,solwlb,solwub,solglb,solgub,\ 91 | C, G, A, B, f, h, Qk, Rk, Sk, Q, bU,\ 92 | P, Pc, P_kal, P_c_kal, pH,pO,pPyx, xm_kal, PX, PY, nd) 93 | kwargsout = {"P_plus": P_k, "U_mhe" : U, "X_mhe" : X, "Xm_mhe" : Xm,\ 94 | "Y_mhe" : Y, "T_mhe" : T,"V_mhe" : V, "W_mhe" : W, "wk" : w_k, "vk" : v_k, "xb" : xb,\ 95 | "C_mhe" : C, "G_mhe" : G, "A_mhe" : A, "B_mhe" : B, "f_mhe" : f,\ 96 | "h_mhe" : h, "Qk_mhe" : Qk, "Rk_mhe" : Rk, "Sk_mhe" : Sk,\ 97 | "Q_mhe" : Q, "bigU_mhe" : bU, "P_mhe" : P, "Pc_mhe" : Pc, \ 98 | "P_kal_mhe" : P_kal, "P_c_kal_mhe" : P_c_kal, "pH_mhe" : pH, \ 99 | "pO_mhe" : pO, "pPyx_mhe" : pPyx, "xm_kal_mhe" : xm_kal,"xc_kal_mhe" : xc_kal,\ 100 | "PX_mhe":PX, "PY_mhe":PY} 101 | return [xhat_corr, kwargsout] 102 | 103 | def Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, *var, **kwargs): 104 | """ 105 | SUMMARY: 106 | Discrete-time steady-state Kalman filter gain calculation for the given 107 | linear system in state space form. 108 | 109 | SYNTAX: 110 | assignment = kalman(ny, nd, nx, Q_kf, R_kf, offree, linmod, *var, **kwargs) 111 | 112 | ARGUMENTS: 113 | + nx, ny , nd - State, output and disturbance dimensions 114 | + Q_kf - Process noise covariance matrix 115 | + R_kf - Measurements noise covariance matrix 116 | + offree - Offset free tag 117 | + linmod - Lineaity of the model tag 118 | + var - Positional variables 119 | + kwargs - Model and Disturbance matrices 120 | 121 | OUTPUTS: 122 | + Kaug - Steady-state Kalman filter gain 123 | """ 124 | if linmod == 'onlyA' or linmod == 'full': 125 | A = kwargs['A'] 126 | 127 | if linmod == 'onlyC' or linmod == 'full': 128 | C = kwargs['C'] 129 | 130 | try: 131 | A 132 | except NameError: 133 | Fx_model = kwargs['Fx'] 134 | x = var[0] 135 | u = var[1] 136 | k = var[2] 137 | d = var[3] 138 | t = var[4] 139 | h = var[5] 140 | px = var[6] 141 | py= var[7] 142 | x_ss = var[8] 143 | u_ss= var[9] 144 | px_ss= var[10] 145 | py_ss= var[11] 146 | 147 | # get the system matrices 148 | Fun_in = SX.get_input(Fx_model) 149 | Adummy = jacobian(Fx_model.call(Fun_in)[0], Fun_in[0]) 150 | 151 | d_ss = DM.zeros(nd) 152 | if offree == 'nl': 153 | xnew = vertcat(x,d) 154 | x_ss_p = vertcat(x_ss,d_ss) 155 | else: 156 | xnew = x 157 | x_ss_p = x_ss 158 | 159 | A_dm = Function('A_dm', [xnew,u,k,t,px], [Adummy]) 160 | 161 | A = A_dm(x_ss_p, u_ss, h, 0, px_ss) 162 | 163 | try: 164 | C 165 | except NameError: 166 | Fy_model = kwargs['Fy'] 167 | Fun_in = SX.get_input(Fy_model) 168 | Cdummy = jacobian(Fy_model.call(Fun_in)[0], Fun_in[0]) 169 | 170 | if 'Fx_model' not in locals: 171 | x = var[0] 172 | u = var[1] 173 | k = var[2] 174 | d = var[3] 175 | t = var[4] 176 | h = var[5] 177 | px = var[6] 178 | py= var[7] 179 | x_ss = var[8] 180 | u_ss= var[9] 181 | px_ss= var[10] 182 | py_ss= var[11] 183 | 184 | C_dm = Function('C_dm', [x,u,d,t,py], [Cdummy]) 185 | 186 | C = C_dm(x_ss, u_ss, d_ss, 0.0, py_ss) 187 | 188 | 189 | Aaug = DM.eye(nx+nd) 190 | Caug = DM.zeros(ny, nx+nd) 191 | 192 | if offree == 'nl': 193 | if A.size2() < nx+nd: 194 | Aaug[0:nx,0:nx] = A 195 | else: 196 | Aaug = A 197 | 198 | if C.size2() < nx+nd: 199 | Caug[0:ny,0:nx] = C 200 | else: 201 | Caug = C 202 | else: 203 | Aaug[0:nx,0:nx] = A 204 | Caug[0:ny,0:nx] = C 205 | 206 | if offree == "lin": 207 | Bd = kwargs["Bd"] 208 | Cd = kwargs["Cd"] 209 | 210 | Aaug[0:nx,nx:nx+nd] = Bd 211 | Caug[0:ny,nx:nx+nd] = Cd 212 | 213 | Ae = np.array(Aaug.T) 214 | Be = np.array(Caug.T) 215 | Qe = np.array(Q_kf) 216 | Re = np.array(R_kf) 217 | Pe = scla.solve_discrete_are(Ae,Be,Qe,Re) 218 | MAT = np.dot(Be.transpose(), Pe) 219 | MAT = np.dot(MAT, Be) + Re 220 | invMAT = np.linalg.inv(MAT) 221 | Ke = np.dot(Pe,Be) 222 | Ke = np.dot(Ke, invMAT) 223 | Kaug = DM(Ke) 224 | 225 | # Eigenvalue checks 226 | Aobs = Aaug - mtimes(mtimes(Aaug, Kaug), Caug) 227 | eigvals, eigvecs = scla.eig(Aobs) 228 | 229 | return (Kaug) 230 | 231 | def kalss(Fx,Fy,y_act,u_k,K,xhat_min,t_k,p_y): 232 | """ 233 | SUMMARY: 234 | Steady-state Discrete-time Kalman filter for the given linear system 235 | in state space form. 236 | 237 | SYNTAX: 238 | assignment = kalman(Fx,Fy,y_act,u_k,K,xhat_min,t_k) 239 | 240 | ARGUMENTS: 241 | + Fx - State correlation function 242 | + Fy - Output correlation function 243 | + y_act - Measurements, i.e. y(k) 244 | + u_k - Input, i.e. u(k) 245 | + K - Kalman filter gain 246 | + xhat_min - Predicted mean of the state, i.e. x(k|k-1) 247 | + t_k - Current time index 248 | 249 | OUTPUTS: 250 | + xhat_corr - Estimated mean of the state, i.e. x(k|k) 251 | """ 252 | # predicted output: y(k|k-1) 253 | yhat = Fy(xhat_min,u_k,t_k,p_y) 254 | 255 | # estimation error 256 | e_k = y_act - yhat 257 | 258 | # estimated mean of the state: x(k|k) 259 | xhat_corr = DM(xhat_min + mtimes(K, e_k)) 260 | 261 | return [xhat_corr] 262 | 263 | def kalman(Fx,Fy,y_act,u_k,Q,R,P_min,xhat_min,t_k,p_y): 264 | """ 265 | SUMMARY: 266 | Discrete-time Kalman filter for the given linear system in state space form. 267 | 268 | SYNTAX: 269 | assignment = kalman(Fx_model,Fy_model,y_act,u,Q,R,P_min,xhat_min) 270 | 271 | ARGUMENTS: 272 | + Fx - State correlation function 273 | + Fy - Output correlation function 274 | + Q - Process noise covariance matrix 275 | + R - Measurements noise covariance matrix 276 | + y_act - Measurements, i.e. y(k) 277 | + u - Input, i.e. u(k) 278 | + P_min - Predicted covariance of the state, i.e. P(k|k-1) 279 | + xhat_min - Predicted mean of the state, i.e. x(k|k-1) 280 | + t_k - Current time index 281 | 282 | OUTPUTS: 283 | + P_plus - Predicted covariance of the state, i.e. P(k+1|k) 284 | + P_corr - Estimated covariance of the state, i.e. P(k|k) 285 | + xhat_corr - Estimated mean of the state, i.e. x(k|k) 286 | """ 287 | # get the system matrices 288 | Fun_in = SX.get_input(Fx) 289 | A_dm = jacobian(Fx.call(Fun_in)[0], Fun_in[0]) 290 | Fun_in = SX.get_input(Fy) 291 | C_dm = jacobian(Fy.call(Fun_in)[0], Fun_in[0]) 292 | 293 | # predicted output: y(k|k-1) 294 | yhat = Fy(xhat_min,u_k,t_k,p_y) #mtimes(C_dm,xhat_min) 295 | 296 | # filter gain 297 | K = (solve((mtimes(mtimes(C_dm,P_min),C_dm.T) + R).T,(mtimes(P_min,C_dm.T)).T)).T 298 | 299 | # estimated covariance of the state: P(k|k) 300 | P_corr = mtimes(DM.eye(A_dm.shape[0]) - mtimes(K,C_dm), P_min) 301 | 302 | # estimation error 303 | e_k = y_act - yhat 304 | 305 | # estimated mean of the state: x(k|k) 306 | xhat_corr = DM(xhat_min + mtimes(K, e_k)) 307 | 308 | # predicted covariance of the state: P(k+1|k) 309 | P_plus = mtimes(mtimes(A_dm, P_corr),A_dm.T) + Q 310 | 311 | return [P_plus, P_corr, xhat_corr] 312 | 313 | def ekf(Fx,Fy,y_act,u_k,Q,R,P_min,xhat_min,ts,t_k,p_y,p_x): 314 | """ 315 | SUMMARY: 316 | Discrete-time extended Kalman filter for the given nonlinear system. 317 | 318 | SYNTAX: 319 | assignment = ekf(Fx_model,Fy_model,y_act,u,Q,R,P_min,xhat_min,h) 320 | 321 | ARGUMENTS: 322 | + Fx - State correlation function 323 | + Fy - Output correlation function 324 | + Q - Process noise covariance matrix 325 | + R - Measurements noise covariance matrix 326 | + y_act - Measurements, i.e. y(k) 327 | + u - Input, i.e. u(k) 328 | + P_min - Predicted covariance of the state, i.e. P(k|k-1) 329 | + xhat_min - Predicted mean of the state, i.e. x(k|k-1) 330 | + ts - Time step 331 | + t_k - Current time index 332 | 333 | 334 | OUTPUTS: 335 | + P_plus - Predicted covariance of the state, i.e. P(k+1|k) 336 | + P_corr - Estimated covariance of the state, i.e. P(k|k) 337 | + xhat_corr - Estimated mean of the state, i.e. x(k|k) 338 | """ 339 | # predicted output: y(k|k-1) 340 | yhat = Fy(xhat_min,u_k,t_k,p_y) 341 | 342 | # get linearization of measurements 343 | Fun_in = SX.get_input(Fy) 344 | C_dm = jacobian(Fy.call(Fun_in)[0], Fun_in[0]) # jac Fy_x 345 | C = Function('C', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3]], [C_dm]) 346 | 347 | # filter gain 348 | C_k = C(xhat_min,u_k,t_k,p_y) 349 | 350 | C_k = C_k.full() 351 | P_min = P_min.full() if hasattr(P_min, 'full') else P_min 352 | R = R.full() if hasattr(R, 'full') else R 353 | 354 | inbrackets = scla.inv(np.linalg.multi_dot([C_k,P_min,C_k.T]) + R) 355 | K_k = np.linalg.multi_dot([P_min,C_k.T,inbrackets]) 356 | 357 | # estimated covariance of the state: P(k|k) 358 | P_corr = P_min - np.linalg.multi_dot([K_k,C_k,P_min]) 359 | 360 | # estimation error 361 | e_k = y_act - yhat 362 | 363 | e_k = e_k.full() 364 | xhat_min = xhat_min.full() 365 | 366 | # estimated mean of the state: x(k|k) 367 | xhat_corr = xhat_min + np.dot(K_k, e_k) 368 | 369 | # get linearization of states 370 | Fun_in = SX.get_input(Fx) 371 | 372 | jac_Fx = jacobian(Fx.call(Fun_in)[0],Fun_in[0]) 373 | A = Function('A', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4]], [jac_Fx]) 374 | 375 | # next predicted covariance of the state: P(k+1|k) 376 | A_k = A(xhat_corr,u_k,ts,t_k,p_x) 377 | 378 | A_k = A_k.full() 379 | Q = Q.full() if hasattr(Q, 'full') else Q 380 | 381 | P_plus = np.linalg.multi_dot([A_k,P_corr,A_k.T]) + Q 382 | 383 | P_plus = DM(P_plus) 384 | xhat_corr = DM(xhat_corr) 385 | 386 | return [P_plus, P_corr, xhat_corr] 387 | 388 | def mhe(Fx,Fy,y_act,u_k,P_k,xhat_min,F_obj,ts,t_k,px,py,U,Y,T,Xmin,X,V,W,w_k,v_k,x_bar,\ 389 | N,mhe_up,N_mhe, solver, w_lb, w_ub, g_lb, g_ub,\ 390 | bigC, bigG, bigA, bigB, bigf, bigh, bigQk, bigRk, bigSk, bigQ, bigU,\ 391 | bigP, bigPc,P_k_kal,P_corr_kal,Hbig,Obig_r,Pycondx_inv_r,xm_kal,PX,PY,nd): 392 | 393 | """ 394 | SUMMARY: 395 | Moving horizon estimation method for the given nonlinear system. 396 | 397 | SYNTAX: 398 | assignment = mhe(Fx,Fy,y_act,u_k,P_k,xhat_min,F_obj,ts,t_k,px,py,U,Y,T,Xmin,X,V,W,w_k,v_k,x_bar,\ 399 | N,mhe_up,N_mhe, solver, w_lb, w_ub, g_lb, g_ub,\ 400 | bigC, bigG, bigA, bigB, bigf, bigh, bigQk, bigRk, bigSk, bigQ, bigU,\ 401 | bigP, bigPc,P_k_kal,P_corr_kal,Hbig,Obig_r,Pycondx_inv_r,xm_kal,PX,PY) 402 | 403 | ARGUMENTS: 404 | + Fx - State correlation function 405 | + Fy - Output correlation function 406 | + y_act - Measurements, i.e. y(k) 407 | + u_k - Input, i.e. u(k) 408 | + P_k - Predicted covariance of the state, i.e. P(k|k-1) 409 | + xhat_min - Predicted mean of the state, i.e. x(k|k-1) 410 | + F_obj - MHE problem objective function 411 | + ts - Time step 412 | + t_k - Current time index 413 | + px,py - Measurable parameters 414 | + U,Y,T,Xmin,X,V,W,DXM,DYM - Data vectors for inputs, measurements, time indeces, state, noises and measurable disturbances 415 | + w_k,v_k - Current process and measurement noises 416 | + x_bar - A priori state estimate 417 | + N - Growing MHE horizon length (once N = N_mhe it does not change anymore) 418 | + mhe_up - Updating prior weight method (choose between "filter" or "smooth") 419 | + N_mhe - MHE horizon length 420 | + solver, w_lb, w_ub, g_lb, g_ub - MHE optimization problem definition 421 | + bigC, bigG, bigA, bigB, bigf, bigh, bigQk, bigRk, bigSk, bigQ, bigU - System matrices/vectors used for smoothing update 422 | + bigP, bigPc,P_k_kal,P_corr_kal, xm_kal - Kalman filter quantities used for smoothing update 423 | + Hbig,Obig_r,Pycondx_inv_r - Inverse matrix values for prior weight calculation in MHE problem in case of smoothing update 424 | 425 | 426 | 427 | OUTPUTS: 428 | + P_plus - Predicted covariance of the state, i.e. P(k+1|k) 429 | + P_corr - Estimated covariance of the state, i.e. P(k|k) 430 | + xhat_corr - Estimated mean of the state, i.e. x(k|k) 431 | + U,Y,T,Xmin,X,V,W,DXM,DYM - Data vectors for inputs, measurements, time indeces, state, noises and measurable disturbances 432 | + bigC, bigG, bigA, bigB, bigf, bigh, bigQk, bigRk, bigSk, bigQ, bigU - System matrices/vectors used for smoothing update 433 | + bigP, bigPc,P_k_kal,P_corr_kal, xm_kal - Kalman filter quantities used for smoothing update 434 | + Hbig,Obig_r,Pycondx_inv_r - Inverse matrix values for prior weight calculation in MHE problem in case of smoothing update 435 | 436 | """ 437 | 438 | ksim = int(round(old_div(t_k,ts))) 439 | n = xhat_min.size1() 440 | m = u_k.size1() 441 | p = y_act.size1() 442 | npx = px.size1() 443 | npy = py.size1() 444 | 445 | 446 | # get linearization of measurements 447 | Fun_in = SX.get_input(Fy) 448 | C_dm = jacobian(Fy.call(Fun_in)[0], Fun_in[0]) # jac Fy_x 449 | C = Function('C', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3]], [C_dm]) 450 | 451 | # get linearization of states 452 | Fun_in = SX.get_input(Fx) 453 | A_dm = jacobian(Fx.call(Fun_in)[0],Fun_in[0]) # jac Fx_x 454 | A = Function('A', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4],Fun_in[5]], [A_dm]) 455 | 456 | B_dm = jacobian(Fx.call(Fun_in)[0],Fun_in[1]) # jac Fx_u 457 | B = Function('B', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4],Fun_in[5]], [B_dm]) 458 | 459 | G_dm = jacobian(Fx.call(Fun_in)[0],Fun_in[4]) # jac Fx_w 460 | G = Function('G', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4],Fun_in[5]], [G_dm]) 461 | 462 | n_w = G_dm.size2() #get w dimension 463 | 464 | nxv = n+p 465 | nxvw = nxv + n_w 466 | n_opt = N*nxvw + n # total # of variables 467 | 468 | # get linearization of objective function 469 | Fun_in = SX.get_input(F_obj) 470 | FobjIn = vertcat(Fun_in[0],Fun_in[1]) 471 | [H_dm,_] = hessian(F_obj.call(Fun_in)[0], FobjIn) 472 | H = Function('H', [Fun_in[0],Fun_in[1],Fun_in[2]], [H_dm]) 473 | 474 | ## Stacking data 475 | if ksim < N_mhe: 476 | if ksim == 0: 477 | U = vertcat(U,u_k) 478 | else: 479 | U = vertcat(U,u_k,u_k) #doubling u_k to maintain the lenght for U (last component is fictuous) 480 | Y = vertcat(Y,y_act) 481 | T = vertcat(T,t_k) 482 | Xmin = vertcat(Xmin,xhat_min) 483 | Yold = Y 484 | PX = vertcat(PX,px) 485 | PY = vertcat(PY,py) 486 | else: 487 | if N_mhe == 1: 488 | U = u_k 489 | Y = y_act 490 | T = t_k 491 | Xmin = xhat_min 492 | PX = px 493 | PY = py 494 | else: 495 | Yold = Y 496 | U = vertcat(U[m:],u_k,u_k) #doubling u_k to maintain the lenght for U (last component is fictuous) 497 | Y = vertcat(Y[p:],y_act) 498 | T = vertcat(T[1:],t_k) 499 | Xmin = vertcat(Xmin[n:],xhat_min) 500 | PX = vertcat(PX[npx:],px) 501 | PY = vertcat(PY[npy:],py) 502 | 503 | ## Initial guess (on the first NLP run) 504 | w_guess = DM.zeros(n_opt) 505 | for key in range(N): 506 | if key == 0: 507 | w_guess[key*nxvw:key*nxvw+n] = x_bar 508 | else: 509 | w_guess[key*nxvw:key*nxvw+n] = Fx(w_guess[(key-1)*nxvw:(key-1)*nxvw+n],U[(key-1)*m:(key-1)*m+m],ts,T[key-1],np.zeros((n_w,1)),PX[(key-1)*(npx):(npx)*key]) 510 | w_guess[key*nxvw+n:key*nxvw+nxv] = np.zeros((p,1))#v_k 511 | w_guess[key*nxvw+nxv:(key+1)*nxvw] = np.zeros((n_w,1))#w_k 512 | w_guess[N*nxvw:N*nxvw+n] = Fx(w_guess[key*nxvw:key*nxvw+n],U[key*m:key*m+m],ts,T[key],np.zeros((n_w,1)),PX[key*(npx):(npx)*(key+1)])#xhat_min #x_N 513 | 514 | 515 | ## Inverting P_k matrix for optimization solver 516 | pk = P_k.full() if hasattr(P_k, 'full') else P_k 517 | P_k_inv = scla.inv(pk) 518 | 519 | P_k_inv_r = DM(P_k_inv).reshape((n*n,1)) #The DM is needed to avoid error in reshape inside solver definition 520 | 521 | ## Set parameter for dynamic optimisation 522 | par = vertcat(U,Y,x_bar,P_k_inv_r,T,Pycondx_inv_r,Hbig,Obig_r,PX,PY) 523 | 524 | # Optimization problem 525 | sol = solver(lbx = w_lb, 526 | ubx = w_ub, 527 | x0 = w_guess, 528 | p = par, 529 | lbg = g_lb, 530 | ubg = g_ub) 531 | 532 | w_opt = sol["x"] 533 | xkp1k = w_opt[-n:] 534 | xhat_corr = w_opt[-n-nxvw:-nxvw] 535 | 536 | v_k = w_opt[-nxvw:-n-n_w] 537 | if ksim != 0 and N_mhe != 1: 538 | w_k = w_opt[-n-n_w:-n] 539 | 540 | 541 | ## Stacking data 542 | if ksim < N_mhe: 543 | X = vertcat(X,xkp1k) 544 | V = vertcat(V,v_k) 545 | W = vertcat(W,w_k) 546 | 547 | else: 548 | if N_mhe == 1: 549 | X = xkp1k 550 | V = v_k 551 | W = w_k 552 | else: 553 | X = vertcat(X[n:],xkp1k) 554 | V = vertcat(V[p:],v_k) 555 | W = vertcat(W[n_w:],w_k) 556 | 557 | 558 | if mhe_up == 'smooth' or mhe_up == 'filter': 559 | H_k = scla.inv(H(w_k,v_k,t_k).full()) 560 | Q_k = H_k[0:n_w,0:n_w] 561 | R_k = H_k[-p:,-p:] 562 | S_k = H_k[0:n_w,-p:] 563 | 564 | 565 | R = Function('R', [Fun_in[0],Fun_in[1],Fun_in[2]], [inv(H_dm[-p:,-p:])]) 566 | R_kk = R(w_k,v_k,t_k).full() 567 | 568 | C_k = C(xhat_corr,u_k,t_k,py).full() 569 | h_k = Y[-p:] - np.dot(C_k,xhat_corr) - v_k 570 | A_k = A(xhat_corr,u_k,ts,t_k,w_k,px).full() 571 | B_k = B(xhat_corr,u_k,ts,t_k,w_k,px).full() 572 | G_k = G(xhat_corr,u_k,ts,t_k,w_k,px).full() 573 | f_k = xkp1k - np.dot(A_k,xhat_corr) - np.dot(B_k,u_k) - np.dot(G_k,w_k) 574 | 575 | 576 | # Bulding the Kalman Filter covariance 577 | inbrackets = scla.inv(np.linalg.multi_dot([C_k,P_k_kal,C_k.T]) + R_k) 578 | K_k = np.linalg.multi_dot([P_k_kal,C_k.T,inbrackets]) 579 | 580 | # estimated covariance of the state: P(k|k) 581 | P_corr_kal = P_k_kal - np.linalg.multi_dot([K_k,C_k,P_k_kal]) 582 | 583 | # storing the right value of P_k_kal 584 | Pi = P_k_kal 585 | 586 | # predicted output: y(k|k-1) 587 | yhat = Fy(xm_kal,u_k,t_k,py) 588 | 589 | # estimation error 590 | e_k = y_act - yhat 591 | 592 | e_k = e_k.full() 593 | xm_kal = xm_kal.full() 594 | 595 | # estimated mean of the state: x(k|k) 596 | xc_kal = xm_kal + np.dot(K_k, e_k) 597 | 598 | # estimated mean of the state: x(k+1|k) 599 | xm_kal = Fx(xc_kal,u_k,ts,t_k,w_k,px) 600 | 601 | # next predicted covariance of the state: P(k+1|k) 602 | M_k = np.dot(-K_k,S_k.T) 603 | 604 | P_k_kal = np.linalg.multi_dot([A_k,P_corr_kal,A_k.T]) + \ 605 | np.linalg.multi_dot([G_k,Q_k,G_k.T]) + \ 606 | np.linalg.multi_dot([A_k,M_k,G_k.T]) + \ 607 | np.linalg.multi_dot([G_k,M_k,A_k.T]) 608 | 609 | # Storing data for covariance update 610 | bigC.append(C_k) 611 | bigG.append(G_k) 612 | bigA.append(A_k) 613 | bigB.append(B_k) 614 | bigf.append(f_k) 615 | bigh.append(h_k) 616 | bigQk.append(Q_k) 617 | bigRk.append(R_k) 618 | bigSk.append(S_k) 619 | bigQ.append(H_k) 620 | bigU.append(u_k) 621 | bigP.append(Pi) 622 | bigPc.append(P_corr_kal) 623 | 624 | 625 | # Update prior weight 626 | if ksim >= N_mhe-1: 627 | if mhe_up == 'filter': #Filtering 628 | # Calculating linearization of objective function 629 | 630 | ################# 631 | H_k = scla.inv(H(W[0:n_w],V[0:p],T[0]).full()) 632 | Q_k = H_k[0:n_w,0:n_w] 633 | R_k = H_k[-p:,-p:] 634 | S_k = H_k[0:n_w,-p:] 635 | ############################ 636 | C_k = C(Xmin[0:n],U[0:m],T[0],PY[0:npy]).full() 637 | inbrackets = scla.inv(np.linalg.multi_dot([C_k,P_k,C_k.T]) + R_k) 638 | K_k = np.linalg.multi_dot([P_k,C_k.T,inbrackets]) 639 | P_corr = P_k - np.linalg.multi_dot([K_k,C_k,P_k]) 640 | 641 | # next predicted covariance of the state: P(k+1|k) 642 | A_k = A(X[0:n],U[0:m],ts,T[0],W[0:n_w],PX[0:npx]).full() 643 | G_k = G(X[0:n],U[0:m],ts,T[0],W[0:n_w],PX[0:npx]).full() 644 | #The following terms comes from the correlation between v and w (Feng et al. 2013) 645 | M_k = np.dot(-K_k,S_k.T) 646 | 647 | P_k = np.linalg.multi_dot([A_k,P_corr,A_k.T]) + \ 648 | np.linalg.multi_dot([G_k,Q_k,G_k.T]) + \ 649 | np.linalg.multi_dot([A_k,M_k,G_k.T]) + \ 650 | np.linalg.multi_dot([G_k,M_k,A_k.T]) 651 | 652 | else: #Smoothing 653 | 654 | ## Backward Riccati iteration for smoothed covariances. 655 | Pisl = np.zeros((n*(N_mhe),n)) 656 | Pis = [Pisl[n*k:n*(k+1),:] for k in range(N_mhe)] 657 | Pis[N_mhe-1] = bigPc[N_mhe-1] 658 | for i in range(N_mhe-2, -1, -1): 659 | Piminv = scla.inv(bigP[i+1]) 660 | 661 | Pis[i] = bigPc[i] + np.linalg.multi_dot([bigPc[i],\ 662 | bigA[i].T,Piminv,(Pis[i+1] - bigP[i+1]),Piminv,bigA[i],bigPc[i]]) 663 | 664 | P_k = Pis[1] 665 | 666 | ############ Running again to build the Pycondx matrix ############# 667 | nvars = n + (N_mhe-2)*n_w + (N_mhe-1)*p #Sequence [x_{k-N_T}, w_{k-N_T}, v_{k-N_T}, ..., w_{k-1}, v_{k-1},v_{k}] #changed 668 | idx = N_mhe-1 669 | 670 | 671 | ## Shifting one step forward 672 | bigC = bigC[1:] 673 | bigG = bigG[1:] 674 | bigA = bigA[1:] 675 | bigB = bigB[1:] 676 | bigf = bigf[1:] 677 | bigh = bigh[1:] 678 | bigQk = bigQk[1:] 679 | bigRk = bigRk[1:] 680 | bigSk = bigSk[1:] 681 | bigQ = bigQ[1:] 682 | bigU = bigU[1:] 683 | bigP = bigP[1:] 684 | bigPc = bigPc[1:] 685 | 686 | ## Building Abig, Cbig, Hbig, Qbig for the smoothing problem 687 | Qbig = P_k 688 | Hbig = np.zeros((p*(idx),1)) 689 | Abig = np.zeros((n*(idx), nvars)) 690 | Arow = np.eye(n) 691 | Abig[0:n,0:n] = Arow 692 | Cbig = np.zeros((p*(idx), nvars)) 693 | 694 | Cbig[0:p, 0:(n+n_w+p)] = np.column_stack([bigC[0], np.zeros((p,n_w)), np.eye(p)]) 695 | Hbig[:p,:] = bigh[0] 696 | 697 | for i in range((N_mhe-2)): 698 | if i == 0: 699 | Apad = np.zeros((n,0)) 700 | else: 701 | Apad = np.zeros((n,p)) 702 | Arow = np.column_stack([np.dot(bigA[i],Arow), Apad , bigG[i]]) 703 | 704 | Abig[(i+1)*n:(i+2)*n, 0:Arow.shape[1]] = Arow; 705 | 706 | if i == N_mhe-3: 707 | Cpad = np.zeros((p,p)) 708 | else: 709 | Cpad = np.zeros((p, n_w+p)) 710 | 711 | Crow = np.column_stack([np.dot(bigC[i+1],Arow), Cpad, np.eye(p)]) 712 | Cbig[(i+1)*p:(i+2)*p, 0:Crow.shape[1]] = Crow 713 | 714 | Qbig = scla.block_diag(Qbig, bigQ[i]) 715 | 716 | if i == 0: 717 | Hrow = np.dot(bigB[i], bigU[i]) + bigf[i] 718 | else: 719 | Hrow = np.dot(bigA[i],Hrow) + np.dot(bigB[i], bigU[i]) + bigf[i] 720 | Hbig[(i+1)*p:(i+2)*p, :] = np.dot(bigC[i+1],Hrow) + bigh[i+1] 721 | 722 | # Adding the last component of Qbig 723 | Qbig = scla.block_diag(Qbig, R_kk) 724 | 725 | 726 | Obig = Cbig[:,0:n] 727 | Gbig = Cbig[:,n:] 728 | 729 | QRbig = Qbig[n:,n:] 730 | 731 | Pycondx = np.linalg.multi_dot([Gbig,QRbig,Gbig.T]) 732 | 733 | Obig_r = DM(Obig).reshape((p*(idx)*n,1)) 734 | Pycondx_inv = scla.inv(Pycondx) 735 | Pycondx_inv_r = DM(Pycondx_inv).reshape((p*(idx)*p*(idx),1)) 736 | 737 | 738 | ### x_bar updating 739 | if ksim >= N_mhe-1: #permitted only after packing enough information 740 | if mhe_up == 'filter': #Filtering 741 | if N_mhe == 1: 742 | x_bar = X 743 | v_bar = V 744 | w_bar = W 745 | else: 746 | x_bar = X[0:n] 747 | v_bar = V[0:p] 748 | w_bar = W[0:n_w] 749 | else: # Smoothing: picking the second component of the optimization sequence 750 | if N_mhe == 1: 751 | x_bar = w_opt[:n] 752 | v_bar = w_opt[n:] 753 | w_bar = w_k 754 | else: 755 | x_bar = w_opt[nxvw:nxvw+n] 756 | v_bar = w_opt[nxvw+n:nxvw+nxv] 757 | w_bar = w_opt[nxvw+nxv:2*nxvw] 758 | 759 | 760 | # Eliminating the last fictuous component 761 | if ksim == 0: 762 | U = [] 763 | else: 764 | U = U[:-m] 765 | 766 | 767 | return [P_k, xhat_corr, w_k,v_k,U,Y,T,Xmin,X,V,W,x_bar, bigC ,bigG,\ 768 | bigA, bigB, bigf, bigh, bigQk, bigRk, bigSk, bigQ, bigU,\ 769 | bigP, bigPc,P_k_kal,P_corr_kal,Hbig,Obig_r,Pycondx_inv_r,xm_kal,xc_kal,PX,PY] -------------------------------------------------------------------------------- /Ex_ENMPC.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from casadi import * 8 | from casadi.tools import * 9 | from matplotlib import pylab as plt 10 | import math 11 | import scipy.linalg as scla 12 | import numpy as np 13 | from Utilities import* 14 | 15 | ### 1) Simulation Fundamentals 16 | 17 | # 1.1) Simulation discretization parameters 18 | Nsim = 21 # Simulation length 19 | 20 | N = 25 # Horizon 21 | 22 | h = 2.0 # Time step 23 | 24 | # 3.1.2) Symbolic variables 25 | xp = SX.sym("xp", 2) # process state vector 26 | x = SX.sym("x", 2) # model state vector 27 | u = SX.sym("u", 1) # control vector 28 | y = SX.sym("y", 2) # measured output vector 29 | d = SX.sym("d", 2) # disturbance 30 | 31 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 32 | ### 2) Process and Model construction 33 | StateFeedback = True # Set to True if you have all the states measured 34 | 35 | cA0 = 1.0 # kmol/m^3 36 | V = 1.0 # m^3 ???? 37 | k1 = 1. # min^-1 38 | k2 = 0.05 # min^-1 39 | alfa = 1. # reactant price 40 | beta = 4. # product price 41 | 42 | # 2.1) Process Parameters 43 | 44 | # State map 45 | def User_fxp_Cont(xp,t,u,pxp,pxmp): 46 | """ 47 | SUMMARY: 48 | It constructs the function User_fxp_Cont for the non-linear case 49 | 50 | SYNTAX: 51 | assignment = User_fxp_Cont(xp,t,u) 52 | 53 | ARGUMENTS: 54 | + xp,u - Process State and input variable 55 | + t - Variable that indicate the current iteration 56 | 57 | OUTPUTS: 58 | + fx_p - Non-linear plant function 59 | """ 60 | fx_p = vertcat(u[0]*(cA0 - xp[0])/V - k1*xp[0], \ 61 | -u[0]*xp[1]/V + k1*xp[0] - k2*xp[1]) 62 | return fx_p 63 | 64 | Mx = 10 # Number of elements in each time step 65 | 66 | 67 | # # White Noise 68 | # G_wn = 1e-2*np.array([[1.0, 0.0], [0.0, 1.0]]) # State white noise matrix 69 | # Q_wn = 1e-1*np.array([[1.0, 0.0], [0.0, 1.0]]) # State white noise covariance matrix 70 | 71 | # 2.2) Model Parameters 72 | 73 | # State Map 74 | def User_fxm_Cont(x,u,d,t,px): 75 | """ 76 | SUMMARY: 77 | It constructs the function fx_model for the non-linear case 78 | 79 | SYNTAX: 80 | assignment = User_fxm_Cont(x,u,d,t) 81 | 82 | ARGUMENTS: 83 | + x,u,d - State, input and disturbance variable 84 | + t - Variable that indicate the real time 85 | 86 | OUTPUTS: 87 | + x_model - Non-linear model function 88 | """ 89 | x_model = vertcat(u[0]*(cA0 - x[0])/V - k1*x[0], \ 90 | -u[0]*x[1]/V + k1*x[0] - k2*x[1]) 91 | return x_model 92 | 93 | 94 | 95 | Mx = 10 # Number of elements in each time step 96 | 97 | # 2.3) Disturbance model for Offset-free control 98 | offree = "lin" 99 | Bd = np.zeros((d.size1(),d.size1())) 100 | Cd = np.eye(d.size1()) 101 | 102 | # 2.4) Initial condition 103 | x0_p = np.array([0.9, 0.1]) # plant 104 | x0_m = np.array([1.2, 0.5]) # model 105 | u0 = np.array([0.]) 106 | 107 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 108 | ### 3) State Estimation 109 | mhe_mod = 'on' # Decide to use MHE ('on') as estimator or EKF ('off') 110 | 111 | if mhe_mod == 'off': 112 | #### Extended Kalman filter tuning params ################################### 113 | ekf = True # Set True if you want the Kalman filter 114 | nx = x.size1() 115 | ny = y.size1() 116 | nd = d.size1() 117 | Qx_kf = 1.0e-8*np.eye(nx) 118 | Qd_kf = 1.0*np.eye(nd) 119 | Q_kf = scla.block_diag(Qx_kf, Qd_kf) 120 | R_kf = 1.0e-8*np.eye(ny) 121 | P0 = 1.0e-8*np.eye(nx+nd) 122 | else: 123 | 124 | #### Moving Horizon Estimation params ################################### 125 | mhe = True # Set True if you want the MHE 126 | N_mhe = 10 # Horizon lenght 127 | mhe_up = 'smooth' # Updating method for prior weighting and x_bar 128 | nx = x.size1() 129 | ny = y.size1() 130 | nd = d.size1() 131 | w = SX.sym("w", nx+nd) # state noise 132 | P0 = np.eye(nx+nd) 133 | x_bar = np.row_stack((np.atleast_2d(x0_m).T,np.zeros((nd,1)))) 134 | 135 | # Defining the state map 136 | def User_fx_mhe_Cont(x,u,d,t,px,w): 137 | """ 138 | SUMMARY: 139 | It constructs the function fx_model for the non-linear case in MHE problem 140 | 141 | SYNTAX: 142 | assignment = User_fx_mhe_Cont(x,u,d,t,w) 143 | 144 | ARGUMENTS: 145 | + x,u,d - State, input and disturbance variable 146 | + t - Variable that indicate the real time 147 | + w - Process noise variable 148 | 149 | OUTPUTS: 150 | + x_model - Non-linear model function 151 | """ 152 | x_model = vertcat(u[0]*(cA0 - x[0])/V - k1*x[0], \ 153 | -u[0]*x[1]/V + k1*x[0] - k2*x[1]) 154 | 155 | return x_model 156 | 157 | # Defining the MHE cost function 158 | def User_fobj_mhe(w,v,t): 159 | """ 160 | SUMMARY: 161 | It constructs the objective function for MHE problem 162 | 163 | SYNTAX: 164 | assignment = User_fobj_mhe(w,v,t) 165 | 166 | ARGUMENTS: 167 | + w,v - Process and Measurement noise variables 168 | + t - Variable that indicate the real time 169 | 170 | OUTPUTS: 171 | + fobj_mhe - Objective function 172 | """ 173 | Q = np.eye(nx+nd) 174 | R = np.eye(ny) 175 | fobj_mhe = 0.5*(xQx(w,inv(Q))+xQx(v,inv(R))) 176 | 177 | return fobj_mhe 178 | 179 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 180 | ### 4) Steady-state and dynamic optimizers 181 | 182 | # 4.1) Setpoints 183 | # As this is an economic MPC example, setpoints are not useful, 184 | # so there is no need to define them. 185 | 186 | # 4.2) Bounds constraints 187 | ## Input bounds 188 | umin = [0.00] 189 | umax = [2.0] 190 | 191 | ## State bounds 192 | xmin = np.array([0.00, 0.00]) 193 | xmax = np.array([1.00, 1.00]) 194 | 195 | # 4.3) Steady-state optimization : objective function 196 | def User_fssobj(x,u,y,xsp,usp,ysp): 197 | """ 198 | SUMMARY: 199 | It constructs the objective function for steady-state optimization 200 | 201 | SYNTAX: 202 | assignment = User_fssobj(x,u,y,xsp,usp,ysp) 203 | 204 | ARGUMENTS: 205 | + x,u,y - State, input and output variables 206 | + xsp,usp,ysp - State, input and output setpoint variables 207 | 208 | OUTPUTS: 209 | + obj - Objective function 210 | """ 211 | 212 | obj = u[0]*(alfa*cA0 - beta*y[1]) 213 | 214 | return obj 215 | 216 | # 4.4) Dynamic optimization : objective function 217 | def User_fobj_Cont(x,u,y,xs,us,ys): 218 | """ 219 | SUMMARY: 220 | It constructs the objective function for dynamic optimization 221 | 222 | SYNTAX: 223 | assignment = User_fobj_Cont(x,u,y,xs,us,ys) 224 | 225 | ARGUMENTS: 226 | + x,u,y - State, input and output variables 227 | + xs,us,ys - State, input and output stationary variables 228 | 229 | OUTPUTS: 230 | + obj - Objective function 231 | """ 232 | obj = u[0]*(alfa*cA0 - beta*y[1]) 233 | return obj 234 | 235 | # Terminal weight 236 | def User_vfin(x,xs): 237 | """ 238 | SUMMARY: 239 | It constructs the terminal weight for the dynamic optimization problem 240 | 241 | SYNTAX: 242 | assignment = User_vfin(x) 243 | 244 | ARGUMENTS: 245 | + x - State variables 246 | 247 | OUTPUTS: 248 | + vfin - Terminal weight 249 | """ 250 | diffx = x - xs 251 | vfin = mtimes(diffx.T,mtimes(2000,diffx)) 252 | return vfin 253 | 254 | # Options 255 | Sol_itmax = 200 256 | 257 | 258 | # S_cust = 20 259 | 260 | # R_cust = 10 261 | 262 | # Q_cust = 5*np.eye((x.size1())) 263 | # K_cust = 5*np.eye((x.size1())) #weight of adding variables for collocation methods 264 | 265 | 266 | # b1 = 0.5; b2 = 0.5 267 | 268 | 269 | # def User_fobj_Dis(x,u,y,xs,us,ys): 270 | # """ 271 | # SUMMARY: 272 | # It constructs the objective function for dynamic optimization 273 | 274 | 275 | 276 | # SYNTAX: 277 | # assignment = User_fobj_Dis(x,u,y,xs,us,ys) 278 | 279 | 280 | 281 | # ARGUMENTS: 282 | # + x,u,y - State, input and output variables 283 | # + xs,us,ys - State, input and output stationary variables 284 | 285 | 286 | 287 | # OUTPUTS: 288 | # + obj - Objective function 289 | # """ 290 | 291 | # obj = 0.5*(xQx(x,Q_cust) + xQx(u,R_cust) + xQx(us,S_cust)) 292 | 293 | 294 | # return obj 295 | 296 | 297 | 298 | # def User_fobj_Coll(x,u,y,xs,us,ys,s_Coll): 299 | # """ 300 | # SUMMARY: 301 | # It constructs the objective function for dynamic optimization 302 | 303 | 304 | 305 | # SYNTAX: 306 | # assignment = User_fobj_Coll(x,u,y,xs,us,ys,s_Coll) 307 | 308 | 309 | 310 | # ARGUMENTS: 311 | # + x,u,y - State, input and output variables 312 | # + xs,us,ys - State, input and output stationary variables 313 | # + s_Coll - Internal state variables 314 | 315 | 316 | 317 | # OUTPUTS: 318 | # + obj - Objective function 319 | # """ 320 | # s_Coll1 = s_Coll[0:x.size1()] 321 | # s_Coll2 = s_Coll[x.size1():2*x.size1()] 322 | 323 | # # obj = 0.5*h*(b1*(xQx((s_Coll1-xs), K_cust) + xQx(u,R_cust) + xQx(us,S_cust)) + 324 | # # b2*(xQx((s_Coll2-xs), K_cust) + xQx(u,R_cust) + xQx(us,S_cust))) 325 | # # obj = 0.5*(xQx(x,Q_cust) + xQx(u,R_cust) + xQx(us,S_cust)) 326 | # obj = u[0]*(alfa*cA0 - beta*y[1]) 327 | 328 | # return obj 329 | 330 | # DUFormEcon = True 331 | # QForm = True 332 | # Collocation = True 333 | ContForm = True 334 | 335 | -------------------------------------------------------------------------------- /Ex_LMPC_CSTR.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from casadi import * 8 | from casadi.tools import * 9 | from matplotlib import pylab as plt 10 | import math 11 | import scipy.linalg as scla 12 | import numpy as np 13 | from Utilities import* 14 | 15 | ### 1) Simulation Fundamentals 16 | 17 | # 1.1) Simulation discretization parameters 18 | Nsim = 100 # Simulation length 19 | 20 | N = 50 # Horizon 21 | 22 | h = 1 # Time step 23 | 24 | # 3.1.2) Symbolic variables 25 | xp = SX.sym("xp", 3) # process state vector 26 | x = SX.sym("x", 3) # model state vector 27 | u = SX.sym("u", 2) # control vector 28 | y = SX.sym("y", 3) # measured output vector 29 | d = SX.sym("d", 3) # disturbance 30 | 31 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 32 | ### 2) Process and Model construction 33 | 34 | # 2.1) Process Parameters 35 | Ap = np.array([[0.2511, -3.368*1e-03, -7.056*1e-04], [11.06, .3296, -2.545], [0.0, 0.0, 1.0]]) 36 | Bp = np.array([[-5.426*1e-03, 1.53*1e-05], [1.297, .1218], [0.0, -6.592*1e-02]]) 37 | Cp = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0],[0.0, 0.0, 1.0]]) 38 | 39 | # Additive State Disturbances 40 | def def_pxp(t): 41 | """ 42 | SUMMARY: 43 | It constructs the additive disturbances for the linear case 44 | 45 | SYNTAX: 46 | assignment = defdp(k) 47 | 48 | ARGUMENTS: 49 | + t - Variable that indicate the current time 50 | 51 | OUTPUTS: 52 | + dxp - State disturbance value 53 | """ 54 | 55 | if t <= 20: 56 | dxp = np.array([0.1, 0.0, 0.0]) # State disturbance 57 | else: 58 | dxp = np.array([0.0, 0.0, 0.0]) # State disturbance 59 | 60 | return [dxp] 61 | 62 | def def_pyp(t): 63 | """ 64 | SUMMARY: 65 | It constructs the additive disturbances for the linear case 66 | 67 | SYNTAX: 68 | assignment = defdp(k) 69 | 70 | ARGUMENTS: 71 | + t - Variable that indicate the current time 72 | 73 | OUTPUTS: 74 | + dyp - Output disturbance value 75 | """ 76 | 77 | dyp = np.array([0.1, 0.1, 0.0]) # Output disturbance 78 | 79 | return [dyp] 80 | 81 | 82 | # 2.2) Model Parameters 83 | A = np.array([[0.2511, -3.368*1e-03, -7.056*1e-04], [11.06, .3296, -2.545], [0.0, 0.0, 1.0]]) 84 | B = np.array([[-5.426*1e-03, 1.53*1e-05], [1.297, .1218], [0.0, -6.592*1e-02]]) 85 | C = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0],[0.0, 0.0, 1.0]]) 86 | 87 | # 2.3) Disturbance model for Offset-free control 88 | offree = "lin" 89 | Bd = np.eye(d.size1()) 90 | Cd = np.zeros((y.size1(),d.size1())) 91 | 92 | # 2.4) Initial condition 93 | x0_p = 3*np.ones((xp.size1(),1)) 94 | x0_m = 3*np.ones((x.size1(),1)) 95 | u0 = 0*np.ones((u.size1(),1)) 96 | 97 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 98 | ### 3) State Estimation 99 | 100 | # Kalman filter tuning params 101 | kal = True # Set True if you want the Kalman filter 102 | ########## Variables dimensions ############### 103 | nx = x.size1() # state vector # 104 | nu = u.size1() # control vector # 105 | ny = y.size1() # measured output vector # 106 | nd = d.size1() # disturbance # 107 | ############################################### 108 | Qx_kf = 1.0e-7*np.eye(nx) 109 | Qd_kf = np.eye(nd) 110 | Q_kf = scla.block_diag(Qx_kf, Qd_kf) 111 | R_kf = 1.0e-7*np.eye(ny) 112 | P0 = 1.0e-8*np.eye(nx+nd) 113 | 114 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 115 | ### 4) Steady-state and dynamic optimizers 116 | 117 | # 4.1) Setpoints 118 | 119 | def defSP(t): 120 | """ 121 | SUMMARY: 122 | It constructs the setpoints vectors for the steady-state optimisation 123 | 124 | SYNTAX: 125 | assignment = defSP(t) 126 | 127 | ARGUMENTS: 128 | + t - Variable that indicates the current time 129 | 130 | OUTPUTS: 131 | + ysp, usp, xsp - Input, output and state setpoint values 132 | """ 133 | xsp = np.array([0.0, 0.0, 0.0]) # State setpoints 134 | if t<= 15: 135 | ysp = np.array([0.2, 0.0, 0.0]) # Output setpoint 136 | usp = np.array([0., 0.]) # Input setpoints 137 | else: 138 | ysp = np.array([0.0, 0.0, 0.1]) # Output setpoint 139 | usp = np.array([0., 0.]) # Control setpoints 140 | 141 | return [ysp, usp, xsp] 142 | 143 | # 4.2) Bounds constraints 144 | ## Input bounds 145 | umin = -10.0*np.ones((u.size1(),1)) 146 | umax = 10.0*np.ones((u.size1(),1)) 147 | 148 | ## State bounds 149 | xmin = np.array([-10.0, -8.0, -10.0]) 150 | xmax = 10.0*np.ones((x.size1(),1)) 151 | 152 | ## Output bounds 153 | ymin = np.array([-10.0, -8.0, -10.0]) 154 | ymax = 10.0*np.ones(y.size1()) 155 | 156 | # 4.3) Steady-state optimization : objective function 157 | Qss = np.array([[20.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) #Output matrix 158 | Rss = np.zeros((u.size1(),u.size1())) # Control matrix 159 | 160 | # 4.4) Dynamic optimization : objective function 161 | Q = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) 162 | R = 0.1*np.eye(u.size1()) 163 | -------------------------------------------------------------------------------- /Ex_LMPC_WB.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from casadi import * 8 | from casadi.tools import * 9 | from matplotlib import pylab as plt 10 | import math 11 | import scipy.linalg as scla 12 | import numpy as np 13 | from Utilities import* 14 | 15 | ### 1) Simulation Fundamentals 16 | 17 | # 1.1) Simulation discretization parameters 18 | Nsim = 100 # Simulation length 19 | 20 | N = 50 # Horizon 21 | 22 | h = 1 # Time step 23 | 24 | # 3.1.2) Symbolic variables 25 | xp = SX.sym("xp", 4) # process state vector 26 | x = SX.sym("x", 4) # model state vector 27 | u = SX.sym("u", 2) # control vector 28 | y = SX.sym("y", 2) # measured output vector 29 | d = SX.sym("d", 2) # disturbance 30 | 31 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 32 | ### 2) Process and Model construction 33 | 34 | # 2.1) Process Parameters 35 | Ap = np.diag([0.8871,0.8324,0.9092,0.8703]) 36 | Bp = np.array([[1,0], [1,0], [0.0, 1.], [0,2.]]) 37 | Cp = np.array([[1.4447, 0.0, -1.7169, 0.], [0.0, 1.1064, 0.0, -1.2579]]) 38 | 39 | 40 | # 2.2) Model Parameters 41 | A = np.diag([0.8871,0.8324,0.9092,0.8703]) 42 | A2 = 2*np.diag([0.01,-0.01,-0.01,0.01]) 43 | A = A + A2 44 | B = np.array([[1,0], [1,0], [0.0, 1.], [0,2.]]) 45 | C = np.array([[1.4447, 0.0, -1.7169, 0.], [0.0, 1.1064, 0.0, -1.2579]]) 46 | 47 | # 2.3) Disturbance model for Offset-free control 48 | offree = "lin" 49 | Bd = np.zeros((x.size1(),d.size1())) 50 | Cd = np.eye(d.size1()) 51 | 52 | # 2.4) Initial condition 53 | x0_p = 0*np.ones((xp.size1(),1)) 54 | x0_m = 0*np.ones((x.size1(),1)) 55 | u0 = 0*np.ones((u.size1(),1)) 56 | 57 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 58 | ### 3) State Estimation 59 | 60 | # Dimensions 61 | nx = x.size1() # state vector # 62 | nu = u.size1() # control vector # 63 | ny = y.size1() # measured output vector # 64 | nd = d.size1() # disturbance # 65 | 66 | # Luemberger filter tuning params 67 | lue = True 68 | Kx = np.zeros((nx,nd)) 69 | Kd = np.eye(nd) 70 | K = np.vstack((Kx,Kd)) 71 | 72 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 73 | ### 4) Steady-state and dynamic optimizers 74 | 75 | # 4.1) Setpoints 76 | 77 | def defSP(t): 78 | """ 79 | SUMMARY: 80 | It constructs the setpoints vectors for the steady-state optimisation 81 | 82 | SYNTAX: 83 | assignment = defSP(t) 84 | 85 | ARGUMENTS: 86 | + t - Variable that indicates the current time 87 | 88 | OUTPUTS: 89 | + ysp, usp, xsp - Input, output and state setpoint values 90 | """ 91 | xsp = np.array([0.0, 0.0,0.0, 0.0]) # State setpoints 92 | if t<= 10: 93 | ysp = np.array([0.0, 0.0]) # Output setpoint 94 | usp = np.array([0., 0.]) # Input setpoints 95 | else: 96 | ysp = np.array([1.0, -1.0]) # Output setpoint 97 | usp = np.array([0., 0.]) # Control setpoints 98 | 99 | return [ysp, usp, xsp] 100 | 101 | # 4.2) Bounds constraints 102 | ## Input bounds 103 | umin = -0.5*np.ones((u.size1(),1)) 104 | umax = 0.5*np.ones((u.size1(),1)) 105 | 106 | ## State bounds 107 | # xmin = np.array([-10.0, -8.0, -10.0]) 108 | # xmax = 10.0*np.ones((x.size1(),1)) 109 | 110 | ## Output bounds 111 | # ymin = np.array([-10.0, -8.0, -10.0]) 112 | # ymax = 10.0*np.ones((y.size1(),1)) 113 | 114 | # 4.3) Steady-state optimization : objective function 115 | Qss = np.diag([1,1]) 116 | Rss = np.zeros((u.size1(),u.size1())) # Control matrix 117 | 118 | # 4.4) Dynamic optimization : objective function 119 | Qy = np.diag([1,1]) 120 | Q = np.dot(C.T,np.dot(Qy,C)) 121 | S = np.diag([10,20]) 122 | -------------------------------------------------------------------------------- /Ex_LMPC_nlplant.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from __future__ import division 8 | from past.utils import old_div 9 | from casadi import * 10 | from casadi.tools import * 11 | from matplotlib import pylab as plt 12 | import math 13 | import scipy.linalg as scla 14 | import numpy as np 15 | from Utilities import* 16 | 17 | 18 | ### 1) Simulation Fundamentals 19 | 20 | # 1.1) Simulation discretization parameters 21 | Nsim = 200 # Simulation length 22 | 23 | N = 50 # Horizon 24 | 25 | h = 0.2 # Time step 26 | 27 | # 3.1.2) Symbolic variables 28 | xp = SX.sym("xp", 3) # process state vector 29 | x = SX.sym("x", 3) # model state vector 30 | u = SX.sym("u", 2) # control vector 31 | y = SX.sym("y", 2) # measured output vector 32 | d = SX.sym("d", 2) # disturbance 33 | 34 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 35 | ### 2) Process and Model construction 36 | 37 | # 2.1) Process Parameters 38 | 39 | # State map 40 | def User_fxp_Cont(x,t,u,pxp,pxmp): 41 | """ 42 | SUMMARY: 43 | It constructs the function fx_p for the non-linear case 44 | 45 | SYNTAX: 46 | assignment = User_fxp_Cont(x,t,u) 47 | 48 | ARGUMENTS: 49 | + x - State variable 50 | + t - Current time 51 | + u - Input variable 52 | 53 | OUTPUTS: 54 | + fx_p - Non-linear plant function 55 | """ 56 | F0 = 0.1 # m^3/min 57 | T0 = 350 # K 58 | c0 = 1.0 # kmol/m^3 59 | r = 0.219 # m 60 | k0 = 7.2e10 # min^-1 61 | EoR = 8750 # K 62 | U0 = 915.6*60/1000 # kJ/min*m^2*K 63 | rho = 1000.0 # kg/m^3 64 | Cp2 = 0.239 # kJ/kg 65 | DH = -5.0e4 # kJ/kmol 66 | pi = math.pi 67 | kT0 = k0*exp(old_div(-EoR,T0)) 68 | 69 | fx_p = vertcat\ 70 | (\ 71 | F0*(c0 - x[0])/(pi* r**2 *x[2]) - kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0], \ 72 | F0*(T0 - x[1])/(pi* (r**2) *x[2]) -DH/(rho*Cp2)*kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0] + 2*U0/(r*rho*Cp2)*(u[0] - x[1]), \ 73 | old_div((F0 - u[1]),(pi*r**2)) \ 74 | ) 75 | 76 | return fx_p 77 | 78 | Mx = 10 # Number of elements in each time step 79 | 80 | # Output map 81 | Cp = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) 82 | 83 | 84 | # 2.2) Model Parameters 85 | A = np.array([[0.51448, -0.00917517, -0.117995],[53.6817, 2.15004, -3.77725], [0.0, 0.0, 1]]) 86 | B = np.array([[-0.0017669, 0.0864569], [0.639423, 1.60696], [0.0, -1.32737]]) 87 | C = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) 88 | 89 | # Linearization parameters 90 | xlin = np.array([0.5, 350, 0.659]) 91 | ulin = np.array([300, 0.1]) 92 | 93 | # 2.3) Disturbance model for Offset-free control 94 | offree = "lin" 95 | Bd = np.array([[-0.0017669, 0.0864569], [0.639423, 1.60696], [0.0, -1.32737]]) 96 | Cd = np.zeros((y.size1(),d.size1())) 97 | 98 | # 2.4) Initial condition 99 | x0_p = np.array([0.5, 350, 0.659]) # plant 100 | x0_m = np.array([0.5, 350, 0.659]) # model 101 | u0 = np.array([300, 0.1]) 102 | 103 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 104 | ### 3) State Estimation 105 | 106 | # Kalman filter tuning params 107 | kal = True # Set True if you want the Kalman filter 108 | nx = x.size1() 109 | ny = y.size1() 110 | nd = d.size1() 111 | Qx_kf = 1.0e-5*np.eye(nx) 112 | Qd_kf = np.eye(nd) 113 | Q_kf = scla.block_diag(Qx_kf, Qd_kf) 114 | R_kf = 1.0e-4*np.eye(ny) 115 | P0 = 1e-3*Q_kf 116 | 117 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 118 | ### 4) Steady-state and dynamic optimizers 119 | 120 | # 4.1) Setpoints 121 | def defSP(t): 122 | """ 123 | SUMMARY: 124 | It constructs the setpoints vectors for the steady-state optimisation 125 | 126 | SYNTAX: 127 | assignment = defSP(t) 128 | 129 | ARGUMENTS: 130 | + t - Variable that indicates the current time 131 | 132 | OUTPUTS: 133 | + ysp, usp, xsp - Input, output and state setpoint values 134 | """ 135 | xsp = np.array([0.0, 0.0, 0.0]) # State setpoints 136 | usp = np.array([299.963, 0.1]) # Control setpoints 137 | 138 | if t < 20: 139 | ysp = np.array([0.5, 0.659]) # Output setpoint 140 | elif t < 40: 141 | ysp = np.array([0.51, 0.659]) # Output setpoint 142 | else: 143 | ysp = np.array([0.50, 0.659]) 144 | 145 | return [ysp, usp, xsp] 146 | 147 | # 4.2) Bounds constraints 148 | ## Input bounds 149 | umin = np.array([295, 0.00]) 150 | umax = np.array([305, 0.25]) 151 | 152 | ## State bounds 153 | xmin = np.array([0.0, 320, 0.45]) 154 | xmax = np.array([1.0, 375, 0.75]) 155 | 156 | # 4.3) Steady-state optimization : objective function 157 | Qss = np.array([[10.0, 0.0], [0.0, 0.01]]) #Output matrix 158 | Rss = np.array([[0.0, 0.0], [0.0, 0.0]]) # Control matrix 159 | 160 | # 4.4) Dynamic optimization : objective function 161 | Q = np.array([[10.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) 162 | S = np.array([[0.1, 0.0], [0.0, 0.1]]) # DeltaU matrix -------------------------------------------------------------------------------- /Ex_LMPCxp_nlplant.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from __future__ import division 8 | from past.utils import old_div 9 | from casadi import * 10 | from casadi.tools import * 11 | from matplotlib import pylab as plt 12 | import math 13 | import scipy.linalg as scla 14 | import numpy as np 15 | from Utilities import* 16 | 17 | ### 1) Simulation Fundamentals 18 | 19 | # 1.1) Simulation discretization parameters 20 | Nsim = 200 # Simulation length 21 | 22 | N = 50 # Horizon 23 | 24 | h = 0.2 # Time step 25 | 26 | # 3.1.2) Symbolic variables 27 | xp = SX.sym("xp", 3) # process state vector 28 | x = SX.sym("x", 4) # model state vector 29 | u = SX.sym("u", 2) # control vector 30 | y = SX.sym("y", 2) # measured output vector 31 | d = SX.sym("d", 2) # disturbance 32 | 33 | nx = x.size1() 34 | ny = y.size1() 35 | nd = d.size1() 36 | nu = u.size1() 37 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 38 | ### 2) Process and Model construction 39 | 40 | # 2.1) Process Parameters 41 | 42 | # State map 43 | def User_fxp_Cont(x,t,u,pxp,pxmp): 44 | """ 45 | SUMMARY: 46 | It constructs the function fx_p for the non-linear case 47 | 48 | SYNTAX: 49 | assignment = User_fxp_Cont(x,t,u) 50 | 51 | ARGUMENTS: 52 | + x - State variable 53 | + t - Current time 54 | + u - Input variable 55 | 56 | OUTPUTS: 57 | + fx_p - Non-linear plant function 58 | """ 59 | F0 = 0.1 # m^3/min 60 | T0 = 350 # K 61 | c0 = 1.0 # kmol/m^3 62 | r = 0.219 # m 63 | k0 = 7.2e10 # min^-1 64 | EoR = 8750 # K 65 | U0 = 915.6*60/1000 # kJ/min*m^2*K 66 | rho = 1000.0 # kg/m^3 67 | Cp2 = 0.239 # kJ/kg 68 | DH = -5.0e4 # kJ/kmol 69 | pi = math.pi 70 | kT0 = k0*exp(old_div(-EoR,T0)) 71 | 72 | fx_p = vertcat\ 73 | (\ 74 | F0*(c0 - x[0])/(pi* r**2 *x[2]) - kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0], \ 75 | F0*(T0 - x[1])/(pi* (r**2) *x[2]) -DH/(rho*Cp2)*kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0] + 2*U0/(r*rho*Cp2)*(u[0] - x[1]), \ 76 | old_div((F0 - u[1]),(pi*r**2)) \ 77 | ) 78 | 79 | return fx_p 80 | 81 | Mx = 10 # Number of elements in each time step 82 | 83 | # Output map 84 | Cp = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) 85 | 86 | 87 | # 2.2) Model Parameters 88 | Alin = np.array([[0.51448, -0.00917517, -0.117995],[53.6817, 2.15004, -3.77725], [0.0, 0.0, 1]]) 89 | Blin = np.array([[-0.0017669, 0.0864569], [0.639423, 1.60696], [0.0, -1.32737]]) 90 | Clin = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) 91 | 92 | ## Extra state (to test different plant/model size) 93 | Phi = 0.01 94 | B_Phi = np.array([[1.0 - Phi, 0.0]]) 95 | C_Phi = (old_div(Phi,10.0))*np.array([[1.0],[0.0]]) 96 | 97 | A = scla.block_diag(Alin, Phi) 98 | B = np.row_stack([Blin,B_Phi]) 99 | C = np.column_stack([Clin, C_Phi]) 100 | 101 | # Linearisation parameters 102 | xlin = np.array([0.5, 350, 0.659, 0.0]) 103 | ulin = np.array([300, 0.1]) 104 | ylin = np.array([0.5, 0.659]) 105 | 106 | # 2.3) Disturbance model for Offset-free control 107 | offree = "lin" 108 | Bd = B 109 | Cd = np.zeros((ny,nd)) 110 | 111 | # 2.4) Initial condition 112 | x0_p = np.array([0.5, 350, 0.659]) # plant 113 | x0_m = np.array([0.5, 350, 0.659, 0.0]) # plant 114 | u0 = np.array([300, 0.1]) 115 | 116 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 117 | ### 3) State Estimation 118 | 119 | # Kalman filter tuning params 120 | kal = True # Set True if you want the Kalman filter 121 | Qx_kf = 1.0e-2*np.eye(nx) 122 | Qd_kf = np.eye(nd) 123 | Q_kf = scla.block_diag(Qx_kf, Qd_kf) 124 | R_kf = 1.0e-2*np.eye(ny) 125 | P0 = Q_kf 126 | 127 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 128 | ### 4) Steady-state and dynamic optimizers 129 | 130 | # 4.1) Setpoints 131 | def defSP(t): 132 | """ 133 | SUMMARY: 134 | It constructs the setpoints vectors for the steady-state optimization 135 | 136 | SYNTAX: 137 | assignment = defSP(t) 138 | 139 | ARGUMENTS: 140 | + t - Variable that indicates the current time 141 | 142 | OUTPUTS: 143 | + ysp, usp, xsp - Input, output and state setpoint values 144 | """ 145 | xsp = np.array([0.0, 0.0, 0.0, 0.0]) # State setpoints 146 | usp = np.array([300., 0.1]) # Control setpoints 147 | 148 | if t < 20: 149 | ysp = np.array([0.5, 0.659]) # Output setpoint 150 | else: 151 | ysp = np.array([0.51, 0.659]) # Output setpoint 152 | 153 | return [ysp, usp, xsp] 154 | 155 | # 4.2) Bounds constraints 156 | ## Input bounds 157 | umin = np.array([295, 0.00]) 158 | umax = np.array([305, 0.25]) 159 | 160 | ## State bounds 161 | xmin = np.array([0.0, 300, 0.45, -1.0]) 162 | xmax = np.array([1.0, 375, 0.75, 1.0]) 163 | 164 | ## Output bounds 165 | ymin = np.array([0.0, 0.0]) 166 | ymax = np.array([1.0, 1.0]) 167 | 168 | # 4.3) Steady-state optimization : objective function 169 | Qss = np.array([[1.0, 0.0], [0.0, 1.0]]) #Output matrix 170 | Rss = np.array([[0.0, 0.0], [0.0, 0.0]]) # Control matrix 171 | 172 | # 4.4) Dynamic optimization : objective function 173 | Q = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, .1]]) 174 | S = 0.10*np.eye(nu) # DeltaU matrix -------------------------------------------------------------------------------- /Ex_NMPC.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from __future__ import division 8 | from past.utils import old_div 9 | from casadi import * 10 | from casadi.tools import * 11 | from matplotlib import pylab as plt 12 | import math 13 | import scipy.linalg as scla 14 | import numpy as np 15 | from Utilities import* 16 | 17 | ### 1) Simulation Fundamentals 18 | 19 | # 1.1) Simulation discretization parameters 20 | Nsim = 201 # Simulation length 21 | 22 | N = 50 # Horizon 23 | 24 | h = 0.2 # Time step 25 | 26 | # 3.1.2) Symbolic variables 27 | xp = SX.sym("xp", 3) # process state vector 28 | x = SX.sym("x", 3) # model state vector 29 | u = SX.sym("u", 2) # control vector 30 | y = SX.sym("y", 2) # measured output vector 31 | d = SX.sym("d", 2) # disturbance 32 | 33 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 34 | ### 2) Process and Model construction 35 | 36 | # 2.1) Process Parameters 37 | 38 | 39 | # State map 40 | def User_fxp_Cont(x,t,u,pxp,pxmp): 41 | """ 42 | SUMMARY: 43 | It constructs the function fx_p for the non-linear case 44 | 45 | SYNTAX: 46 | assignment = User_fxp_Cont(x,t,u) 47 | 48 | ARGUMENTS: 49 | + x - State variable 50 | + t - Current time 51 | + u - Input variable 52 | 53 | OUTPUTS: 54 | + fx_p - Non-linear plant function 55 | """ 56 | 57 | F0 = if_else(t <= 5, 0.1, if_else(t<= 15, 0.15, if_else(t<= 25, 0.08, 0.1))) 58 | T0 = 350 # K 59 | c0 = 1.0 # kmol/m^3 60 | r = 0.219 # m 61 | k0 = 7.2e10 # min^-1 62 | EoR = 8750 # K 63 | U0 = 915.6*60/1000 # kJ/min*m^2*K 64 | rho = 1000.0 # kg/m^3 65 | Cp2 = 0.239 # kJ/kg 66 | DH = -5.0e4 # kJ/kmol 67 | Ar = math.pi*(r**2) 68 | kT0 = k0*exp(old_div(-EoR,T0)) 69 | 70 | fx_p = vertcat\ 71 | (\ 72 | F0*(c0 - x[0])/(Ar *x[2]) - kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0], \ 73 | F0*(T0 - x[1])/(Ar *x[2]) -DH/(rho*Cp2)*kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0] + \ 74 | 2*U0/(r*rho*Cp2)*(u[0] - x[1]), \ 75 | old_div((F0 - u[1]),Ar)\ 76 | ) 77 | 78 | return fx_p 79 | 80 | Mx = 10 # Number of elements in each time step 81 | 82 | # Output map 83 | def User_fyp(x,u,t,pyp,pymp): 84 | """ 85 | SUMMARY: 86 | It constructs the function User_fyp for the non-linear case 87 | 88 | SYNTAX: 89 | assignment = User_fyp(x,t) 90 | 91 | ARGUMENTS: 92 | + x - State variable 93 | + t - Variable that indicate the current iteration 94 | 95 | OUTPUTS: 96 | + fy_p - Non-linear plant function 97 | """ 98 | 99 | fy_p = vertcat\ 100 | (\ 101 | x[0],\ 102 | x[2] \ 103 | ) 104 | 105 | return fy_p 106 | 107 | # White Noise 108 | R_wn = 1e-7*np.array([[1.0, 0.0], [0.0, 1.0]]) # Output white noise covariance matrix 109 | 110 | 111 | # 2.2) Model Parameters 112 | 113 | # State Map 114 | def User_fxm_Cont(x,u,d,t,px): 115 | """ 116 | SUMMARY: 117 | It constructs the function fx_model for the non-linear case 118 | 119 | SYNTAX: 120 | assignment = User_fxm_Cont(x,u,d,t) 121 | 122 | ARGUMENTS: 123 | + x,u,d - State, input and disturbance variable 124 | + t - Variable that indicate the real time 125 | 126 | OUTPUTS: 127 | + x_model - Non-linear model function 128 | """ 129 | F0 = d[1] 130 | T0 = 350 # K 131 | c0 = 1.0 # kmol/m^3 132 | r = 0.219 # m 133 | k0 = 7.2e10 # min^-1 134 | EoR = 8750 # K 135 | U0 = 915.6*60/1000 # kJ/min*m^2*K 136 | rho = 1000.0 # kg/m^3 137 | Cp2 = 0.239 # kJ/kg 138 | DH = -5.0e4 # kJ/kmol 139 | pi = math.pi 140 | kT0 = k0*exp(old_div(-EoR,T0)) 141 | 142 | x_model = vertcat\ 143 | (\ 144 | F0*(c0 - x[0])/(pi* r**2 *x[2]) - kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0], \ 145 | F0*(T0 - x[1])/(pi* (r**2) *x[2]) -DH/(rho*Cp2)*kT0*exp(-EoR*(old_div(1.0,x[1])-old_div(1.0,T0)))*x[0] + \ 146 | 2*U0/(r*rho*Cp2)*(u[0] - x[1]), \ 147 | old_div((F0 - u[1]),(pi*r**2))\ 148 | ) 149 | 150 | return x_model 151 | 152 | # Output Map 153 | def User_fym(x,u,d,t,py): 154 | """ 155 | SUMMARY: 156 | It constructs the function fy_m for the non-linear case 157 | 158 | SYNTAX: 159 | assignment = User_fym(x,u,d,t) 160 | 161 | ARGUMENTS: 162 | + x,d - State and disturbance variable 163 | + t - Variable that indicate the current iteration 164 | 165 | OUTPUTS: 166 | + fy_p - Non-linear plant function 167 | """ 168 | 169 | fy_model = vertcat\ 170 | (\ 171 | x[0],\ 172 | x[2]\ 173 | ) 174 | 175 | return fy_model 176 | 177 | Mx = 10 # Number of elements in each time step 178 | 179 | # 2.3) Disturbance model for Offset-free control 180 | offree = "nl" 181 | 182 | # 2.4) Initial condition 183 | x0_p = np.array([0.874317, 325, 0.6528]) # plant 184 | x0_m = np.array([0.874317, 325, 0.6528]) # model 185 | u0 = np.array([300.157, 0.1]) 186 | dhat0 = np.array([0, 0.1]) 187 | 188 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 189 | ### 3) State Estimation 190 | 191 | # Extended Kalman filter tuning params 192 | ekf = True # Set True if you want the Extended Kalman filter 193 | Qx_kf = 1.0e-5*np.eye(x.size1()) 194 | Qd_kf = np.eye(d.size1()) 195 | Q_kf = scla.block_diag(Qx_kf, Qd_kf) 196 | R_kf = 1.0e-4*np.eye(y.size1()) 197 | P0 = np.ones((x.size1()+d.size1(),x.size1()+d.size1())) 198 | 199 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 200 | ### 4) Steady-state and dynamic optimizers 201 | 202 | # 4.1) Setpoints 203 | def defSP(t): 204 | """ 205 | SUMMARY: 206 | It constructs the setpoints vectors for the steady-state optimisation 207 | 208 | SYNTAX: 209 | assignment = defSP(t) 210 | 211 | ARGUMENTS: 212 | + t - Variable that indicates the current time 213 | 214 | OUTPUTS: 215 | + ysp, usp, xsp - Input, output and state setpoint values 216 | """ 217 | xsp = np.array([0.0, 0.0, 0.0]) # State setpoints 218 | ysp = np.array([0.874317, 0.6528]) # Output setpoint 219 | usp = np.array([300.157, 0.1]) # Control setpoints 220 | 221 | return [ysp, usp, xsp] 222 | 223 | # 4.2) Bounds constraints 224 | ## Input bounds 225 | umin = np.array([295, 0.00]) 226 | umax = np.array([305, 0.25]) 227 | 228 | ## State bounds 229 | xmin = np.array([0.0, 315, 0.50]) 230 | xmax = np.array([1.0, 375, 0.75]) 231 | 232 | ## Output bounds 233 | ymin = np.array([0.0, 0.5]) 234 | ymax = np.array([1.0, 1.0]) 235 | 236 | ## Disturbance bounds 237 | dmin = -100*np.ones((d.size1(),1)) 238 | dmax = 100*np.ones((d.size1(),1)) 239 | 240 | # 4.3) Steady-state optimization : objective function 241 | Qss = np.array([[10.0, 0.0], [0.0, 1.0]]) #Output matrix 242 | Rss = np.array([[0.0, 0.0], [0.0, 0.0]]) # Control matrix 243 | 244 | # 4.4) Dynamic optimization : objective function 245 | Q = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) 246 | R = np.array([[0.1, 0.0], [0.0, 0.1]]) 247 | 248 | slacks = False 249 | 250 | Ws = np.eye(4) -------------------------------------------------------------------------------- /Ex_NMPC_dis.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Tue Jan 12 12:11:54 2016 4 | 5 | @author: marcovaccari 6 | """ 7 | from __future__ import division 8 | from builtins import range 9 | from past.utils import old_div 10 | from casadi import * 11 | from casadi.tools import * 12 | from matplotlib import pylab as plt 13 | import math 14 | import scipy.linalg as scla 15 | import numpy as np 16 | from Utilities import* 17 | 18 | ### 1) Simulation Fundamentals 19 | 20 | # 1.1) Simulation discretization parameters 21 | Nsim = 1000 # Simulation length 22 | 23 | N = 50 # Horizon 24 | 25 | h = 5.0 # Time step 26 | 27 | # 3.1.2) Symbolic variables 28 | xp = SX.sym("xp", 6) # process state vector # 6 --> 4 tanks + 2 valves output 29 | x = SX.sym("x", 6) # model state vector # 6 --> 4 tanks + 2 valves output 30 | u = SX.sym("u", 2) # control vector # 2 --> 2 valves input 31 | y = SX.sym("y", 2) # measured output vector # 2 --> 2 lower tanks 32 | d = SX.sym("d", 2) # disturbance # 2 --> 2 lower tanks 33 | 34 | 35 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 36 | ### 2) Process and Model construction 37 | # 2.1) Process Parameters 38 | 39 | # Time CONTINUOUS dynamics of 4 tanks 40 | def Fdyn_p(x,u): 41 | # TANKS parameters 42 | 43 | # g acceleration of gravity [cm/s^2] 44 | g = 981.0 45 | 46 | # ai cross-section of the outlet hole [cm^2] 47 | a1 = 0.071 48 | a2 = 0.057 49 | a3 = 0.071 50 | a4 = 0.057 51 | 52 | # Ai cross-section of Tank [cm^2] 53 | A1 = 28.0 54 | A2 = 32.0 55 | A3 = 28.0 56 | A4 = 32.0 57 | 58 | # gmi flow splitting factor in (0,1) 59 | gm1 = 0.7 60 | gm2 = 0.6 61 | 62 | # max tank level [cm] 63 | h1_max = 20.0 64 | h2_max = 20.0 65 | 66 | # max flowrate [cm^3/s] 67 | q1_max = (a1+a4)*(2.0*g*h1_max)**0.5 68 | q2_max = (a2+a3)*(2.0*g*h2_max)**0.5 69 | K1 = old_div(q1_max,100.0) 70 | K2 = old_div(q2_max,100.0) 71 | 72 | fx = SX(4,1) 73 | 74 | # To avoid numerical instability 75 | for i in range(x.shape[0]): 76 | x[i] = if_else(x[i]<0, 0., x[i]) 77 | x[i] = if_else(x[i]>20, 20., x[i]) 78 | 79 | # TC system of equations: 80 | ## tank #1 x[2]:fx_p[2] (left lower) 81 | fx[0] = -(old_div(a1,A1))*(2.0*g*x[0])**0.5 + (old_div(a3,A1))*(2.0*g*x[2])**0.5 + (old_div(gm1,A1))*K1*u[0] 82 | 83 | ## tank #2 x[3]:fx_p[3] (right lower) 84 | fx[1] = -(old_div(a2,A2))*(2.0*g*x[1])**0.5 + (old_div(a4,A2))*(2.0*g*x[3])**0.5 + (old_div(gm2,A2))*K2*u[1] 85 | 86 | ## tank #3 x[4]:fx_p[4] (left upper) 87 | fx[2] = -(old_div(a3,A3))*(2.0*g*x[2])**0.5 + (old_div((1.0 - gm2),A3))*K2*u[1] 88 | 89 | ## tank #4 x[5]:fx_p[5] (right upper) 90 | fx[3] = -(old_div(a4,A4))*(2.0*g*x[3])**0.5 + (old_div((1.0 - gm1),A4))*K1*u[0] 91 | 92 | return fx 93 | 94 | # State map 95 | def User_fxp_Dis(x,t,u,pxp,pxmp): 96 | """ 97 | SUMMARY: 98 | It constructs the function User_fxp_Dis for the non-linear case 99 | 100 | SYNTAX: 101 | assignment = User_fxp_Cont(xp,t,u) 102 | 103 | ARGUMENTS: 104 | + xp,u - Process State and input variable 105 | + t - Variable that indicate the current iteration 106 | 107 | OUTPUTS: 108 | + fx_p - Non-linear plant function 109 | """ 110 | ## Corresponding time DISCRETE dynamics of 4 tanks (integration by hand) 111 | 112 | # initialize variables 113 | fx_p = SX(x.size1(),1) 114 | 115 | # Explicit Runge-Kutta 4 (TC dynamics integrateed by hand) 116 | Mx = 5 # Number of elements in each time step 117 | dt = old_div(h,Mx) 118 | x0 = x[2:6] 119 | fx_p[0:2] = u 120 | for i in range(Mx): 121 | k1 = Fdyn_p(x0, u) 122 | k2 = Fdyn_p(x0 + dt/2.0*k1, u) 123 | k3 = Fdyn_p(x0 + dt/2.0*k2, u) 124 | k4 = Fdyn_p(x0 + dt*k3, u) 125 | x0 = x0 + (old_div(dt,6.0))*(k1 + 2.0*k2 + 2.0*k3 + k4) 126 | fx_p[2:6] = x0 127 | 128 | return fx_p 129 | 130 | # Output Map 131 | def User_fyp(x,u,t,pyp,pymp): 132 | """ 133 | SUMMARY: 134 | It constructs the function User_fyp for the non-linear case 135 | 136 | SYNTAX: 137 | assignment = User_fyp(x,t) 138 | 139 | ARGUMENTS: 140 | + x - State variable 141 | + t - Variable that indicate the current iteration 142 | 143 | OUTPUTS: 144 | + fy_p - Non-linear plant function 145 | """ 146 | # (output equation) 147 | fy_p = vertcat\ 148 | (\ 149 | x[2],\ 150 | x[3] \ 151 | ) 152 | 153 | return fy_p 154 | 155 | # Additive State Disturbances 156 | def def_pxp(t): 157 | """ 158 | SUMMARY: 159 | It constructs the additive disturbances for the linear case 160 | 161 | SYNTAX: 162 | assignment = defdp(k) 163 | 164 | ARGUMENTS: 165 | + t - Variable that indicate the current time 166 | 167 | OUTPUTS: 168 | + dxp - State disturbance value 169 | """ 170 | 171 | if t <= 2250: 172 | dxp = np.array([0., 0., 0.5, 0., 0., 0.]) # State disturbance 173 | elif t <= 4000: 174 | dxp = np.array([0., 0., 0., 0.5, 0., 0.]) # State disturbance 175 | else: 176 | dxp = np.array([0., 0., 0., 0., 0., 0.]) # State disturbance 177 | 178 | return [dxp] 179 | 180 | 181 | 182 | # 2.2) Model Parameters 183 | 184 | ## Time CONTINUOUS dynamics of 4 tanks 185 | def Fdyn_m(x,u): 186 | # TANKS parameters 187 | 188 | # g acceleration of gravity [cm/s^2] 189 | g = 981.0 190 | 191 | # ai cross-section of the outlet hole [cm^2] 192 | a1 = 0.071 193 | a2 = 0.057 194 | a3 = 0.071 195 | a4 = 0.057 196 | 197 | # Ai cross-section of Tank [cm^2] 198 | A1 = 28.0 199 | A2 = 32.0 200 | A3 = 28.0 201 | A4 = 32.0 202 | 203 | # gmi flow splitting factor in (0,1) 204 | gm1 = 0.7 205 | gm2 = 0.6 206 | 207 | # max tank level [cm] 208 | h1_max = 20.0 209 | h2_max = 20.0 210 | 211 | # max flowrate [cm^3/s] 212 | q1_max = (a1+a4)*(2.0*g*h1_max)**0.5 213 | q2_max = (a2+a3)*(2.0*g*h2_max)**0.5 214 | K1 = old_div(q1_max,100.0) 215 | K2 = old_div(q2_max,100.0) 216 | 217 | fx = SX(4,1) 218 | 219 | # To avoid numerical instability 220 | for i in range(x.shape[0]): 221 | x[i] = if_else(x[i]<0, 0., x[i]) 222 | x[i] = if_else(x[i]>20, 20., x[i]) 223 | 224 | # TC system of equations: 225 | ## tank #1 x[2]:fx_p[2] (left lower) 226 | fx[0] = -(old_div(a1,A1))*(2.0*g*x[0])**0.5 + (old_div(a3,A1))*(2.0*g*x[2])**0.5 + (old_div(gm1,A1))*K1*u[0] 227 | 228 | ## tank #2 x[3]:fx_p[3] (right lower) 229 | fx[1] = -(old_div(a2,A2))*(2.0*g*x[1])**0.5 + (old_div(a4,A2))*(2.0*g*x[3])**0.5 + (old_div(gm2,A2))*K2*u[1] 230 | 231 | ## tank #3 x[4]:fx_p[4] (left upper) 232 | fx[2] = -(old_div(a3,A3))*(2.0*g*x[2])**0.5 + (old_div((1.0 - gm2),A3))*K2*u[1] 233 | 234 | ## tank #4 x[5]:fx_p[5] (right upper) 235 | fx[3] = -(old_div(a4,A4))*(2.0*g*x[3])**0.5 + (old_div((1.0 - gm1),A4))*K1*u[0] 236 | 237 | return fx 238 | 239 | 240 | # State Map 241 | def User_fxm_Dis(x,u,d,t,px): 242 | """ 243 | SUMMARY: 244 | It constructs the function User_fxm_Dis for the non-linear case 245 | 246 | SYNTAX: 247 | assignment = User_fxm_Dis(x,u,d,t) 248 | 249 | ARGUMENTS: 250 | + x,u,d - State, input and disturbance variable 251 | + t - Variable that indicate the real time 252 | 253 | OUTPUTS: 254 | + fx_model - Non-linear MODEL plant function 255 | """ 256 | ## Corresponding time DISCRETE dynamics of 4 tanks (integration by hand) 257 | 258 | # initialize variables 259 | fx_model = SX(x.size1(),1) 260 | 261 | # Explicit Runge-Kutta 4 (TC dynamics integrateed by hand) 262 | Mx = 5 # Number of elements in each time step 263 | dt = old_div(h,Mx) 264 | x0 = x[2:6] 265 | fx_model[0:2] = u 266 | for i in range(Mx): 267 | k1 = Fdyn_m(x0, u) 268 | k2 = Fdyn_m(x0 + dt/2.0*k1, u) 269 | k3 = Fdyn_m(x0 + dt/2.0*k2, u) 270 | k4 = Fdyn_m(x0 + dt*k3, u) 271 | x0 = x0 + (old_div(dt,6.0))*(k1 + 2.0*k2 + 2.0*k3 + k4) 272 | fx_model[2:6] = x0 273 | 274 | return fx_model 275 | 276 | # Output Map 277 | def User_fym(x,u,d,t,px): 278 | 279 | """ 280 | SUMMARY: 281 | It constructs the function User_fym for the non-linear case 282 | 283 | SYNTAX: 284 | assignment = User_fym(x,t) 285 | 286 | ARGUMENTS: 287 | + x - State variable 288 | + t - Variable that indicate the current iteration 289 | 290 | OUTPUTS: 291 | + fy_model - Non-linear MODEL plant function 292 | 293 | """ 294 | 295 | # (output equation) 296 | # --> PV (h1, h2) 297 | fy_model = vertcat\ 298 | (\ 299 | x[2],\ 300 | x[3]\ 301 | ) 302 | 303 | return fy_model 304 | 305 | # 2.3) Disturbance model for Offset-free control 306 | offree = "lin" 307 | Bd = np.zeros((x.size1(),d.size1())) 308 | Cd = np.eye(d.size1()) 309 | 310 | # 2.4) Initial condition 311 | x0_p = np.array([39.5794, 38.1492, 11.9996, 12.1883, 1.51364, 1.42194]) # [plant] 312 | x0_m = np.array([39.5794, 38.1492, 11.9996, 12.1883, 1.51364, 1.42194]) # [model] 313 | u0 = np.array([39.5794, 38.1492]) 314 | 315 | 316 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 317 | ### 3) State Estimation 318 | 319 | ## Luemberger observer tuning params 320 | lue = True # Set True if you want the Luemberger observer 321 | nx = x.size1() 322 | ny = y.size1() 323 | nd = d.size1() 324 | Kx = np.zeros((nx,ny)) 325 | Kd = np.eye(nd) 326 | K = np.row_stack([Kx,Kd]) 327 | 328 | # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # # 329 | ### 4) Steady-state and dynamic optimizers 330 | 331 | # 4.1) Setpoints 332 | def defSP(t): 333 | """ 334 | SUMMARY: 335 | It constructs the setpoints vectors for the steady-state optimisation 336 | 337 | SYNTAX: 338 | assignment = defSP(t) 339 | 340 | ARGUMENTS: 341 | + t - Variable that indicates the current time [min] 342 | 343 | OUTPUTS: 344 | + ysp, usp - Input and output setpoint values 345 | """ 346 | if t<= 50: 347 | ysp = np.array([11.9996, 12.1883]) # Output setpoint 348 | usp = np.array([39.5185, 38.1743]) # Input setpoints 349 | xsp = np.array([50.0, 50.0, 10.0, 10.0, 2.0, 2.0]) # State setpoints 350 | elif t>50 and t<=1000: 351 | ysp = np.array([11.9996, 6.0]) # Output setpoint 352 | usp = np.array([39.5185, 38.1743]) # Control setpoints 353 | xsp = np.array([60.0, 50.0, 12.0, 8.0, 2.0, 2.0]) # State setpoints 354 | elif t>1000 and t<=2000: 355 | ysp = np.array([6.0, 6.0]) # Output setpoint 356 | usp = np.array([39.5185, 38.1743]) # Control setpoints 357 | xsp = np.array([60.0, 40.0, 12.0, 8.0, 2.0, 2.0]) # State setpoints 358 | elif t>2000 and t<=3000: 359 | ysp = np.array([12.0, 12.0]) # Output setpoint 360 | usp = np.array([39.5185, 38.1743]) # Control setpoints 361 | xsp = np.array([40.0, 40.0, 8.0, 8.0, 2.0, 2.0]) # State setpoints 362 | elif t>3000 and t<=4000: 363 | ysp = np.array([8.0, 12.0]) # Output setpoint 364 | usp = np.array([39.5185, 38.1743]) # Control setpoints 365 | xsp = np.array([40.0, 60.0, 8.0, 12.0, 2.0, 2.0]) # State setpoints 366 | elif t>4000 and t<=5000: 367 | ysp = np.array([10.0, 10.0]) # Output setpoint 368 | usp = np.array([39.5185, 38.1743]) # Control setpoints 369 | xsp = np.array([50.0, 50.0, 10.0, 10.0, 2.0, 2.0]) # State setpoints 370 | else: 371 | ysp = np.array([8.0, 12.0]) # Output setpoint 372 | usp = np.array([39.5185, 38.1743]) # Control setpoints 373 | xsp = np.array([40.0, 40.0, 8.0, 12.0, 2.0, 2.0]) # State setpoints 374 | 375 | return [ysp, usp, xsp] 376 | 377 | # 4.2) Bounds constraints 378 | ## Input bounds 379 | umin = np.array([0.0, 0.0]) 380 | umax = np.array([100.0, 100.0]) 381 | 382 | ## State bounds 383 | xmin = np.zeros((x.size1(),1)) 384 | xmax = np.array([100.0, 100.0, 20.0, 20.0, 20.0, 20.0]) 385 | 386 | ## Output bounds 387 | ymin = np.array([0.0, 0.0]) 388 | ymax = np.array([20.0, 20.0]) 389 | 390 | ## Input rate-of-change bounds 391 | Dumin = np.array([-50.0, -50.0]) 392 | Dumax = np.array([50.0, 50.0]) 393 | 394 | # 4.3) Steady-state optimization : objective function 395 | Qss = np.array([[1.0, 0.0], [0.0, 1.0]]) # Output matrix 396 | Sss = np.array([[0.0, 0.0], [0.0, 0.0]]) # Delta Control matrix 397 | 398 | 399 | # 4.4) Dynamic optimization : objective function 400 | Q = np.array([[1e3, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1e3, 0.0, 0.0, 0.0, 0.0], \ 401 | [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0, 0.0, 0.0], \ 402 | [0.0, 0.0, 0.0, 0.0, 1e-6, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1e-6]]) 403 | S = np.array([[10., 0.0], [0.0, 10.0]]) # DeltaU matrix 404 | 405 | # Terminal weight 406 | def User_vfin(x,xs): 407 | """ 408 | SUMMARY: 409 | It constructs the terminal weight for the dynamic optimization problem 410 | 411 | SYNTAX: 412 | assignment = User_vfin(x) 413 | 414 | ARGUMENTS: 415 | + x - State variables 416 | 417 | OUTPUTS: 418 | + vfin - Terminal weight 419 | """ 420 | Vn = 100.0 421 | vfin = mtimes(x.T,mtimes(Vn,x)) 422 | return vfin 423 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /MPC_code.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on December 3, 2015 4 | 5 | @author: Marco, Mirco, Gabriele 6 | 7 | MPC code main file 8 | """ 9 | from builtins import range 10 | from casadi import * 11 | from casadi.tools import * 12 | from matplotlib import pylab as plt 13 | import math 14 | import scipy.linalg as scla 15 | import scipy.optimize as scopt 16 | from scipy.integrate import odeint 17 | import numpy as np 18 | import time 19 | from Utilities import* 20 | from Target_Calc import * 21 | from Estimator import * 22 | from Control_Calc import * 23 | from Default_Values import * 24 | 25 | ex_name = __import__('Ex_LMPC_WB') # Insert here your file name 26 | import sys 27 | sys.modules['ex_name'] = ex_name 28 | from ex_name import * #Loading example 29 | 30 | ########## Dimensions ############################# 31 | nx = x.size1() # model state size # 32 | nxp = xp.size1()# process state size # 33 | nu = u.size1() # control size # 34 | ny = y.size1() # measured output/disturbance size# 35 | nd = d.size1() # disturbance size # 36 | if LinPar == False: 37 | npx = px.size1() # model state parameters size 38 | npy = py.size1() # model output parameters size 39 | if Fp_nominal == True: 40 | npxp = npx # process state parameters size 41 | npyp = npy # process output parameters size 42 | else: 43 | npxp = pxp.size1() # process state parameters size 44 | npyp = pyp.size1() # process output parameters size 45 | else: 46 | npx = nx 47 | npxp = nxp 48 | npy = npyp = ny # 49 | nxu = nx + nu # state+control # 50 | nxuy = nx + nu + ny # state+control # 51 | nxuk = nx + nu + 2*nx # state + control + internal states 52 | nw = nx*(N+1) + nu*N # total of variables # 53 | nw_c = nx*(N+1) + nu*N + 2*nx*N # total of variables for collocation methods 54 | 55 | if 'Ws' in locals() and slacks == True: 56 | Ws = Ws 57 | ns = Ws.shape[0] 58 | else: 59 | Ws = [] 60 | ns = 0 61 | 62 | ################################################### 63 | 64 | ########## Fixed symbolic variables ######################################### 65 | k = SX.sym("k", 1) # step integration # 66 | t = SX.sym("t", 1) # time # 67 | pxp = SX.sym('pxp',npxp) #process state parameters 68 | pyp = SX.sym('pyp',npyp) #process output parameters 69 | px = SX.sym('px',npx) #model state parameters 70 | py = SX.sym('py',npy) #model output parameter 71 | pxmp = SX.sym('pxmp',npxp) #process measurable state parameters 72 | pymp = SX.sym('pymp',npyp) #process measurable output parameters 73 | xs = SX.sym('xs',nx) # stationary state value # 74 | xps = SX.sym('xps',nx) # process stationary state value # 75 | us = SX.sym('us',nu) # stationary input value # 76 | ys = SX.sym('ys',ny) # stationary output value # 77 | xsp = SX.sym('xsp',nx) # state setpoint value # 78 | usp = SX.sym('usp',nu) # input setpoint value # 79 | ysp = SX.sym('ysp',ny) # output setpoint value # 80 | lambdaT = SX.sym('lambdaTSX',(ny,nu)) # modifier # 81 | s_Coll = SX.sym('s_Coll',2*nx) #internal states for collocation method 82 | ############################################################################# 83 | 84 | if ssjacid is True: 85 | from SS_JAC_ID import * 86 | [A, B, C, D, xlin, ulin, ylin] = ss_p_jac_id(ex_name, nx, nu, ny, nd, npx, npy, k, t, px, py, LinPar) 87 | # Build linearized model. 88 | if offree == "lin": 89 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, C = C, xlin = xlin, ulin = ulin, ylin = ylin) 90 | else: 91 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, C = C, xlin = xlin, ulin = ulin, ylin = ylin) 92 | else: 93 | #### Model calculation ##################################################### 94 | if 'User_fxm_Cont' in locals(): 95 | if StateFeedback is True: 96 | if offree == "lin": 97 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, fx = User_fxm_Cont, Mx = Mx, SF = StateFeedback) 98 | else: 99 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, fx = User_fxm_Cont, Mx = Mx, SF = StateFeedback) 100 | else: 101 | if 'User_fym' in locals(): 102 | if offree == "lin": 103 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, fx = User_fxm_Cont, Mx = Mx, fy = User_fym) 104 | else: 105 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, fx = User_fxm_Cont, Mx = Mx, fy = User_fym) 106 | else: 107 | if offree == "lin": 108 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, fx = User_fxm_Cont, Mx = Mx, C = C) 109 | else: 110 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, fx = User_fxm_Cont, Mx = Mx, C = C) 111 | elif 'User_fxm_Dis' in locals(): 112 | if StateFeedback is True: 113 | if offree == "lin": 114 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, Fx = User_fxm_Dis, SF = StateFeedback) 115 | else: 116 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Fx = User_fxm_Dis, SF = StateFeedback) 117 | else: 118 | if 'User_fym' in locals(): 119 | if offree == "lin": 120 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, Fx = User_fxm_Dis, fy = User_fym) 121 | else: 122 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Fx = User_fxm_Dis, fy = User_fym) 123 | else: 124 | if offree == "lin": 125 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, Fx = User_fxm_Dis, C = C) 126 | else: 127 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Fx = User_fxm_Dis, C = C) 128 | elif 'A' in locals(): 129 | if StateFeedback is True: 130 | if offree == "lin": 131 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, SF = StateFeedback) 132 | else: 133 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, SF = StateFeedback) 134 | else: 135 | if 'User_fym' in locals(): 136 | if 'xlin' in locals(): 137 | if offree == "lin": 138 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, fy = User_fym, xlin = xlin, ulin = ulin) 139 | else: 140 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, fy = User_fym, xlin = xlin, ulin = ulin) 141 | else: 142 | if offree == "lin": 143 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, fy = User_fym) 144 | else: 145 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, fy = User_fym) 146 | else: 147 | if 'ylin' in locals(): 148 | if 'xlin' in locals(): 149 | if offree == "lin": 150 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, C = C, xlin = xlin, ulin = ulin, ylin = ylin) 151 | else: 152 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, C = C, xlin = xlin, ulin = ulin, ylin = ylin) 153 | else: 154 | if offree == "lin": 155 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, C = C, ylin = ylin) 156 | else: 157 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, C = C, ylin = ylin) 158 | elif 'xlin' in locals(): 159 | if offree == "lin": 160 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, C = C, xlin = xlin, ulin = ulin) 161 | else: 162 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, C = C, xlin = xlin, ulin = ulin) 163 | else: 164 | if offree == "lin": 165 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, Bd = Bd, Cd = Cd, A = A, B = B, C = C) 166 | else: 167 | [Fx_model,Fy_model] = defF_model(x,u,y,d,k,t,px,py,offree,LinPar, A = A, B = B, C = C) 168 | 169 | ############################################################################# 170 | 171 | #### Plant equation ######################################################## 172 | if Fp_nominal is True: 173 | Fx_p = Fx_model 174 | Fy_p = Fy_model 175 | else: 176 | if 'Ap' in locals(): 177 | if StateFeedback is True: # x = Ax + Bu ; y = x 178 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, Ap = Ap, Bp = Bp, SF = StateFeedback) 179 | elif 'User_fyp' in locals(): # x = Ax + Bu ; y = h(x,t) 180 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, Ap = Ap, Bp = Bp, fyp = User_fyp) 181 | else: 182 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, Ap = Ap, Bp = Bp, Cp = Cp) 183 | elif 'User_fxp_Dis' in locals(): 184 | if StateFeedback is True: # x = F(x,t) ; y = x 185 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, Fx = User_fxp_Dis, SF = StateFeedback) 186 | elif 'User_fyp' in locals(): # x = F(x,t) ; y = h(x,t) 187 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, Fx = User_fxp_Dis, fy = User_fyp) 188 | elif 'Cp' in locals(): # x = F(x,t) ; y = Cx 189 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, Fx = User_fxp_Dis, Cp = Cp) 190 | elif 'User_fxp_Cont' in locals(): 191 | if StateFeedback is True: # \dot{x} = f(x,t) ; y = x 192 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, fx = User_fxp_Cont, Mx = Mx, SF = StateFeedback) 193 | elif 'User_fyp' in locals(): # \dot{x} = f(x,t) ; y = h(x,t) 194 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, fx = User_fxp_Cont, Mx = Mx, fy = User_fyp) 195 | else: # \dot{x} = f(x,t) ; y = Cx 196 | [Fx_p,Fy_p] = defF_p(xp,u,y,k,t,pxp,pyp,pxmp,pymp,LinPar, fx = User_fxp_Cont, Mx = Mx, Cp = Cp) 197 | 198 | ############################################################################# 199 | 200 | if estimating is False: 201 | #### Objective function calculation ######################################## 202 | if 'rss_y' in locals(): 203 | if 'A' in locals() and 'C' in locals(): 204 | Sol_Hess_constss = 'yes' 205 | if 'rss_u' in locals(): 206 | Fss_obj = defFss_obj(x, u, y, xsp, usp, ysp, r_y = rss_y, r_u = rss_u) 207 | else: 208 | Fss_obj = defFss_obj(x, u, y, xsp, usp, ysp, r_y = rss_y, r_Du = rss_Du) 209 | DUssForm = True 210 | elif 'Qss' in locals(): 211 | QForm_ss = True 212 | if 'A' in locals() and 'C' in locals(): 213 | Sol_Hess_constss = 'yes' 214 | if 'Rss' in locals(): 215 | Fss_obj = defFss_obj(x, u, y, xsp, usp, ysp, Q = Qss, R = Rss) 216 | else: 217 | Fss_obj = defFss_obj(x, u, y, xsp, usp, ysp, Q = Qss, S = Sss) 218 | DUssForm = True 219 | elif 'User_fssobj' in locals(): 220 | Fss_obj = defFss_obj(x, u, y, xsp, usp, ysp, f_obj = User_fssobj) 221 | 222 | if 'r_x' in locals(): 223 | QForm = True 224 | if 'A' in locals() and 'C' in locals(): 225 | Sol_Hess_constdyn = 'yes' 226 | if 'r_u' in locals(): 227 | F_obj = defF_obj(x, u, y, xs, us, ys, r_x = r_x, r_u = r_u) 228 | else: 229 | F_obj = defF_obj(x, u, y, xs, us, ys, r_x = r_x, r_Du = r_Du) 230 | DUForm = True 231 | elif 'Q' in locals(): 232 | QForm = True 233 | if 'A' in locals() and 'C' in locals(): 234 | Sol_Hess_constdyn = 'yes' 235 | if 'R' in locals(): 236 | F_obj = defF_obj(x, u, y, xs, us, ys, Q = Q, R = R) 237 | else: 238 | F_obj = defF_obj(x, u, y, xs, us, ys, Q = Q, S = S) 239 | DUForm = True 240 | elif 'User_fobj_Cont' in locals(): 241 | ContForm = True 242 | F_obj = defF_obj(x, u, y, xs, us, ys, f_Cont = User_fobj_Cont) 243 | elif 'User_fobj_Dis' in locals(): 244 | F_obj = defF_obj(x, u, y, xs, us, ys, f_Dis = User_fobj_Dis) 245 | elif 'User_fobj_Coll' in locals(): 246 | F_obj = defF_obj(x, u, y, xs, us, ys, f_Coll = User_fobj_Coll, s_Coll = s_Coll) 247 | 248 | if 'User_vfin' in locals(): 249 | Vfin = defVfin(x, xs, vfin_F = User_vfin) 250 | elif 'A' in locals(): # Linear system 251 | if 'Q' in locals(): # QP problem 252 | #Checking S and R matrix for Riccati Equation 253 | if 'S' in locals(): 254 | R = S 255 | Vfin = defVfin(x, xs, A = A, B = B, Q = Q, R = R) 256 | else: 257 | Vfin = defVfin(x, xs) 258 | 259 | ############################################################################# 260 | 261 | ### Solver options ########################################################## 262 | sol_optss = {'ipopt.max_iter':Sol_itmax, 'ipopt.hessian_constant':Sol_Hess_constss,'ipopt.print_level':0,'ipopt.sb':"yes",'print_time':0}#, 'ipopt.tol':1e-10} 263 | sol_optdyn = {'ipopt.max_iter':Sol_itmax, 'ipopt.hessian_constant':Sol_Hess_constdyn,'ipopt.print_level':0,'ipopt.sb':"yes",'print_time':0}#, 'ipopt.tol':1e-10} 264 | 265 | #### Modifiers Adaptatation gradient definition ############################ 266 | if Adaptation is True: 267 | # Defining eventual new bound contraints when nx != nxp 268 | if 'xpmin' not in locals(): xpmin = xmin 269 | if 'xpmax' not in locals(): xpmax = xmax 270 | 271 | # Defining the optimization problem to calculate the plant steady state given the input 272 | (solver_ss_mod, wssp_lb, wssp_ub, gssp_lb, gssp_ub) = opt_ssp(nxp, nu, ny, nd, npx, npy, npxp, Fx_p,Fy_p, sol_optss, xmin = xpmin, xmax = xpmax, h = h) 273 | 274 | alpha_mod = 0.2 if 'alpha_mod' not in locals() else alpha_mod 275 | 276 | # Defining modifier update filtering euqation 277 | LambdaT = defLambdaT(xp,x,u,y,d,k,t,pxp,pyp,px,py, Fx_model, Fx_p, Fy_model, Fy_p, alpha_mod) 278 | 279 | # Defining auxiliary variable and objective function when nx != nxp 280 | if nx != nxp: 281 | xp2 = SX.sym("xp2", nxp) 282 | Fss_obj2 = Function('Fss_obj2', [xp,u,y,xp2,usp,ysp], [User_fssobj(xp,u,y,xp2,usp,ysp)]) # The economic function has to be non-linear 283 | else: 284 | Fss_obj2 = Fss_obj 285 | 286 | # Defining the optimization problem to calculate the true plant optimum 287 | (solver_ss2, wssp2_lb, wssp2_ub, gssp2_lb, gssp2_ub) = opt_ssp2(nxp, nu, ny, nd, npx, npy, npxp, npyp, Fx_p,Fy_p, Fss_obj2, QForm_ss,sol_optss, umin = umin, umax = umax, w_s = None, z_s = None, ymin = ymin, ymax = ymax, xmin = xpmin, xmax = xpmax, h = h) 288 | ############################################################################# 289 | 290 | #### Solver definition ##################################################### 291 | ymin_s = ymin if ymin_ss is None else ymin_ss; ymax_s = ymax if ymax_ss is None else ymax_ss 292 | xmin_s = xmin if xmin_ss is None else xmin_ss; xmax_s = xmax if xmax_ss is None else xmax_ss 293 | umin_s = umin if umin_ss is None else umin_ss; umax_s = umax if umax_ss is None else umax_ss 294 | 295 | if 'User_g_ineq_SS' not in locals(): 296 | User_g_ineq_SS = None 297 | if 'User_h_eq_SS' not in locals(): 298 | User_h_eq_SS = None 299 | 300 | (solver_ss, wss_lb, wss_ub, gss_lb, gss_ub) = opt_ss(nx, nu, ny, nd, npx,npy, Fx_model,Fy_model, Fss_obj, QForm_ss, DUssForm, sol_optss, User_g_ineq_SS, User_h_eq_SS, umin = umin_s, umax = umax_s, w_s = None, z_s = None, ymin = ymin_s, ymax = ymax_s, xmin = xmin_s, xmax = xmax_s, h = h) 301 | 302 | ymin_d = ymin if ymin_dyn is None else ymin_dyn; ymax_d = ymax if ymax_dyn is None else ymax_dyn 303 | xmin_d = xmin if xmin_dyn is None else xmin_dyn; xmax_d = xmax if xmax_dyn is None else xmax_dyn 304 | umin_d = umin if umin_dyn is None else umin_dyn; umax_d = umax if umax_dyn is None else umax_dyn 305 | 306 | if 'User_g_ineq' not in locals(): 307 | User_g_ineq = None 308 | ng = 0 309 | elif 'User_g_ineq' in locals() and slacksG == True: 310 | g_ineq = User_g_ineq(x,u,y,d,t,px,py) 311 | G_ineqSX = Function('G_ineqSX', [x,u,y,d,t,px,py], [g_ineq]) 312 | ng = G_ineqSX(x,u,y,d,t,px,py).size1() 313 | else: 314 | ng = 0 315 | 316 | if 'User_h_eq' not in locals(): 317 | User_h_eq = None 318 | nh = 0 319 | elif 'User_h_eq' in locals() and slacksH == True: 320 | h_eq = User_h_eq(x,u,y,d,t,px,py) 321 | H_eqSX = Function('H_eqSX', [x,u,y,d,t,px,py], [h_eq]) 322 | nh = H_eqSX(x,u,y,d,t,px,py).size1() 323 | else: 324 | nh = 0 325 | 326 | if slacks == True: 327 | nw = nw + ns 328 | nw_c = nw_c + ns 329 | 330 | if 'User_fobj_Cont' in locals(): 331 | (solver, w_lb, w_ub, g_lb, g_ub) = opt_dyn(x, u, y, d, t, px, py, nx, nu, ny, nd, npx, npy, ng, nh, Fx_model,Fy_model, F_obj,Vfin, N, QForm, DUForm, DUFormEcon, ContForm, TermCons, slacks, slacksG, slacksH, nw, sol_optdyn, User_g_ineq, User_h_eq, umin = umin_d, umax = umax_d, W = None, Z = None, ymin = ymin_d, ymax = ymax_d, xmin = xmin_d, xmax = xmax_d, Dumin = Dumin, Dumax = Dumax, h = h, fx = User_fxm_Cont, xstat = xs, ustat = us, Ws = Ws) 332 | elif 'User_fobj_Coll' in locals(): 333 | (solver, w_lb, w_ub, g_lb, g_ub) = opt_dyn_CM(x, u, y, d, t, px, py, nx, nu, ny, nd, npx, npy, ng, nh, Fx_model,Fy_model, F_obj,Vfin, N, QForm, DUForm, DUFormEcon, ContForm, TermCons, slacks, slacksG, slacksH, nw, sol_optdyn, User_g_ineq, User_h_eq, umin = umin_d, umax = umax_d, W = None, Z = None, ymin = ymin_d, ymax = ymax_d, xmin = xmin_d, xmax = xmax_d, Dumin = Dumin, Dumax = Dumax, h = h, fx = User_fxm_Cont, xstat = xs, ustat = us, Ws = Ws) 334 | else: 335 | (solver, w_lb, w_ub, g_lb, g_ub) = opt_dyn(x, u, y, d, t, px, py, nx, nu, ny, nd, npx, npy, ng, nh, Fx_model,Fy_model, F_obj,Vfin, N, QForm, DUForm, DUFormEcon, ContForm, TermCons, slacks, slacksG, slacksH, nw, sol_optdyn, User_g_ineq, User_h_eq, umin = umin_d, umax = umax_d, W = None, Z = None, ymin = ymin_d, ymax = ymax_d, xmin = xmin_d, xmax = xmax_d, Dumin = Dumin, Dumax = Dumax, h = h, Ws = Ws) 336 | ############################################################################# 337 | 338 | #### Kalman steady-state gain definition ################################### 339 | if kalss is True: 340 | if 'A' in locals() and 'C' in locals(): 341 | linmod = 'full' 342 | if offree == "lin": 343 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, Bd = Bd, Cd = Cd, A = A, C = C) 344 | else: 345 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, A = A, C = C) 346 | elif 'A' in locals(): 347 | linmod = 'onlyA' 348 | if offree == "lin": 349 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, x, u, k, d, t, h, px, py, x_ss, u_ss, px_ss, py_ss, Bd = Bd, Cd = Cd, A = A, Fy = Fy_model) 350 | else: 351 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, x, u, k, d, t, h, px, py, x_ss, u_ss, px_ss, py_ss, A = A, Fy = Fy_model) 352 | elif 'C' in locals(): 353 | linmod = 'onlyC' 354 | if offree == "lin": 355 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, x, u, k, d, t, h, px, py, x_ss, u_ss, px_ss, py_ss, Bd = Bd, Cd = Cd, C = C, Fx = Fx_model) 356 | else: 357 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, x, u, k, d, t, h, px, py, x_ss, u_ss, px_ss, py_ss, C = C, Fx = Fx_model) 358 | else: 359 | linmod = 'no' 360 | if offree == "lin": 361 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, x, u, k, d, t, h, px, py, x_ss, u_ss, px_ss, py_ss, Bd = Bd, Cd = Cd, Fx = Fx_model, Fy = Fy_model) 362 | else: 363 | K = Kkalss(ny, nd, nx, Q_kf, R_kf, offree, linmod, x, u, k, d, t, h, px, py, x_ss, u_ss, px_ss, py_ss, Fx = Fx_model, Fy = Fy_model) 364 | 365 | ############################################################################# 366 | 367 | #### MHE optimization problem definition ################################### 368 | if mhe is True: 369 | n_w = w.size1() # state noise dimension 370 | v = SX.sym("v", ny) # output noise 371 | 372 | ## Building MHE objective function 373 | if 'r_w' in locals(): 374 | if 'A' in locals() and 'C' in locals(): 375 | Sol_Hess_constmhe = 'yes' 376 | F_obj_mhe = defF_obj_mhe(w, v, t, r_w = r_w, r_v = r_v) 377 | elif 'Q_mhe' in locals(): 378 | if 'A' in locals() and 'C' in locals(): 379 | Sol_Hess_constmhe = 'yes' 380 | F_obj_mhe = defF_obj_mhe(w, v, t, Q = Q_mhe, R = R_mhe) 381 | elif 'User_fobj_mhe' in locals(): 382 | F_obj_mhe = defF_obj_mhe(w, v, t, f_obj = User_fobj_mhe) 383 | sol_optmhe = {'ipopt.max_iter':Sol_itmax, 'ipopt.hessian_constant':Sol_Hess_constmhe, 'ipopt.tol':1e-10,'ipopt.print_level':0,'ipopt.sb':"yes",'print_time':0} 384 | 385 | 386 | ## Building state dynamics for MHE (modified with w variable) 387 | G_mhe = G_mhe if 'G_mhe' in locals() else DM.eye(nx+nd) 388 | if 'User_fx_mhe_Cont' in locals(): 389 | if offree == "lin": 390 | Fx_mhe = defFx_mhe(x,u,w,d,k,t,px,offree, LinPar, Bd = Bd, fx = User_fx_mhe_Cont, Mx = Mx, G = G_mhe) 391 | else: 392 | Fx_mhe = defFx_mhe(x,u,w,d,k,t,px,offree,LinPar, fx = User_fx_mhe_Cont, Mx = Mx, G = G_mhe) 393 | elif 'User_fx_mhe_Dis' in locals(): 394 | if offree == "lin": 395 | Fx_mhe = defFx_mhe(x,u,w,d,k,t,px,offree,LinPar, Bd = Bd, Fx = User_fx_mhe_Dis, G = G_mhe) 396 | else: 397 | Fx_mhe = defFx_mhe(x,u,w,d,k,t,px,offree,LinPar, Fx = User_fx_mhe_Dis, G = G_mhe) 398 | 399 | xmi = -DM.inf(nx) if xmin is None else xmin 400 | dmi = -DM.inf(nd) if dmin is None else dmin 401 | xmin_mhe = vertcat(xmi,dmi) 402 | xma = DM.inf(nx) if xmax is None else xmax 403 | dma = DM.inf(nd) if dmax is None else dmax 404 | xmax_mhe = vertcat(xma,dma) 405 | 406 | ## Initializing iterative variables 407 | w_k = DM.zeros(n_w,1) 408 | v_k = DM.zeros(ny,1) 409 | U_mhe = [] 410 | X_mhe = [] 411 | Xm_mhe = [] 412 | Y_mhe = [] 413 | T_mhe = [] 414 | V_mhe = [] 415 | W_mhe = [] 416 | 417 | C_mhe = [] 418 | G_mhe = [] 419 | A_mhe = [] 420 | B_mhe = [] 421 | f_mhe = [] 422 | h_mhe = [] 423 | Qk_mhe = [] 424 | Rk_mhe = [] 425 | Sk_mhe = [] 426 | Q_mhe = [] 427 | bigU_mhe = [] 428 | P_mhe = [] 429 | Pc_mhe = [] 430 | P_kal_mhe = P0 431 | Pc_kal_mhe = P0 432 | PX_mhe = [] 433 | PY_mhe = [] 434 | 435 | idx = N_mhe if N_mhe == 1 else N_mhe-1 436 | pH_mhe = DM.zeros(ny*(idx),1) 437 | pO_mhe = DM.zeros(ny*(idx)*(nx+nd),1) 438 | pPyx_mhe = DM.zeros(ny*(idx)*ny*(idx),1) 439 | 440 | 441 | ############################################################################# 442 | p_xp = DM.zeros((npxp,1)) #Dummy variable when parameter is not present 443 | p_yp = DM.zeros((npyp,1)) #Dummy variable when parameter is not present 444 | p_xk = DM.zeros((npx,N)) #Dummy variable when parameter is not present 445 | p_yk = DM.zeros((npy,N)) #Dummy variable when parameter is not present 446 | p_xmp = DM.zeros((npxp,1)) #Dummy variable when parameter is not present 447 | p_ymp = DM.zeros((npyp,1)) #Dummy variable when parameter is not present 448 | ysp_k = DM.zeros(ny,1); usp_k = DM.zeros(nu,1); xsp_k = DM.zeros(nx,1); xsp_k_p = DM.zeros(nxp,1) 449 | x_k = DM(x0_p) 450 | u_k = DM(u0) 451 | xhat_k = DM(x0_m) 452 | lambdaT_k = np.zeros((ny,nu)) 453 | cor_k = 0.00*np.ones((ny,1)) 454 | delta_k = np.zeros((nu,ny)) 455 | try: 456 | P_k = P0 457 | except NameError: 458 | P_k = DM.zeros(nx+nd, nx+nd) 459 | 460 | try: 461 | dhat_k = DM(dhat0) 462 | except NameError: 463 | dhat_k = DM.zeros(nd) 464 | 465 | Xp = [] 466 | Yp = [] 467 | U = [] 468 | XS = [] 469 | YS = [] 470 | US = [] 471 | X_HAT = [] 472 | Y_HAT = [] 473 | D_HAT = [] 474 | COR = [] 475 | TIME_SS = [] 476 | TIME_DYN = [] 477 | Upopt = []; Ypopt = [] 478 | LAMBDA = [] 479 | P_K = [] 480 | Esim = [] 481 | X_KF = [] 482 | Ysp = [] 483 | Sl = [] 484 | 485 | for ksim in range(Nsim): 486 | print('Time Iteration ' + str(ksim+1) + ' of ' + str(Nsim)) 487 | t_k = ksim*h #real time updating 488 | 489 | ## Updating the current value of parameters 490 | 491 | # Defining variable parameters along the predicition horizon 492 | if 'def_px' in locals(): 493 | for i in range (N): 494 | [p_xk[:,i]] = def_px(t_k+i) 495 | if 'def_py' in locals(): 496 | for i in range (N): 497 | [p_yk[:,i]] = def_py(t_k+i) 498 | 499 | # Taking the first vector of parameters for the current ksim 500 | p_x_k = p_xk[:,0] 501 | p_y_k = p_yk[:,0] 502 | 503 | if 'def_px' in locals() and'def_pxmp' in locals(): 504 | [p_xmp] = def_pxmp(t_k) 505 | elif 'def_px' in locals(): 506 | p_xmp = p_x_k 507 | if 'def_py' in locals() and 'def_pymp' in locals(): 508 | [p_ymp] = def_pymp(t_k) 509 | elif 'def_py' in locals(): 510 | p_ymp = p_y_k 511 | 512 | if 'def_pxp' in locals(): 513 | [p_xp] = def_pxp(t_k) 514 | if 'def_pyp' in locals(): 515 | [p_yp] = def_pyp(t_k) 516 | 517 | 518 | ## Store current state 519 | Xp.append(x_k) 520 | X_HAT.append(xhat_k) 521 | 522 | 523 | # Model output 524 | yhat_k = Fy_model(xhat_k, u_k, dhat_k, t_k, p_y_k) 525 | 526 | if (ksim==0): 527 | y_k = yhat_k 528 | y_k_prev = y_k 529 | 530 | # Actual output 531 | if Fp_nominal is True: 532 | y_k = Fy_p(x_k, u_k, dhat_k, t_k, p_y_k) 533 | else: #All the other cases 534 | y_k = Fy_p(x_k,u_k,p_yp,t_k,p_ymp) 535 | 536 | 537 | # Introducing white noise on output when present 538 | if 'R_wn' in locals(): 539 | Rv = scla.sqrtm(R_wn) 540 | v_wn_k = mtimes(Rv,DM(np.random.normal(0,1,ny))) 541 | y_k = y_k + v_wn_k 542 | 543 | Yp.append(y_k) 544 | Y_HAT.append(yhat_k) 545 | ################ Estimator calling ############################################ 546 | if offree != "no": 547 | x_es = vertcat(xhat_k,dhat_k) 548 | 549 | csi = SX.sym("csi", nx+nd) 550 | x1 = csi[0:nx] 551 | d1 = csi[nx:nx+nd] 552 | Dx = Function('Dx',[d],[d]) 553 | 554 | if mhe is False: 555 | dummyFx = vertcat(Fx_model(x1,u,k,d1,t,px), Dx(d1)) 556 | Fx_es = Function('Fx_es', [csi,u,k,t,px], [dummyFx]) 557 | else: 558 | Fx_es = Fx_mhe 559 | 560 | dummyFy = Fy_model(x1,u,d1,t,py) 561 | Fy_es = Function('Fy_es', [csi,u,t,py], [dummyFy]) 562 | 563 | else: 564 | if nd != 0.0: 565 | import sys 566 | sys.exit("The disturbance dimension is not zero but no disturbance model has been selected") 567 | x_es = xhat_k 568 | if mhe is False: 569 | dummyFx = Fx_model(x,u,k,d,t,px) 570 | Fx_es = Function('Fx_es', [x,u,k,t,px], [dummyFx]) 571 | else: 572 | dummyFx = Fx_mhe(x,u,k,t,w,px) 573 | Fx_es = Function('Fx_es', [x,u,k,t,w,px], [dummyFx]) 574 | dummyFy = Fy_model(x,u,d,t,py) 575 | Fy_es = Function('Fy_es', [x,u,t,py], [dummyFy]) 576 | 577 | if kalss is True or lue is True: 578 | estype = 'kalss' 579 | if StateFeedback is True and offree == 'no': 580 | K = DM.eye(x_es.size1()) 581 | [x_es, kwargsout] = defEstimator(Fx_es,Fy_es,y_k,u_k, estype, x_es, t_k, p_x_k, p_y_k, K = K) 582 | 583 | elif mhe is True: 584 | estype = 'mhe' 585 | 586 | if ksim == 0: 587 | xm_kal_mhe = x_es 588 | 589 | X_KF.append(DM(xm_kal_mhe)) 590 | 591 | if ksim < N_mhe: 592 | 593 | N_mhe_curr = ksim + 1 594 | 595 | ## Defining the optimization solver 596 | (solver_mhe, w_lb_mhe, w_ub_mhe, g_lb_mhe, g_ub_mhe) = mhe_opt(nx+nd, nd, nu, ny,npx,npy, n_w, F_obj_mhe, Fx_es,Fy_es, \ 597 | N_mhe_curr, N_mhe, ksim, h, mhe_up, sol_optmhe, User_g_ineq, User_h_eq, wmin = wmin, wmax = wmax, vmin = vmin, vmax = vmax, \ 598 | ymin = ymin, ymax = ymax, xmin = xmin_mhe, xmax = xmax_mhe) 599 | 600 | 601 | [x_es, kwargsout] = defEstimator(Fx_es,Fy_es,y_k,u_k, estype, x_es, t_k, p_x_k, p_y_k, P_min = P_k, Fobj = F_obj_mhe,\ 602 | ts = h, wk = w_k, vk = v_k, U = U_mhe, X = X_mhe, Xm = Xm_mhe, Y = Y_mhe, T = T_mhe, V = V_mhe, W = W_mhe, xb = x_bar,\ 603 | sol = solver_mhe, solwlb = w_lb_mhe, solwub = w_ub_mhe, solglb = g_lb_mhe, solgub = g_ub_mhe,\ 604 | N = N_mhe_curr, up = mhe_up, Nmhe = N_mhe, C = C_mhe, G = G_mhe, A = A_mhe, B = B_mhe, f = f_mhe,h = h_mhe, Qk = Qk_mhe, Rk = Rk_mhe, Sk = Sk_mhe,\ 605 | Q = Q_mhe, bU = bigU_mhe, P = P_mhe, Pc = Pc_mhe, P_kal = P_kal_mhe, P_c_kal = Pc_kal_mhe, pH = pH_mhe, pO = pO_mhe, pPyx = pPyx_mhe, xm_kal = xm_kal_mhe, \ 606 | PX = PX_mhe, PY = PY_mhe, nd = nd) 607 | P_k = kwargsout['P_plus'] 608 | U_mhe = kwargsout['U_mhe'] 609 | X_mhe = kwargsout['X_mhe'] 610 | Xm_mhe = kwargsout['Xm_mhe'] 611 | Y_mhe = kwargsout['Y_mhe'] 612 | T_mhe = kwargsout['T_mhe'] 613 | V_mhe = kwargsout['V_mhe'] 614 | W_mhe = kwargsout['W_mhe'] 615 | w_k = kwargsout['wk'] 616 | v_k = kwargsout['vk'] 617 | x_bar = kwargsout['xb'] 618 | C_mhe = kwargsout['C_mhe'] 619 | G_mhe = kwargsout['G_mhe'] 620 | A_mhe = kwargsout['A_mhe'] 621 | B_mhe = kwargsout['B_mhe'] 622 | f_mhe = kwargsout['f_mhe'] 623 | h_mhe = kwargsout['h_mhe'] 624 | Qk_mhe = kwargsout['Qk_mhe'] 625 | Rk_mhe = kwargsout['Rk_mhe'] 626 | Sk_mhe = kwargsout['Sk_mhe'] 627 | Q_mhe = kwargsout['Q_mhe'] 628 | bigU_mhe = kwargsout['bigU_mhe'] 629 | P_mhe = kwargsout['P_mhe'] 630 | Pc_mhe = kwargsout['Pc_mhe'] 631 | P_kal_mhe = kwargsout['P_kal_mhe'] 632 | Pc_kal_mhe = kwargsout['P_c_kal_mhe'] 633 | pH_mhe = kwargsout['pH_mhe'] 634 | pO_mhe = kwargsout['pO_mhe'] 635 | pPyx_mhe = kwargsout['pPyx_mhe'] 636 | xm_kal_mhe = kwargsout['xm_kal_mhe'] 637 | xc_kal_mhe = kwargsout['xc_kal_mhe'] 638 | PX_mhe = kwargsout['PX_mhe'] 639 | PY_mhe = kwargsout['PY_mhe'] 640 | 641 | P_K.append(P_k) 642 | else: 643 | if kal is True: # only for linear system 644 | if 'A' not in locals(): 645 | import sys 646 | sys.exit("You cannot use the kalman filter if the model you have chosen is not linear") 647 | estype = 'kal' 648 | elif ekf is True: 649 | estype = 'ekf' 650 | [x_es, kwargsout] = defEstimator(Fx_es,Fy_es,y_k,u_k, estype, x_es, t_k, p_x_k, p_y_k, P_min = P_k, Q = Q_kf, R = R_kf, ts = h) 651 | P_k = kwargsout['P_plus'] 652 | 653 | 654 | # Extracting x(k|k) and d(k|k) 655 | if offree != "no": 656 | xhat_k = x_es[0:nx] 657 | dhat_k = x_es[nx:nx+nd] 658 | 659 | # dhat_k saturation 660 | if dmin is not None: 661 | for k_d in range(nd): 662 | if dhat_k[k_d] < dmin[k_d]: 663 | dhat_k[k_d] = dmin[k_d] 664 | elif dhat_k[k_d] > dmax[k_d]: 665 | dhat_k[k_d] = dmax[k_d] 666 | else: 667 | xhat_k = x_es 668 | D_HAT.append(dhat_k) 669 | ############################################################################### 670 | # Check for feasibile condition 671 | if np.any(np.isnan(xhat_k.__array__())): 672 | import sys 673 | sys.exit("xhat_k has some components that are NaN. This is caused by an error in the integrator. Please check the white-noise or disturbance input: maybe they are too large!") 674 | 675 | if estimating is False: 676 | 677 | if 'defSP' in locals(): 678 | ## Setpoint updating 679 | [ysp_k, usp_k, xsp_k] = defSP(t_k) 680 | Ysp.append(ysp_k) 681 | 682 | if (ksim==0): 683 | us_k = u_k 684 | xs_k = x0_m 685 | 686 | uk_prev = u_k 687 | us_prev = us_k #just a reminder that this must be us_(k-1) 688 | xs_prev = xs_k #just a reminder that this must be xs_(k-1) 689 | 690 | lambdaT_k_r = DM(lambdaT_k).reshape((nu*ny,1)) #shaping lambda matrix in order to enter par_ss 691 | 692 | ### Paramenter for Target optimization 693 | par_ss = vertcat(usp_k,ysp_k,xsp_k,dhat_k,us_prev,lambdaT_k_r,t_k,p_x_k,p_y_k) 694 | 695 | ## Target calculation initial guess 696 | wss_guess = DM.zeros(nxuy) 697 | wss_guess[0:nx] = x0_m 698 | wss_guess[nx:nxu] = u0 699 | y0 = Fy_model(x0_m,u0,dhat_k,t_k,p_y_k) 700 | wss_guess[nxu:nxuy] = y0 701 | 702 | ## Solve the Target calculation problem 703 | start_time = time.time() 704 | sol_ss = solver_ss(lbx = wss_lb, 705 | ubx = wss_ub, 706 | x0 = wss_guess, 707 | p = par_ss, 708 | lbg = gss_lb, 709 | ubg = gss_ub) 710 | 711 | etimess = time.time()-start_time 712 | 713 | ## Check feasibility: reject infeasible solutions 714 | if solver_ss.stats()['return_status'] != 'Infeasible_Problem_Detected': 715 | wss_opt = sol_ss["x"] 716 | xs_k = wss_opt[0:nx] 717 | us_k = wss_opt[nx:nxu] 718 | ys_k_opt = wss_opt[nxu:nxuy] 719 | 720 | 721 | if Adaptation is True: 722 | # Updating correction for modifiers-adaptation method 723 | cor_k = mtimes(lambdaT_k,(us_k - us_prev)) 724 | COR.append(cor_k) 725 | 726 | # Storing variables for Target calculation 727 | XS.append(xs_k) 728 | US.append(us_k) 729 | TIME_SS.append(etimess) 730 | ys_k = Fy_model(xs_k,us_k,dhat_k,t_k,p_y_k) 731 | YS.append(ys_k) 732 | 733 | ## Set current state as initial value 734 | w_lb[0:nx] = w_ub[0:nx] = xhat_k 735 | 736 | ## Set current targets 737 | cur_tar = vertcat(xs_k,us_k) 738 | 739 | 740 | if (ksim==0): 741 | ## Initial guess (on the first OCP run) 742 | if Collocation == True: 743 | w_guess = DM.zeros(nw_c) 744 | else: 745 | w_guess = DM.zeros(nw) 746 | 747 | for key in range(1,N+1,1): 748 | if Collocation == True: 749 | w_guess[key*nxuk-nu-2*nx:key*nxuk-nu] = vertcat(x0_m,x0_m) #internal states 750 | w_guess[key*nxuk-nu:key*nxuk] = u0 751 | w_guess[key*nxuk:key*nxuk+nx] = x0_m 752 | else: 753 | w_guess[key*nxu-nu:key*nxu] = u0 754 | w_guess[key*nxu:key*nxu+nx] = x0_m 755 | 756 | w_guess[0:nx] = x0_m #x0 757 | else: 758 | ## Initial guess (warm start) 759 | if Collocation == True: 760 | if solver.stats()['return_status'] != 'Infeasible_Problem_Detected': 761 | w_guess = vertcat(w_opt[nxuk:nw_c-ns],xs_prev,xs_prev,us_prev,xs_prev,w_opt[nw_c-ns:nw_c]) 762 | else: 763 | if solver.stats()['return_status'] != 'Infeasible_Problem_Detected': 764 | w_guess = vertcat(w_opt[nxu:nw-ns],us_prev,xs_prev,w_opt[nw-ns:nw]) 765 | 766 | 767 | ## Set parameter for dynamic optimisation 768 | # Reshaping parameter matrices along the horizon into vectors 769 | p_xk_r = reshape(p_xk,npx*N,1) 770 | p_yk_r = reshape(p_yk,npy*N,1) 771 | 772 | par = vertcat(xhat_k,cur_tar,dhat_k,u_k,t_k,lambdaT_k_r,p_xk_r,p_yk_r) 773 | 774 | ## Solve the OCP 775 | start_time = time.time() 776 | sol = solver(lbx = w_lb, 777 | ubx = w_ub, 778 | x0 = w_guess, 779 | p = par, 780 | lbg = g_lb, 781 | ubg = g_ub) 782 | 783 | etime = time.time()-start_time 784 | 785 | ## Check feasibility: reject infeasible solutions 786 | if solver.stats()['return_status'] != 'Infeasible_Problem_Detected': 787 | f_opt = sol["f"] 788 | w_opt = sol["x"] 789 | 790 | ## Get the optimal input and the Next predicted state (already including the disturbance estimate) i.e.x(k|k-1) 791 | if Collocation == True: 792 | u_k = w_opt[nxuk-nu:nxuk] 793 | s1_k = w_opt[nxuk-nu-2*nx:nxuk-nu-nx] 794 | s2_k = w_opt[nxuk-nu-nx:nxuk-nu] 795 | xhat_k = w_opt[nxuk:nxuk+nx] 796 | sl_k = w_opt[nw_c-ns:nw_c] 797 | else: 798 | u_k = w_opt[nxu-nu:nxu] 799 | xhat_k = w_opt[nxu:nxu+nx] 800 | sl_k = w_opt[nw-ns:nw] 801 | 802 | # Next predicted state (already including the disturbance estimate) 803 | # i.e.x(k|k-1) 804 | elif solver.stats()['return_status'] == 'Infeasible_Problem_Detected': 805 | xhat_k = Fx_model(xhat_k, u_k, h, dhat_k, t_k, p_x_k) 806 | 807 | U.append(u_k) 808 | if slacks == True: 809 | Sl.append(sl_k) 810 | TIME_DYN.append(etime) 811 | 812 | ############### Updating variables xp and xhat ########################### 813 | if Fp_nominal is True: 814 | x_k = Fx_p(x_k, u_k, h, dhat_k, t_k, p_xmp) 815 | else: # All the other cases 816 | x_k = Fx_p(x_k, u_k, p_xp, t_k, h, p_xmp) 817 | 818 | # Check for feasibile condition 819 | if np.any(np.isnan(x_k.__array__())): 820 | import sys 821 | sys.exit("x_k has some components that are NaN. This is caused by an error in the integrator. Please check the white-noise or disturbance input: maybe they are too large!") 822 | 823 | # Introducing white noise on state when present 824 | if 'G_wn' in locals(): 825 | Qw = scla.sqrtm(Q_wn) 826 | w_wn_k = mtimes(Qw,DM(np.random.normal(0,1,nxp))) 827 | x_k = x_k + mtimes(G_wn,w_wn_k) 828 | 829 | if estimating is False: 830 | 831 | if Adaptation is True: 832 | par_ssp = vertcat(t_k,us_k,p_xp,p_xmp,p_ymp) 833 | 834 | ## Target process calculation 835 | wssp_guess = x0_p 836 | 837 | sol_ss_mod = solver_ss_mod(lbx = wssp_lb, 838 | ubx = wssp_ub, 839 | x0 = wssp_guess, 840 | p = par_ssp, 841 | lbg = gssp_lb, 842 | ubg = gssp_ub) 843 | 844 | xs_kp = sol_ss_mod["x"] 845 | fss_p = sol_ss_mod["f"] 846 | 847 | ## Evaluate the updated term for the correction 848 | lambdaT_k = LambdaT(xs_kp,xs_k,us_k,dhat_k,ys_k,h,t_k,p_xp,p_yp,p_x_k,p_y_k, lambdaT_k) 849 | LAMBDA.append(lambdaT_k) 850 | 851 | 852 | ## Process economic optimum calculation 853 | par_ssp2 = vertcat(usp_k,ysp_k,xsp_k_p,p_yp,t_k,p_xp,p_xmp,p_ymp) 854 | 855 | wssp2_guess = DM.zeros(nxp+nu+ny) 856 | wssp2_guess[0:nxp] = x0_p 857 | wssp2_guess[nxp:nxp+nu] = u0 858 | y0_p = Fy_p(x0_p,p_yp,t_k,p_ymp) 859 | wssp2_guess[nxp+nu:nxp+nu+ny] = y0_p 860 | 861 | sol_ss2 = solver_ss2(lbx = wssp2_lb, 862 | ubx = wssp2_ub, 863 | x0 = wssp2_guess, 864 | p = par_ssp2, 865 | lbg = gssp2_lb, 866 | ubg = gssp2_ub) 867 | 868 | wssp2_opt = sol_ss2["x"] 869 | xs_kp2 = wssp2_opt[0:nxp] 870 | us_kp2 = wssp2_opt[nxp:nxp+nu] 871 | ys_kp2 = wssp2_opt[nxp+nu:nxp+nu+ny] 872 | fss_p2 = sol_ss2["f"] 873 | Upopt.append(us_kp2) 874 | Ypopt.append(ys_kp2) 875 | 876 | 877 | Xp = vertcat(*Xp) 878 | Yp = vertcat(*Yp) 879 | X_HAT = vertcat(*X_HAT) 880 | Y_HAT = vertcat(*Y_HAT) 881 | D_HAT = vertcat(*D_HAT) 882 | P_K = vertcat(*P_K) 883 | X_KF = vertcat(*X_KF) 884 | Sl = vertcat(*Sl) 885 | if estimating is False: 886 | U = vertcat(*U) 887 | XS = vertcat(*XS) 888 | YS = vertcat(*YS) 889 | US = vertcat(*US) 890 | TIME_SS = vertcat(*TIME_SS) 891 | TIME_DYN = vertcat(*TIME_DYN) 892 | COR = vertcat(*COR) 893 | LAMBDA = vertcat(*LAMBDA) 894 | Upopt = vertcat(*Upopt) 895 | Ypopt = vertcat(*Ypopt) 896 | 897 | ## Defining time for plotting 898 | tsim = plt.linspace(0, (Nsim-1)*h, Nsim ) 899 | 900 | plt.close("all") 901 | 902 | 903 | # Defining path for saving figures 904 | pf = pathfigure if 'pathfigure' in locals() else './' 905 | 906 | if not os.path.exists(pf): 907 | os.makedirs(pf) 908 | 909 | if estimating is True: 910 | [X_HAT, _, _] = makeplot(tsim,X_HAT,'State ',pf, Xp, lableg = 'True Value') 911 | [Y_HAT, Yp, _] = makeplot(tsim,Y_HAT,'Output ',pf, Yp, lableg = 'True Value') 912 | [X_KF, Xp, _] = makeplot(tsim,X_KF,'KF State ',pf, Xp, lableg = 'True Value') 913 | 914 | else: 915 | if Adaptation is True: 916 | [Upopt2, US2, _] = makeplot(tsim,Upopt,'Optimal Input VS Target ',pf, US, pltopt = 'steps') 917 | [U2, Upopt2, _] = makeplot(tsim,U,'Optimal Input VS Actual ',pf, Upopt, pltopt = 'steps', lableg = 'Optimal Value') 918 | [Yp2, Ypopt, _] = makeplot(tsim,Yp,'Optimal Output VS Actual ',pf, Ypopt, lableg = 'Optimal Value') 919 | [Upopt,_, _] = makeplot(tsim,Upopt,'Optimal flow ',pf, pltopt = 'steps') 920 | [COR, _, _] = makeplot(tsim,COR,'Correction on Output ',pf) 921 | [X_HAT, XS, _] = makeplot(tsim,X_HAT,'State ',pf, XS) 922 | # [Xp, _, _] = makeplot(tsim,Xp,'Process State ',pf) 923 | [U, US, _] = makeplot(tsim,U,'Input ',pf, US, pltopt = 'steps') 924 | if 'defSP' in locals(): 925 | Ysp = vertcat(*Ysp) 926 | [Yp, YS, Ysp] = makeplot(tsim,Yp,'Output ',pf,YS,Ysp) 927 | else: 928 | [Yp, YS, _] = makeplot(tsim,Yp,'Output ',pf,YS) 929 | 930 | [D_HAT, _, _ ] = makeplot(tsim,D_HAT,'Disturbance Estimate ',pf) 931 | -------------------------------------------------------------------------------- /SS_JAC_ID.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python2 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Jul 21 11:24:07 2017 5 | 6 | @author: marcovaccari 7 | """ 8 | 9 | from casadi import * 10 | from casadi.tools import * 11 | import math 12 | from Utilities import* 13 | 14 | def ss_p_jac_id(ex_name, nx, nu, ny, nd, npx, npy, k, t, px, py, LinPar): 15 | 16 | import ex_name as en 17 | import Default_Values as DV 18 | 19 | # setting the offree value momentaneusly to no when disturbance model is linear 20 | if hasattr(en, 'offree'): 21 | NOLINoffree = 'no' if en.offree == 'lin' else en.offree 22 | else: 23 | NOLINoffree = 'no' #Takes the offree from Default_Values 24 | 25 | #### Model calculation ##################################################### 26 | if hasattr(en, 'User_fxm_Cont'): 27 | if hasattr(en, 'StateFeedback'): 28 | if en.StateFeedback is True: 29 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, fx = en.User_fxm_Cont, Mx = en.Mx, SF = en.StateFeedback) 30 | else: 31 | if hasattr(en, 'User_fym'): 32 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, fx = en.User_fxm_Cont, Mx = en.Mx, fy = en.User_fym) 33 | else: 34 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, fx = en.User_fxm_Cont, Mx = en.Mx, C = en.C) 35 | elif hasattr(en, 'User_fxm_Dis'): 36 | if hasattr(en, 'StateFeedback'): 37 | if en.StateFeedback is True: 38 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, Fx = en.User_fxm_Dis, SF = en.StateFeedback) 39 | else: 40 | if hasattr(en, 'User_fym'): 41 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, Fx = en.User_fxm_Dis, fy = en.User_fym) 42 | else: 43 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, Fx = en.User_fxm_Dis, C = en.C) 44 | elif hasattr(en, 'A') and hasattr(en, 'User_fym'): 45 | [Fx_model,Fy_model] = defF_model(en.x,en.u,en.y,en.d,k,t,px,py,NOLINoffree,LinPar, A = en.A, B = en.B, fy = en.User_fym) 46 | 47 | 48 | def get_attr(attr_name, mod1, mod2): 49 | if hasattr(mod1, attr_name): 50 | return getattr(mod1, attr_name) 51 | elif hasattr(mod2, attr_name): 52 | return getattr(mod2, attr_name) 53 | else: 54 | raise AttributeError(f"Attribute '{attr_name}' is neither in '{mod1.__name__}' nor in '{mod2.__name__}'.") 55 | 56 | attr_list = ["umax", "umin", "ymax", "ymin", "xmax", "xmin"] 57 | 58 | ResultConstr = {attr: get_attr(attr, en, DV) for attr in attr_list} 59 | 60 | (solver_ss, wss_lb, wss_ub, gss_lb, gss_ub) = opt_ss_id(nx, nu, ny, nd, npx, npy, Fx_model,Fy_model, \ 61 | umin = ResultConstr["umin"], umax = ResultConstr["umax"], w_s = None, z_s = None, ymin = ResultConstr["ymin"], ymax = ResultConstr["ymax"], \ 62 | xmin = ResultConstr["xmin"], xmax = ResultConstr["xmax"], h = en.h) 63 | 64 | # Set default values in ss point 65 | d_0 = DM.zeros(nd,1) 66 | t_0 = 0.0 67 | p_xk0 = DM.zeros((npx,1)) #Dummy variable when parameter is not present 68 | p_yk0 = DM.zeros((npy,1)) #Dummy variable when parameter is not present 69 | 70 | ## Paramenter for Target optimization 71 | par_ss = vertcat(d_0,t_0, p_xk0, p_yk0) 72 | 73 | # Define useful dimension 74 | nxu = nx + nu # state+control 75 | nxuy = nx + nu + ny # state+control 76 | 77 | ## Linearization point calculation 78 | wss_guess = DM.zeros(nxuy) 79 | wss_guess[0:nx] = en.x0_m 80 | wss_guess[nx:nxu] = en.u0 81 | y0 = Fy_model(en.x0_m,en.u0,d_0,t_0,p_yk0) 82 | wss_guess[nxu:nxuy] = y0 83 | 84 | # start_time = time.time() 85 | sol_ss = solver_ss(lbx = wss_lb, 86 | ubx = wss_ub, 87 | x0 = wss_guess, 88 | p = par_ss, 89 | lbg = gss_lb, 90 | ubg = gss_ub) 91 | 92 | # etimess = time.time()-start_time 93 | wss_opt = sol_ss["x"] 94 | xlin = wss_opt[0:nx] 95 | ulin = wss_opt[nx:nxu] 96 | ylin = wss_opt[nxu:nxuy] 97 | 98 | 99 | # get linearization of states 100 | Fun_in = SX.get_input(Fx_model) 101 | A_dm = jacobian(Fx_model.call(Fun_in)[0],Fun_in[0]) 102 | A = Function('A', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4],Fun_in[5]], [A_dm]) 103 | 104 | B_dm = jacobian(Fx_model.call(Fun_in)[0],Fun_in[1]) 105 | B = Function('B', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4],Fun_in[5]], [B_dm]) 106 | 107 | # get linearization of measurements 108 | Fun_in = SX.get_input(Fy_model) 109 | C_dm = jacobian(Fy_model.call(Fun_in)[0], Fun_in[0]) 110 | C = Function('C', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4]], [C_dm]) 111 | 112 | D_dm = jacobian(Fy_model.call(Fun_in)[0], Fun_in[1]) 113 | D = Function('D', [Fun_in[0],Fun_in[1],Fun_in[2],Fun_in[3],Fun_in[4]], [D_dm]) 114 | 115 | A_k = A(xlin,ulin,en.h,d_0,t_0,p_xk0).full() 116 | B_k = B(xlin,ulin,en.h,d_0,t_0,p_xk0).full() 117 | 118 | C_k = C(xlin,ulin,d_0,t_0,p_yk0).full() 119 | D_k = D(xlin,ulin,d_0,t_0,p_yk0).full() 120 | 121 | return [A_k, B_k, C_k, D_k, xlin, ulin, ylin] 122 | 123 | 124 | def opt_ss_id(n, m, p, nd, npx, npy, Fx_model,Fy_model, umin = None, umax = None, w_s = None, z_s = None, ymin = None, ymax = None, xmin = None, xmax = None, h = None): 125 | """ 126 | SUMMARY: 127 | It builds the steady-state hunt problem 128 | """ 129 | nxu = n+m 130 | nxuy = nxu + p 131 | 132 | # Define symbolic optimization variables 133 | wss = MX.sym("wss",nxuy) 134 | 135 | # Get states 136 | Xs = wss[0:n] 137 | 138 | # Get controls 139 | Us = wss[n:nxu] 140 | 141 | # Get output 142 | Ys = wss[nxu:nxuy] 143 | 144 | # Define parameters 145 | par_ss = MX.sym("par_ss", nd+1+npx+npy) 146 | d = par_ss[:+nd] 147 | t = par_ss[nd:nd+1] 148 | px = par_ss[nd+1:nd+1+npx] 149 | py = par_ss[nd+1+npx:nd+1+npx+npy] 150 | 151 | # Defining constraints 152 | if ymin is None: 153 | ymin = -DM.inf(p) 154 | if ymax is None: 155 | ymax = DM.inf(p) 156 | if xmin is None: 157 | xmin = -DM.inf(n) 158 | if xmax is None: 159 | xmax = DM.inf(n) 160 | if umin is None: 161 | umin = -DM.inf(m) 162 | if umax is None: 163 | umax = DM.inf(m) 164 | 165 | if h is None: 166 | h = .1 #Defining integrating step if not provided from the user 167 | gss = [] 168 | 169 | Xs_next = Fx_model( Xs, Us, h, d, t, px) 170 | 171 | gss.append(Xs_next - Xs) 172 | gss = vertcat(*gss) 173 | 174 | Ys_next = Fy_model( Xs, Us, d, t, py) 175 | gss = vertcat(gss , Ys_next- Ys) 176 | 177 | # Defining obj_fun 178 | fss_obj = mtimes((Xs_next - Xs).T,(Xs_next - Xs)) + mtimes((Ys_next - Ys).T,(Ys_next - Ys)) 179 | 180 | wss_lb = -DM.inf(nxuy) 181 | wss_ub = DM.inf(nxuy) 182 | wss_lb[0:n] = xmin 183 | wss_ub[0:n] = xmax 184 | wss_lb[n: nxu] = umin 185 | wss_ub[n: nxu] = umax 186 | wss_lb[nxu: nxuy] = ymin 187 | wss_ub[nxu: nxuy] = ymax 188 | 189 | try: 190 | ng = gss.size1() 191 | except AttributeError: 192 | ng = gss.__len__() 193 | gss_lb = DM.zeros(ng,1) # Equalities identification 194 | gss_ub = DM.zeros(ng,1) 195 | 196 | 197 | nlp_ss = {'x':wss, 'p':par_ss, 'f':fss_obj, 'g':gss} 198 | 199 | solver_ss = nlpsol('solver','ipopt', nlp_ss) 200 | 201 | return [solver_ss, wss_lb, wss_ub, gss_lb, gss_ub] 202 | 203 | 204 | -------------------------------------------------------------------------------- /Target_Calc.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on December 3, 2015 4 | 5 | @author: Marco, Mirco, Gabriele 6 | 7 | Target calculation 8 | """ 9 | 10 | from casadi import * 11 | from casadi.tools import * 12 | from matplotlib import pylab as plt 13 | import math 14 | from math import * 15 | import scipy.linalg as scla 16 | import numpy as np 17 | from Utilities import* 18 | 19 | # Target calculation: model constraints and objective function 20 | def opt_ss(n, m, p, nd, npx, npy, Fx_model,Fy_model,Fss_obj,QForm_ss,DUssForm, sol_opts, G_ineq_SS, H_eq_SS, umin = None, umax = None, w_s = None, z_s = None, ymin = None, ymax = None, xmin = None, xmax = None, h = None): 21 | """ 22 | SUMMARY: 23 | It builds the target optimization problem 24 | """ 25 | nxu = n+m 26 | nxuy = nxu + p 27 | 28 | # Define symbolic optimization variables 29 | wss = MX.sym("wss",nxuy) 30 | 31 | # Get states 32 | Xs = wss[0:n] 33 | 34 | # Get controls 35 | Us = wss[n:nxu] 36 | 37 | # Get output 38 | Ys = wss[nxu:nxuy] 39 | 40 | # Define parameters 41 | par_ss = MX.sym("par_ss", 2*m+p+nd+p*m+n+1+npx+npy) 42 | usp = par_ss[0:m] 43 | ysp = par_ss[m:m+p] 44 | xsp = par_ss[m+p:m+p+n] 45 | d = par_ss[m+p+n:m+p+n+nd] 46 | Us_prev = par_ss[m+p+n+nd:2*m+p+n+nd] 47 | lambdaT_r = par_ss[2*m+p+n+nd:2*m+p+n+nd+p*m] 48 | t = par_ss[2*m+p+nd+n+p*m:2*m+p+nd+n+p*m+1] 49 | px = par_ss[2*m+p+nd+n+p*m+1:2*m+p+nd+n+p*m+1+npx] 50 | py = par_ss[2*m+p+nd+n+p*m+1+npx:2*m+p+nd+n+p*m+1+npx+npy] 51 | 52 | lambdaT = lambdaT_r.reshape((p,m)) #shaping lambda_r vector in order to reconstruct the matrix 53 | 54 | # Defining constraints 55 | if ymin is None: 56 | ymin = -DM.inf(p) 57 | if ymax is None: 58 | ymax = DM.inf(p) 59 | if xmin is None: 60 | xmin = -DM.inf(n) 61 | if xmax is None: 62 | xmax = DM.inf(n) 63 | if umin is None: 64 | umin = -DM.inf(m) 65 | if umax is None: 66 | umax = DM.inf(m) 67 | 68 | if h is None: 69 | h = .1 #Defining integrating step if not provided from the user 70 | 71 | gss = [] 72 | gss1 = [] 73 | gss2 = [] 74 | 75 | Xs_next = Fx_model( Xs, Us, h, d, t,px) 76 | 77 | gss.append(Xs_next - Xs) 78 | gss = vertcat(*gss) 79 | 80 | Ys_next = Fy_model( Xs, Us, d, t, py) + mtimes(lambdaT,(Us - Us_prev)) 81 | gss = vertcat(gss , Ys_next- Ys) 82 | 83 | # initialization of parameters as symbolic variables 84 | xSX = SX.sym("xSX", n); uSX = SX.sym("uSX", m); dSX = SX.sym("dSX", nd) 85 | tSX = SX.sym("tSX", 1); pxSX = SX.sym("pxSX", npx); pySX = SX.sym("pySX",npy); ySX = SX.sym("ySX", p) 86 | 87 | if G_ineq_SS != None: 88 | g_ineq_SS = G_ineq_SS(xSX,uSX,ySX,dSX,tSX,pxSX, pySX) 89 | G_ineqSS_SX = Function('G_ineqSS_SX', [xSX,uSX,ySX,dSX,tSX,pxSX,pySX], [g_ineq_SS]) 90 | 91 | if H_eq_SS != None: 92 | h_eq_SS = H_eq_SS(xSX,uSX,ySX,dSX,tSX,pxSX,pySX) 93 | H_eqSS_SX = Function('H_eqSS_SX', [xSX,uSX,ySX,dSX,tSX,pxSX,pySX], [h_eq_SS]) 94 | 95 | if G_ineq_SS != None: 96 | G_ss = G_ineqSS_SX(Xs, Us, Ys, d, t, px, py) 97 | else: 98 | G_ss = [] 99 | 100 | if H_eq_SS != None: 101 | H_ss = H_eqSS_SX(Xs, Us, Ys, d, t, px, py) 102 | else: 103 | H_ss = [] 104 | 105 | gss1.append(G_ss) 106 | gss2.append(H_ss) 107 | 108 | gss1 = vertcat(*gss1) 109 | gss2 = vertcat(*gss2) 110 | 111 | # Defining obj_fun 112 | dy = Ys 113 | du = Us 114 | dx = Xs 115 | 116 | if QForm_ss is True: #Checking if the OF is quadratic 117 | dx = dx - xsp 118 | dy = dy - ysp 119 | du = du - usp 120 | 121 | if DUssForm is True: 122 | du = Us - Us_prev #Adding weight on du 123 | 124 | fss_obj = Fss_obj( dx, du, dy, xsp, usp, ysp) 125 | 126 | #Defining bound constraint 127 | wss_lb = -DM.inf(nxuy) 128 | wss_ub = DM.inf(nxuy) 129 | wss_lb[0:n] = xmin 130 | wss_ub[0:n] = xmax 131 | wss_lb[n: nxu] = umin 132 | wss_ub[n: nxu] = umax 133 | wss_lb[nxu: nxuy] = ymin 134 | wss_ub[nxu: nxuy] = ymax 135 | 136 | try: 137 | ng = gss.size1() 138 | except AttributeError: 139 | ng = gss.__len__() 140 | try: 141 | ng1 = gss1.size1() 142 | except AttributeError: 143 | ng1 = gss1.__len__() 144 | try: 145 | ng2 = gss2.size1() 146 | except AttributeError: 147 | ng2 = gss2.__len__() 148 | 149 | gss_lb = DM.zeros(ng+ng1+ng2,1) # Equalities identification 150 | gss_ub = DM.zeros(ng+ng1+ng2,1) 151 | 152 | if ng1 != 0: 153 | gss_lb[ng:ng+ng1] = -DM.inf(ng1) 154 | 155 | gss = vertcat (gss, gss1, gss2) 156 | 157 | nlp_ss = {'x':wss, 'p':par_ss, 'f':fss_obj, 'g':gss} 158 | 159 | solver_ss = nlpsol('solver','ipopt', nlp_ss, sol_opts) 160 | 161 | return [solver_ss, wss_lb, wss_ub, gss_lb, gss_ub] -------------------------------------------------------------------------------- /User_Guide.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CPCLAB-UNIPI/MPC-code/f60f44c7128e51dca119c0ec2d23d8730509064f/User_Guide.pdf -------------------------------------------------------------------------------- /Utilities.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on December 3, 2015 4 | 5 | @author: Marco, Mirco, Gabriele 6 | 7 | Utilities for general purposes 8 | """ 9 | from __future__ import division 10 | 11 | from builtins import str 12 | from builtins import range 13 | from past.utils import old_div 14 | from casadi import * 15 | from casadi.tools import * 16 | from matplotlib import pylab as plt 17 | import math 18 | import scipy.linalg as scla 19 | import numpy as np 20 | 21 | def defF_p(x, u, y, k, t, pxp, pyp, pxmp, pymp,LinPar, **plant): 22 | """ 23 | SUMMARY: 24 | Starting from system matrix or equation builds the system model 25 | 26 | SYNTAX: 27 | assignment = defFp(x, u, y, k, t, **plant) 28 | 29 | ARGUMENTS: 30 | + x, u, y - State, Input and Output symbolic variable 31 | + k - Integration step symbolic variable 32 | + t - Current time 33 | + pxp, pyp - Parameters on State and Output maps 34 | + pxmp, pymp - Measurable Parameters on State and Output maps 35 | + LinPar - Tag for setting additive Parameters on State and Output maps 36 | + plant - Plant equations/system matrices 37 | 38 | OUTPUTS: 39 | + Fx_p - State correlation function 40 | + Fy_p - Output correlation function 41 | """ 42 | nx = x.size1() 43 | 44 | for key in plant: 45 | if key == 'Ap': # Linear model 46 | Ap = plant['Ap'] 47 | Bp = plant['Bp'] 48 | fx_p = (mtimes(Ap,x) + mtimes(Bp,u)) + pxp + pxmp 49 | Fx_p = Function('Fx_p', [x,u,pxp,t,k,pxmp], [fx_p]) 50 | 51 | elif key == 'Fx': # NON-Linear Discrete-Time 52 | Fx_p = plant['Fx'] 53 | dummyF = Fx_p(x,t,u,pxp,pxmp) 54 | if LinPar is True: 55 | dummyF = dummyF + pxp + pxmp 56 | Fx_p = Function('Fx_p', [x,u,pxp,t,k,pxmp], [dummyF]) 57 | 58 | elif key == 'fx': # NON-Linear Continuous-Time 59 | deffxp = plant['fx'] 60 | Mx = plant['Mx'] 61 | dummyF = deffxp(x,t,u,pxp,pxmp) 62 | xnew = vertcat(x,t) 63 | unew = vertcat(u,pxp) 64 | unew = vertcat(unew,pxmp) 65 | 66 | # Constructing the augmented system for the integrator 67 | dummyF2 = vertcat(dummyF, SX(1.)) 68 | dummyF3 = Function('dummyF3', [xnew,unew], [dummyF2]) 69 | 70 | Int_Fx_p = simpleRK(dummyF3, Mx) 71 | dummyF_f = vertcat(Int_Fx_p(xnew,unew,k)) 72 | Fx_p2 = Function('Fx_p2', [x,u,t,k,pxp,pxmp], [dummyF_f]) 73 | 74 | # Caring only about the [:nx] of the output: 75 | Fx_p = Function('Fx_p',[x,u,t,k,pxp,pxmp],[Fx_p2(x,u,t,k,pxp,pxmp)[:nx,:]]) 76 | 77 | # Adding disturbance dx linearly 78 | if LinPar is True: 79 | Pxp = Function('Pxp', [pxp], [pxp]) 80 | Pxmp = Function('Pxmp', [pxmp], [pxmp]) 81 | dummyF = vertcat(Fx_p(x,u,t,k,pxp,pxmp)) + vertcat(Pxp(pxp))+ vertcat(Pxmp(pxmp)) 82 | Fx_p = Function('Fx_p', [x,u,pxp,t,k,pxmp], [dummyF]) 83 | 84 | if key == 'SF': 85 | fy_p = x 86 | Fy_p = Function('Fy_p', [x,u,pyp,t,pymp], [fy_p]) 87 | else: 88 | if key == 'Cp': # Linear model 89 | Cp = plant['Cp'] 90 | fy_p = mtimes(Cp,x) + pyp + pymp 91 | Fy_p = Function('Fy_p', [x,u,pyp,t,pymp], [fy_p]) 92 | 93 | elif key == 'fy': # NON-Linear model 94 | deffyp = plant['fy'] 95 | dummyF = deffyp(x,u,t,pyp,pymp) 96 | if LinPar is True: 97 | dummyF = dummyF + pyp + pymp 98 | Fy_p = Function('Fy_p', [x,u,pyp,t,pymp], [dummyF]) 99 | 100 | return [Fx_p,Fy_p] 101 | 102 | def defF_model(x, u, y, d, k, t, px, py, offree, LinPar, **model): 103 | """ 104 | SUMMARY: 105 | Starting from system matrix or equation builds the system model 106 | 107 | SYNTAX: 108 | assignment = defF_model(x, u, y, d, k, t, offree, **model) 109 | 110 | ARGUMENTS: 111 | + x, u, y, d - State, Input, Output and Disturbance symbolic variable 112 | + k - Integration step symbolic variable 113 | + t - Current time 114 | + px, py - Parameters on State and Output maps 115 | + offree - Offset free tag 116 | + LinPar - Tag for setting additive Parameters on State and Output maps 117 | + model - Model equations/system matrices 118 | 119 | OUTPUTS: 120 | + Fx_model - State correlation function 121 | + Fy_model - Output correlation function 122 | """ 123 | if offree == "lin": 124 | Bd = model['Bd'] 125 | Cd = model['Cd'] 126 | 127 | if offree == 'nl': 128 | unew = vertcat(u,d) 129 | else: 130 | unew = u 131 | 132 | nx = x.size1() 133 | 134 | for key in model: 135 | if key == 'A': # Linear model 136 | A = model['A'] 137 | B = model['B'] 138 | for bkey in model: 139 | if bkey == 'xlin': 140 | xlin = model['xlin'] 141 | ulin = model['ulin'] 142 | fx_model = mtimes(A,x - xlin) + mtimes(B,u - ulin) + xlin # The state model is linearised in xlin, ulin 143 | break 144 | try: 145 | fx_model 146 | except NameError: 147 | fx_model = mtimes(A,x) + mtimes(B,u) 148 | 149 | if offree == "lin": 150 | fx_model = fx_model + mtimes(Bd,d) 151 | 152 | # Adding measurable parameters linearly 153 | fx_model = fx_model + px 154 | 155 | Fx_model = Function('Fx_model',[x,u,k,d,t,px], [fx_model]) 156 | 157 | elif key == 'fx': # NON-Linear continuous model 158 | fx = model['fx'] 159 | Mx = model['Mx'] 160 | 161 | fx_model = fx(x,u,d,t,px) 162 | 163 | dummyF = vertcat(fx_model, SX(1.)) 164 | xnew = vertcat(x,t) 165 | unew = vertcat(unew,px) 166 | 167 | dummyF2 = Function('dummyF2', [xnew,unew], [dummyF]) 168 | Int_Fx_m = simpleRK(dummyF2, Mx) 169 | dummyF_f = vertcat(Int_Fx_m(xnew,unew,k)) 170 | Fx_model2 = Function('Fx_model2', [x,u,k,d,t,px], [dummyF_f]) 171 | 172 | Fx_model = Function('Fx_model',[x,u,k,d,t,px],[Fx_model2(x,u,k,d,t,px)[:nx,:]]) 173 | 174 | if offree == "lin": 175 | Dx = Function('Dx', [d], [mtimes(Bd,d)]) 176 | dummyF = vertcat(Fx_model(x,u,k,d,t,px)) + vertcat(Dx(d)) 177 | Fx_model = Function('Fx_model', [x,u,k,d,t,px], [dummyF]) 178 | 179 | # Adding measurable parameter linearly 180 | if LinPar is True: 181 | Px = Function('Px', [px], [px]) 182 | dummyF = vertcat(Fx_model(x,u,k,d,t,px)) + vertcat(Px(px)) 183 | Fx_model = Function('Fx_model', [x,u,k,d,t,px], [dummyF]) 184 | 185 | 186 | elif key == 'Fx': # NON-linear discrete model 187 | 188 | Fx_model = model['Fx'] 189 | 190 | dummyF = Fx_model(x,u,d,t,px) 191 | 192 | if offree == "lin": 193 | dummyF = dummyF + mtimes(Bd,d) 194 | 195 | if LinPar is True: 196 | dummyF = dummyF + px 197 | 198 | Fx_model = Function('Fx_model', [x,u,k,d,t,px], [dummyF]) 199 | 200 | 201 | if key == 'SF': 202 | fy_model = x 203 | 204 | if offree == "lin": 205 | fy_model = fy_model + mtimes(Cd,d) 206 | 207 | else: 208 | if key == 'C': # Linear model 209 | C = model['C'] 210 | for bkey in model: 211 | if bkey == 'ylin': 212 | ylin = model['ylin'] 213 | for tkey in model: 214 | if tkey == 'xlin': 215 | xlin = model['xlin'] 216 | fy_model = mtimes(C,x - xlin) + ylin # Both the state and the output model are linearised 217 | break 218 | try: 219 | fy_model 220 | break 221 | except NameError: 222 | fy_model = mtimes(C,x) + ylin # Only the output model is linearised 223 | break 224 | try: 225 | fy_model 226 | except NameError: 227 | fy_model = mtimes(C,x) # The system is linear and there was no need to linearised it 228 | 229 | if offree == "lin": 230 | fy_model = fy_model + mtimes(Cd,d) 231 | 232 | elif key == 'fy': # NON-Linear model 233 | fy = model['fy'] 234 | 235 | fy_model = fy(x,u,d,t,py) 236 | 237 | if offree == "lin": 238 | fy_model = fy_model + mtimes(Cd,d) 239 | 240 | if LinPar is True: 241 | # Adding linearly parameters 242 | fy_model = fy_model + py 243 | 244 | Fy_model = Function('Fy_model', [x,u,d,t,py], [fy_model]) 245 | return [Fx_model,Fy_model] 246 | 247 | def xQx(x,Q): 248 | """ 249 | SUMMARY: 250 | Starting from a vector x and a square matrix Q, the function execute the 251 | operation x'Qx 252 | 253 | SYNTAX: 254 | result = sysaug(x, Q) 255 | 256 | ARGUMENTS: 257 | + x - Column vector 258 | + Q - Square matrix 259 | 260 | OUTPUTS: 261 | + Qx2 - Result of the operation x'Qx 262 | """ 263 | Qx = mtimes(Q,x) 264 | Qx2 = mtimes(x.T,Qx) 265 | return Qx2 266 | 267 | def defFss_obj(x, u, y, xsp, usp, ysp, **kwargs): 268 | """ 269 | SUMMARY: 270 | It construct the steady-state optimisation objective function 271 | 272 | SYNTAX: 273 | assignment = defFss_obj(x, u, y, xsp, usp, ysp, **kwargs) 274 | 275 | ARGUMENTS: 276 | + x, u, y - State, Input and Output symbolic variables 277 | + xsp, usp, ysp - State, Input and Output setpoints 278 | + kwargs - Objective function/Matrix for QP/Vector for LP problem 279 | 280 | OUTPUTS: 281 | + Fss_obj - Steady-state optimisation objective function 282 | """ 283 | for key in kwargs: 284 | if key == 'r_y': # LP problem 285 | rss_y = kwargs['r_y'] 286 | x_abs = fabs(x) 287 | u_abs = fabs(u) 288 | for bkey in kwargs: 289 | if bkey is 'r_u': 290 | rss_u = kwargs['r_u'] 291 | fss_obj = mtimes(rss_y,y) + mtimes(rss_u,u_abs) 292 | break 293 | elif bkey is 'r_Du': 294 | rss_Du = kwargs['r_Du'] 295 | fss_obj = mtimes(rss_y,y) + mtimes(rss_Du,u_abs) 296 | break 297 | 298 | break 299 | elif key == 'Q': # QP problem 300 | Qss = kwargs['Q'] 301 | yQss2 = xQx(y,Qss) 302 | 303 | for bkey in kwargs: 304 | if bkey is 'R': 305 | Rss = kwargs['R'] 306 | uRss2 = xQx(u, Rss) 307 | fss_obj = 0.5*(yQss2 + uRss2) 308 | break 309 | elif bkey is 'S': 310 | Sss = kwargs['S'] 311 | uSss2 = xQx(u,Sss) 312 | fss_obj = 0.5*(yQss2 + uSss2) 313 | break 314 | break 315 | elif key == 'f_obj': # NON-linear function 316 | f_obj1 = kwargs['f_obj'] 317 | fss_obj = f_obj1(x,u,y,xsp,usp,ysp) 318 | break 319 | 320 | Fss_obj = Function('Fss_obj', [x,u,y,xsp,usp,ysp], [fss_obj]) 321 | return Fss_obj 322 | 323 | def defF_obj(x, u, y, xs, us, ys, **kwargs): 324 | """ 325 | SUMMARY: 326 | It constructs the dynamic optimisation objective function 327 | 328 | SYNTAX: 329 | assignment = defF_obj(x, u, y, xs, us, ys, **kwargs) 330 | 331 | ARGUMENTS: 332 | + x, u, y - State, Input and Output symblic variables 333 | + xs, us, ys - State, Input and Output target symblic variables 334 | + kwargs - Objective function/Matrix for QP/Vector for LP problem 335 | 336 | OUTPUTS: 337 | + F_obj - Dynamic optimisation objective function 338 | """ 339 | 340 | for key in kwargs: 341 | if key is 'r_x': # LP problem 342 | r_x = kwargs['r_x'] 343 | x_abs = fabs(x) 344 | u_abs = fabs(u) 345 | for bkey in kwargs: 346 | if bkey is 'r_u': 347 | r_u = kwargs['r_u'] 348 | f_obj = mtimes(r_x,x_abs) + mtimes(r_u,u_abs) 349 | elif bkey is 'r_Du': 350 | r_Du = kwargs['r_Du'] 351 | f_obj = mtimes(r_x,x_abs) + mtimes(r_Du,u_abs) 352 | F_obj = Function('F_obj', [x,u,y,xs,us,ys], [f_obj]) 353 | elif key is 'Q': # QP problem 354 | Q = kwargs['Q'] 355 | xQ2 = xQx(x,Q) 356 | for bkey in kwargs: 357 | if bkey is 'R': 358 | R = kwargs['R'] 359 | uR2 = xQx(u,R) 360 | f_obj = 0.5*(xQ2 + uR2) 361 | break 362 | elif bkey is 'S': 363 | S = kwargs['S'] 364 | uS2 = xQx(u,S) 365 | f_obj = 0.5*(xQ2 + uS2) 366 | break 367 | F_obj = Function('F_obj', [x,u,y,xs,us,ys], [f_obj]) 368 | elif key is 'f_Cont': # NON-linear continuous function 369 | f_obj = kwargs['f_Cont'] 370 | F_obj = f_obj 371 | elif key is 'f_Dis': 372 | f_obj = kwargs['f_Dis'] 373 | F_obj1 = f_obj(x,u,y,xs,us,ys) 374 | F_obj = Function('F_obj', [x,u,y,xs,us,ys], [F_obj1]) 375 | elif key is 'f_Coll': 376 | s_Coll = kwargs['s_Coll'] 377 | f_obj = kwargs['f_Coll'] 378 | F_obj1 = f_obj(x,u,y,xs,us,ys,s_Coll) 379 | F_obj = Function('F_obj', [x,u,y,xs,us,ys,s_Coll], [F_obj1]) 380 | 381 | return F_obj 382 | 383 | def defVfin(x,xs, **Tcost): 384 | """ 385 | SUMMARY: 386 | It constructs the terminal cost for the dynamic optimisation objective function 387 | 388 | SYNTAX: 389 | assignment = defVfin(x, **Tcost): 390 | 391 | ARGUMENTS: 392 | + x - State symbolic variable 393 | + Tcost - Terminal weigth specified by the user/ Matrices to calculate Riccati equation 394 | 395 | OUTPUTS: 396 | + Vfin - terminal cost for the dynamic optimisation objective function 397 | """ 398 | 399 | if Tcost == {}: 400 | vfin = 0.0 401 | else: 402 | for key in Tcost: 403 | if key == 'A': # Linear system & QP problem 404 | A = Tcost['A'] 405 | B = Tcost['B'] 406 | Q = Tcost['Q'] 407 | R = Tcost['R'] 408 | ## Solution to Riccati Equation 409 | P = DM(scla.solve_discrete_are(np.array(A), np.array(B), \ 410 | np.array(Q), np.array(R))) 411 | 412 | vfin = 0.5*(xQx(x,P)) 413 | break 414 | elif key == 'vfin_F': 415 | vfin_F = Tcost['vfin_F'] 416 | vfin = vfin_F(x,xs) 417 | break 418 | Vfin = Function('Vfin', [x,xs], [vfin]) 419 | 420 | return Vfin 421 | 422 | def makeplot(tsim,X1,label,pf,*var,**kwargs): 423 | """ 424 | SUMMARY: 425 | It constructs the plot where tsim is on the x-axis, 426 | X1,X2,X3 on the y-axis, and label is the label of the y-axis 427 | 428 | SYNTAX: 429 | makeplot(tsim,X1,label,*var): 430 | 431 | ARGUMENTS: 432 | + tsim - x-axis vector (time of the simulation (min)) 433 | + X1,X2,X3 - y-axis vectors. 434 | X1 represent the actual value 435 | X2 the target (eventual) 436 | X3 the setpoint (eventual) 437 | + label - label for the y-axis 438 | + pf - path where the plots are saved 439 | + var - positional variables to include another vector/s X2 and X3 to plot together with X1 440 | + kwargs - plot options including linestyle and changing the default legend values 441 | """ 442 | 443 | linetype = '-' #defaul value for linetype 444 | lableg = 'Target' #defaul value for legend label 445 | for kwkey in kwargs: 446 | if kwkey == 'pltopt': 447 | linetype = kwargs['pltopt'] 448 | if kwkey == 'lableg': 449 | lableg = kwargs['lableg'] 450 | 451 | nt = int(tsim.size) 452 | 453 | X1 = np.array(X1) 454 | 455 | sz = old_div(X1.size,nt) 456 | Xout1 = np.zeros((nt,sz)) 457 | Xout2 = np.zeros((nt,sz)) 458 | Xout3 = np.zeros((nt,sz)) 459 | 460 | for k in range(sz): 461 | x1 = X1[k::sz] 462 | 463 | plt.figure() 464 | try: 465 | plt.plot(tsim, x1, ls = linetype) 466 | except ValueError: 467 | plt.step(tsim,x1) 468 | plt.xlabel('Time ') 469 | plt.ylabel(label + str(k+1)) 470 | plt.gca().set_xlim(left=0,right=tsim[-1]) 471 | 472 | Xout1[:,k] = np.reshape(x1,(nt,)) 473 | 474 | for i_var in range(len(var)): 475 | # extract dimension of var 476 | var_i = var[i_var] 477 | Xi = np.array(var_i) 478 | xi = Xi[k::sz] 479 | try: 480 | plt.plot(tsim, xi, ls = linetype) 481 | except ValueError: 482 | plt.step(tsim,xi) 483 | if i_var == 0: 484 | plt.legend(('Actual', lableg)) 485 | plt.gca().set_xlim(left=0,right=tsim[-1]) 486 | Xout2[:,k] = np.reshape(xi,(nt,)) 487 | elif i_var == 1: 488 | plt.legend(('Actual', 'Target', 'Set-Point')) 489 | plt.gca().set_xlim(left=0,right=tsim[-1]) 490 | Xout3[:,k] = np.reshape(xi,(nt,)) 491 | 492 | plt.grid(True) 493 | 494 | plt.savefig(pf + label + str(k+1) + '.pdf', format = 'pdf', transparent = True, bbox_inches = 'tight' ) 495 | 496 | return [Xout1, Xout2, Xout3] 497 | 498 | def defLambdaT(xp,x,u,y,d,k,t,pxp,pyp,px,py, fx_model, fxp, Fy_model, Fy_p, alphalss): 499 | """ 500 | SUMMARY: 501 | It constructs the function to evaluate the modifiers adaptation correction term 502 | 503 | SYNTAX: 504 | assignment = defLambdaT(xp,x,u,y,d,k, fx_model, fxp, Fy_model, Fy_p): 505 | 506 | ARGUMENTS: 507 | + xp,x,u,y,d,k - State, Input, Output, Disturbance symbolic variables 508 | + fx_model - Model state function 509 | + fxp - Process state function 510 | + Fy_model - Model output function 511 | + Fy_p - Process output function 512 | + alphalss - Filtering factor (default value = 0.2) 513 | 514 | OUTPUTS: 515 | + LambdaT - Function to evaluate the modifiers correction term 516 | """ 517 | lambdaTprev = SX.sym('lambdaTprev',(y.size1(),u.size1())) 518 | 519 | Fun_in = SX.get_input(fx_model) 520 | Nablaxfx = jacobian(fx_model.call(Fun_in)[0], Fun_in[0]) 521 | Nablaufx = jacobian(fx_model.call(Fun_in)[0], Fun_in[1]) 522 | inv_Nablaxfx = solve((DM.eye(Nablaxfx.size1())- Nablaxfx), Nablaufx) 523 | Fun_in = SX.get_input(Fy_model) 524 | Nablaxfy = jacobian(Fy_model.call(Fun_in)[0], Fun_in[0]) 525 | gradyModel = mtimes(Nablaxfy,inv_Nablaxfx) 526 | 527 | Fun_in = SX.get_input(fxp) 528 | Nablaxfxp = jacobian(fxp.call(Fun_in)[0], Fun_in[0]) 529 | Nablaufxp = jacobian(fxp.call(Fun_in)[0], Fun_in[1]) 530 | inv_Nablaxfxp = solve((DM.eye(Nablaxfxp.size1())- Nablaxfxp), Nablaufxp) 531 | Fun_in = SX.get_input(Fy_p) 532 | Nablaxfyp = jacobian(Fy_p.call(Fun_in)[0], Fun_in[0]) 533 | gradyPlant = mtimes(Nablaxfyp,inv_Nablaxfxp) 534 | 535 | gradydiff = gradyPlant - gradyModel 536 | 537 | lambdaT = (1-alphalss)*lambdaTprev + alphalss*gradydiff 538 | 539 | LambdaT = Function('LambdaT', [xp,x,u,d,y,k,t,pxp,pyp,px,py,lambdaTprev], [lambdaT]) 540 | 541 | return LambdaT 542 | 543 | def opt_ssp(n, m, p, nd, npx, npy, npxp, Fx ,Fy ,sol_opts, xmin = None, xmax = None, h = None): 544 | """ 545 | SUMMARY: 546 | It builds the process steady-state point optimization problem 547 | """ 548 | # Define symbolic optimization variables 549 | Xs = MX.sym("wss",n) 550 | 551 | # Define parameters 552 | par_ss = MX.sym("par_ss", 1+m+npx+npxp+npyp) 553 | t = par_ss[0] 554 | us_k = par_ss[1:m+1] 555 | pxp = par_ss[m+1:m+npxp+1] 556 | px = par_ss[m+npxp+1:m+npxp+1+npx] 557 | py = par_ss[m+npxp+1+npx:m+npxp+1+npx+npy] 558 | 559 | if xmin is None: 560 | xmin = -DM.inf(n) 561 | if xmax is None: 562 | xmax = DM.inf(n) 563 | 564 | if h is None: 565 | h = .1 #Defining integrating step if not provided from the user 566 | gss = [] 567 | 568 | Xs_next = Fx( Xs, us_k, pxp, t, h, px) 569 | 570 | gss.append(Xs_next - Xs) 571 | gss = vertcat(*gss) 572 | 573 | fss_obj = mtimes((Xs_next - Xs).T,(Xs_next - Xs)) 574 | 575 | ng = gss.size1() 576 | gss_lb = DM.zeros(ng,1) # Equalities identification 577 | gss_ub = DM.zeros(ng,1) 578 | 579 | nlp_ss = {'x':Xs, 'p':par_ss, 'f':fss_obj, 'g':gss} 580 | 581 | solver_ss = nlpsol('solver','ipopt', nlp_ss, sol_opts) 582 | 583 | return [solver_ss, xmin, xmax, gss_lb, gss_ub] 584 | 585 | def opt_ssp2(n, m, p, nd, npx, npy, npxp, npyp, Fx ,Fy ,Fss_obj,QForm_ss,sol_opts, umin = None, umax = None, w_s = None, z_s = None, ymin = None, ymax = None, xmin = None, xmax = None, h = None): 586 | """ 587 | SUMMARY: 588 | It searches the true optimal plant value 589 | """ 590 | nxu = n+m 591 | nxuy = nxu + p 592 | 593 | # Define symbolic optimization variables 594 | wss = MX.sym("wss",nxuy) 595 | 596 | # Get states 597 | Xs = wss[0:n] 598 | 599 | # Get controls 600 | Us = wss[n:nxu] 601 | 602 | # Get output 603 | Ys = wss[nxu:nxuy] 604 | 605 | # Define parameters 606 | par_ss = MX.sym("par_ss", n+m+p+npyp+1+npxp+npx+npy) 607 | usp = par_ss[0:m] 608 | ysp = par_ss[m:m+p] 609 | xsp = par_ss[m+p:m+p+n] 610 | pyp = par_ss[m+p+n:m+p+npyp+n] 611 | t = par_ss[m+p+npyp+n:m+p+npyp+n+1] 612 | pxp = par_ss[m+p+npyp+n+1:m+p+npyp+n+1+npxp] 613 | px = par_ss[m+p+npyp+n+1+npxp:m+p+npyp+n+1+npxp+npx] 614 | py = par_ss[m+p+npyp+n+1+npxp+npx:m+p+npyp+n+1+npxp+npx+npy] 615 | 616 | # Defining constraints 617 | if ymin is None: 618 | ymin = -DM.inf(p) 619 | if ymax is None: 620 | ymax = DM.inf(p) 621 | if xmin is None: 622 | xmin = -DM.inf(n) 623 | if xmax is None: 624 | xmax = DM.inf(n) 625 | if umin is None: 626 | umin = -DM.inf(m) 627 | if umax is None: 628 | umax = DM.inf(m) 629 | 630 | if h is None: 631 | h = .1 #Defining integrating step if not provided from the user 632 | gss = [] 633 | 634 | Xs_next = Fx( Xs, Us, pxp, t, h, px) 635 | 636 | gss.append(Xs_next - Xs) 637 | gss = vertcat(*gss) 638 | 639 | Ys_next = Fy( Xs,Us, pyp, t, py) 640 | gss = vertcat(gss , Ys_next- Ys) 641 | 642 | # Defining obj_fun 643 | dy = Ys 644 | du = Us 645 | dx = Xs 646 | 647 | if QForm_ss is True: #Checking if the OF is quadratic 648 | dx = dx - Xs 649 | dy = dy - ysp 650 | du = du - usp 651 | 652 | fss_obj = Fss_obj( dx, du, dy, xsp, usp, ysp) 653 | 654 | wss_lb = -DM.inf(nxuy) 655 | wss_ub = DM.inf(nxuy) 656 | wss_lb[0:n] = xmin 657 | wss_ub[0:n] = xmax 658 | wss_lb[n: nxu] = umin 659 | wss_ub[n: nxu] = umax 660 | wss_lb[nxu: nxuy] = ymin 661 | wss_ub[nxu: nxuy] = ymax 662 | 663 | ng = gss.size1() 664 | gss_lb = DM.zeros(ng,1) # Equalities identification 665 | gss_ub = DM.zeros(ng,1) 666 | 667 | 668 | nlp_ss = {'x':wss, 'p':par_ss, 'f':fss_obj, 'g':gss} 669 | 670 | solver_ss = nlpsol('solver','ipopt', nlp_ss, sol_opts) 671 | 672 | return [solver_ss, wss_lb, wss_ub, gss_lb, gss_ub] 673 | 674 | 675 | def defF_obj_mhe(w, v, t, **kwargs): 676 | """ 677 | SUMMARY: 678 | It constructs the objective function for the mhe optimization problem 679 | 680 | SYNTAX: 681 | assignment = defF_obj(w, v, y, **kwargs) 682 | 683 | ARGUMENTS: 684 | + w, v, t - State, Input and Time symblic variables 685 | + kwargs - Objective function/Matrix for QP/Vector for LP problem 686 | 687 | OUTPUTS: 688 | + F_obj - Dynamic optimisation objective function 689 | """ 690 | 691 | for key in kwargs: 692 | if key is 'r_w': # LP problem 693 | r_w = kwargs['r_w'] 694 | r_v = kwargs['r_v'] 695 | f_obj = mtimes(r_w,w) + mtimes(r_v,v) 696 | F_obj = Function('F_obj', [w,v,t], [f_obj]) 697 | elif key is 'Q': # QP problem 698 | Q = kwargs['Q'] 699 | wQ2 = xQx(w,Q) 700 | R = kwargs['R'] 701 | vR2 = xQx(v,R) 702 | f_obj = 0.5*(wQ2 + vR2) 703 | F_obj = Function('F_obj', [w,v,t], [f_obj]) 704 | elif key is 'f_obj': 705 | f_obj = kwargs['f_obj'] 706 | F_obj1 = f_obj(w,v,t) 707 | F_obj = Function('F_obj', [w,v,t], [F_obj1]) 708 | 709 | return F_obj 710 | 711 | 712 | 713 | def defFx_mhe(x, u, w, d, k, t, px, offree, LinPar, **model): 714 | """ 715 | SUMMARY: 716 | Starting from equation builds the system model 717 | 718 | SYNTAX: 719 | assignment = defFx_mhe(x, u, w, d, k, t, offree, **model) 720 | 721 | ARGUMENTS: 722 | + x, u, w, d - State, Input, State noise and Disturbance symbolic variable 723 | + k - Integration step symbolic variable 724 | + t - Current time 725 | + px - Parameters on State map 726 | + offree - Offset free tag 727 | + model - Model equations/system matrices 728 | 729 | OUTPUTS: 730 | + Fx_mhe - State correlation function 731 | """ 732 | if offree == 'nl': 733 | unew = vertcat(u,d) 734 | else: 735 | unew = u 736 | 737 | nx = x.size1() 738 | nd = d.size1() 739 | 740 | csi = SX.sym("csi", nx+nd) 741 | x1 = csi[0:nx] 742 | d1 = csi[nx:nx+nd] 743 | 744 | 745 | for key in model: 746 | if key == 'fx': # NON-Linear continuous model 747 | fx = model['fx'] 748 | Mx = model['Mx'] 749 | fx_mhe = fx(x,u,d,t,px,w) # to trasform in SX to be processed in casadi function 750 | 751 | dummyF = vertcat(fx_mhe, SX(1.)) 752 | xnew = vertcat(x,t) 753 | unew = vertcat(unew,w) 754 | unew = vertcat(unew,px) 755 | 756 | dummyF2 = Function('dummyF2', [xnew,unew], [dummyF]) 757 | Int_Fx_m = simpleRK(dummyF2, Mx) 758 | dummyF_f = vertcat(Int_Fx_m(xnew,unew,k)) 759 | Fx_2 = Function('Fx_2', [x,u,k,t,w,px], [dummyF_f]) #In this way the output is always a SX 760 | 761 | # Caring only about the [:nx] of the output: 762 | Fx_mhe = Function('Fx_mhe',[x,u,k,t,w,px],[Fx_2(x,u,k,t,w,px)[:nx,:]]) 763 | 764 | if offree == "no": 765 | G = model['G'] 766 | Gw = Function('Gw', [w], [mtimes(G,w)]) 767 | if LinPar == true: 768 | Px = Function('Px', [px], [px]) 769 | dummyF = vertcat(Fx_mhe(x,u,k,t,w,px)) + vertcat(Gw(w)) + vertcat(Px(px)) 770 | Fx_mhe = Function('Fx_mhe', [x,u,k,t,w,px], [dummyF]) 771 | else: 772 | dummyF = vertcat(Fx_mhe(x,u,k,t,w,px)) + vertcat(Gw(w)) 773 | Fx_mhe = Function('Fx_mhe', [x,u,k,t,w,px], [dummyF]) 774 | break 775 | 776 | elif key == 'Fx': # NON-linear discrete model 777 | Fx = model['Fx'] 778 | if offree == 'nl': 779 | dummyF = Fx(x1,u,d1,t,px,w) 780 | Fx_mhe = Function('Fx_mhe', [csi,u,k,t,w,px], [dummyF]) 781 | 782 | Dx = Function('Dx',[d],[d]) 783 | dummyFx = vertcat(Fx_mhe(csi,u,k,t,w,px), Dx(d1)) 784 | Fx_mhe2 = Function('Fx_mhe2', [csi,u,k,t,w,px], [dummyFx]) 785 | 786 | G = model['G'] 787 | Gw = Function('Gw', [w], [mtimes(G,w)]) 788 | if LinPar == True: 789 | Px = Function('Px', [px], [vertcat(px,DM.zeros(nd))]) 790 | dummyF = vertcat(Fx_mhe2(csi,u,k,t,w,px)) + vertcat(Gw(w)) + vertcat(Px(px)) 791 | Fx_mhe = Function('Fx_mhe', [csi,u,k,t,w,px], [dummyF]) 792 | else: 793 | dummyF = vertcat(Fx_mhe2(csi,u,k,t,w,px)) + vertcat(Gw(w)) 794 | Fx_mhe = Function('Fx_mhe', [csi,u,k,t,w,px], [dummyF]) 795 | else: 796 | dummyF = Fx_model(x,u,d,t,px) 797 | 798 | if LinPar is True: 799 | dummyF = dummyF + px 800 | 801 | Fx_mhe = Function('Fx_mhe', [x,u,k,t,w,px], [dummyF]) 802 | break 803 | 804 | if offree == "lin": 805 | Bd = model['Bd'] 806 | Dx = Function('Dx', [d], [mtimes(Bd,d)]) 807 | dummyF = vertcat(Fx_mhe(x,u,k,t,w,px)) + vertcat(Dx(d)) 808 | Fx_mhe1 = Function('Fx_mhe1', [x,u,k,t,w,d,px], [dummyF]) 809 | Dx = Function('Dx',[d],[d]) 810 | dummyFx = vertcat(Fx_mhe1(x1,u,k,t,w,d1,px), Dx(d1)) 811 | Fx_mhe2 = Function('Fx_mhe2', [csi,u,k,t,w,px], [dummyFx]) 812 | 813 | G = model['G'] 814 | Gw = Function('Gw', [w], [mtimes(G,w)]) 815 | if LinPar == True: 816 | Px = Function('Px', [px], [vertcat(px,DM.zeros(nd))]) 817 | dummyF = vertcat(Fx_mhe2(csi,u,k,t,w,px)) + vertcat(Gw(w)) + vertcat(Px(px)) 818 | Fx_mhe = Function('Fx_mhe', [csi,u,k,t,w,px], [dummyF]) 819 | else: 820 | dummyF = vertcat(Fx_mhe2(csi,u,k,t,w,px)) + vertcat(Gw(w)) 821 | Fx_mhe = Function('Fx_mhe', [csi,u,k,t,w,px], [dummyF]) 822 | 823 | return Fx_mhe 824 | 825 | def mhe_opt(n, nd, m, p,npx,npy, n_w, F_obj_mhe, Fx_model, Fy_model, N, N_mhe, ksim, h, mhe_up, sol_opts, G_ineq, H_eq, wmin = None, wmax = None, vmin = None, vmax = None, ymin = None, ymax = None, xmin = None, xmax = None): 826 | """ 827 | SUMMARY: 828 | It builds the MHE optimization problem 829 | """ 830 | # Extract dimensions 831 | nxv = n+p 832 | nxvw = nxv + n_w 833 | n_opt = N*nxvw + n # total # of variables 834 | nmd = n-nd 835 | 836 | # Define symbolic optimization variables 837 | w_opt = MX.sym("w",n_opt) # w_opt = [x[T],w[T], ... ,x[T+N-1],w[T+N-1]] 838 | 839 | # Get states 840 | X = [w_opt[nxvw*k : nxvw*k+n] for k in range(N+1)] 841 | 842 | # Get output rumor 843 | V = [w_opt[nxvw*k+n : nxvw*k + nxv] for k in range(N)] 844 | 845 | # Get state rumor 846 | W = [w_opt[nxvw*k+nxv : nxvw*k + nxvw] for k in range(N)] 847 | 848 | idx = N_mhe if N_mhe == 1 else N_mhe-1 849 | 850 | # Define parameters U,Y,x_bar,P_k_r 851 | par = MX.sym("par", N*m+N*p+n+n*n+N+(p*(idx))**2+p*(idx)+p*(idx)*n+(p+nmd)*N) 852 | U = [par[m*k : m*k+m] for k in range(N)] 853 | Y = [par[N*m + p*k : N*m + p*k+p] for k in range(N)] 854 | x_bar = par[N*(m+p):N*(m+p)+n] 855 | P_k_inv_r = par[N*(m+p)+n:N*(m+p)+n+n*n] 856 | t = par[N*(m+p)+n+n*n:N*(m+p)+n+n*n+N] 857 | Pycondx_inv_r = par[N*(m+p)+n+n*n+N:N*(m+p)+n+n*n+N+(p*(idx))**2] 858 | Hbig = par[N*(m+p)+n+n*n+N+(p*(idx))**2:N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx)] 859 | Obig_r = par[N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx):N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx)+p*(idx)*n] 860 | PX = [par[N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx)+p*(idx)*n + npx*k : N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx)+p*(idx)*n+ npx*k+npx] for k in range(N)] 861 | PY = [par[N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx)+p*(idx)*n +npx*N + npy*k : N*(m+p)+n+n*n+N+(p*(idx))**2+p*(idx)+p*(idx)*n+ N*npx + npy*k+npy] for k in range(N)] 862 | 863 | Pycondx_inv = Pycondx_inv_r.reshape((p*(idx),p*(idx))) 864 | Obig = Obig_r.reshape((p*(idx),n)) 865 | 866 | P_k_inv = P_k_inv_r.reshape((n,n)) #shaping P_k vector in order to reconstruct the matrix 867 | 868 | # Defining bound constraint 869 | if ymin is None and ymax is None: 870 | yFree = True 871 | else: 872 | yFree = False 873 | if ymin is None: 874 | ymin = -DM.inf(p) 875 | if ymax is None: 876 | ymax = DM.inf(p) 877 | if xmin is None: 878 | xmin = -DM.inf(n) 879 | if xmax is None: 880 | xmax = DM.inf(n) 881 | if wmin is None: 882 | wmin = -DM.inf(n_w) 883 | if wmax is None: 884 | wmax = DM.inf(n_w) 885 | if vmin is None: 886 | vmin = -DM.inf(p) 887 | if vmax is None: 888 | vmax = DM.inf(p) 889 | 890 | if h is None: 891 | h = .1 #Defining integrating step if not provided from the user 892 | 893 | 894 | if G_ineq != None: 895 | g_ineq = G_ineq(xSX,uSX,dSX,tSX,pxSX) 896 | G_ineqSX = Function('G_ineqSX', [xSX,uSX,dSX,tSX,pxSX], [g_ineq]) 897 | 898 | if H_eq != None: 899 | h_eq = H_eq(xSX,uSX,dSX,tSX,pxSX) 900 | H_eqSX = Function('H_eqSX', [xSX,uSX,dSX,tSX,pxSX], [h_eq]) 901 | 902 | # Initializing constraints vectors and obj fun 903 | g = [] 904 | g1 = [] # Costraint vector for y bounds 905 | g4 = [] # User defined inequality constraints 906 | g5 = [] # User defined equality constraints 907 | 908 | f_obj = 0.0; 909 | 910 | for k in range(N): 911 | Y_k = Fy_model( X[k],U[k], t[k], PY[k]) + V[k] 912 | 913 | if G_ineq != None: 914 | G_k = G_ineqSX(X[k], U[k], d, t, p_xk[:,k]) 915 | else: 916 | G_k = [] 917 | if H_eq != None: 918 | H_k = H_eqSX(X[k], U[k], d, t, p_xk[:,k]) 919 | else: 920 | H_k = [] 921 | 922 | g4.append(G_k) 923 | g5.append(H_k) 924 | 925 | if yFree is False: 926 | g1.append(Y_k) #bound constraint on Y_k 927 | 928 | g.append(Y_k - Y[k]) 929 | 930 | X_next = Fx_model( X[k], U[k], h, t[k], W[k], PX[k]) 931 | 932 | g.append(X_next - X[k+1]) 933 | 934 | f_obj_new = F_obj_mhe( W[k], V[k], t[k] ) 935 | 936 | f_obj += f_obj_new 937 | 938 | 939 | g = vertcat(*g) 940 | g1 = vertcat(*g1) #bound constraint on Y_k 941 | g4 = vertcat(*g4) 942 | g5 = vertcat(*g5) 943 | 944 | v_in = 0.5*xQx((X[0]-x_bar) ,P_k_inv) 945 | f_obj += v_in #adding the prior weight 946 | 947 | 948 | ## Subtracting repeated terms from the prior weight when using smoothing update 949 | if mhe_up == 'smooth' and ksim >= N_mhe: 950 | yes = vertcat(*Y[0:N_mhe-1])-mtimes(Obig,X[0])-Hbig 951 | v_s = 0.5*xQx(yes ,Pycondx_inv) 952 | f_obj += -v_s 953 | 954 | #Defining bound constraint 955 | w_lb = -DM.inf(n_opt) 956 | w_ub = DM.inf(n_opt) 957 | w_lb[0:n] = xmin 958 | w_ub[0:n] = xmax 959 | 960 | ng = g.size1() 961 | ng1 = g1.size1() 962 | ng = g.size1() 963 | ng4 = g4.size1() 964 | ng5 = g5.size1() 965 | g_lb = DM.zeros(ng+ng1+ng4+ng5,1) 966 | g_ub = DM.zeros(ng+ng1+ng4+ng5,1) 967 | 968 | if ng4 != 0: 969 | g_lb[ng+ng1+ng2:ng+ng1+ng2+ng4] = -DM.inf(ng4) 970 | 971 | for k in range(1,N+1,1): 972 | w_lb[nxvw*k : nxvw*k+n] = xmin 973 | w_ub[nxvw*k : nxvw*k+n] = xmax 974 | w_lb[nxvw*k-p-n_w : nxvw*k-n_w] = vmin 975 | w_ub[nxvw*k-p-n_w : nxvw*k-n_w] = vmax 976 | w_lb[nxvw*k-n_w : nxvw*k] = wmin 977 | w_ub[nxvw*k-n_w : nxvw*k] = wmax 978 | 979 | 980 | if yFree is False: 981 | g_lb[ng+(k-1)*p: ng+k*p] = ymin + 0.5*ymin 982 | g_ub[ng+(k-1)*p: ng+k*p] = ymax + 0.5*ymax 983 | 984 | g = vertcat(g, g1, g4, g5) 985 | 986 | nlp = {'x':w_opt, 'p':par, 'f':f_obj, 'g':g} 987 | 988 | solver = nlpsol('solver', 'ipopt', nlp, sol_opts) 989 | 990 | return [solver, w_lb, w_ub, g_lb, g_ub] 991 | --------------------------------------------------------------------------------