├── .gitattributes ├── Fast_MPC ├── Fast_MPC2.m ├── backtracking_inf_newton.m ├── fast_mpc_eq_const.m ├── fast_mpc_ineq_const.m ├── fast_mpc_init.m ├── fast_mpc_objective.m ├── finite_difference_jacob.m ├── inf_newton_KKT_H.m ├── inf_newton_solver.m ├── inf_nw_test.m └── linesearch_inf_nw.m ├── README.md └── test_fast_mpc.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /Fast_MPC/Fast_MPC2.m: -------------------------------------------------------------------------------- 1 | classdef Fast_MPC2 2 | properties 3 | Q 4 | R 5 | S 6 | q 7 | r 8 | Qf 9 | qf 10 | x_min 11 | x_max 12 | u_min 13 | u_max 14 | T 15 | x0 16 | A 17 | B 18 | w 19 | x_final 20 | x_init 21 | end 22 | methods 23 | function cs = Fast_MPC2(Q,R,S,Qf,q,r,qf,xmin,xmax,umin,umax,T,x0,... 24 | A,B,w,xf,x_init) 25 | if nargin > 1 26 | cs.Q = Q; 27 | cs.R = R; 28 | cs.S = S; 29 | cs.Qf = Qf; 30 | cs.q = q; 31 | cs.r = r; 32 | cs.qf = qf; 33 | cs.x_min = xmin; 34 | cs.x_max = xmax; 35 | cs.u_min = umin; 36 | cs.u_max = umax; 37 | cs.T = T; 38 | cs.x0 = x0; 39 | cs.A = A; 40 | cs.B = B; 41 | cs.w = w; 42 | cs.x_final = xf; 43 | cs.x_init = x_init; 44 | end 45 | end 46 | function [H,g] = objective_function(obj) 47 | [H,g] = fast_mpc_objective(obj); 48 | end 49 | function [P,h] = inequality_const(obj) 50 | [P,h] = fast_mpc_ineq_const(obj); 51 | end 52 | function [C,b] = equality_const(obj) 53 | [C,b] = fast_mpc_eq_const(obj); 54 | end 55 | function [z_init] = initialize(obj) 56 | z_init = fast_mpc_init(obj); 57 | end 58 | function [J,A_eq,b_eq] = fomulate_mpc(obj,k) 59 | [P,h] = inequality_const(obj); 60 | [H,g] = objective_function(obj); 61 | J = @(z)(z'*H*z + g'*z + k*(-sum(log(h - P*z)))); 62 | 63 | [A_eq,b_eq] = equality_const(obj); 64 | 65 | end 66 | function [x_opt] = matlab_solve(obj) 67 | k = 1; 68 | mu = 1/10; 69 | z_init = initialize(obj); 70 | options = optimoptions(@fmincon,'Algorithm','interior-point','Display','off'); 71 | while k*length(z_init) >= 10e-3 72 | [J,A_eq,b_eq] = fomulate_mpc(obj,k); 73 | x_opt = fmincon(J,z_init,[],[],A_eq,b_eq,[],[],[],options); 74 | z_init = x_opt; 75 | k = mu*k; 76 | end 77 | end 78 | function [x_opt] = mpc_solve_check(obj,k_min,k_max) 79 | k = linspace(k_max, k_min,5); 80 | [z] = initialize(obj); 81 | [H,g] = objective_function(obj); 82 | [P,h] = inequality_const(obj); 83 | [C,b] = equality_const(obj); 84 | nw = []; 85 | for i=1:length(k) 86 | x_opt = inf_newton_solver(H,g,P,h,C,b,k(i),z,nw); 87 | z = x_opt; 88 | end 89 | end 90 | function [x_opt] = mpc_solve_full(obj) 91 | k = 1; 92 | mu = 1/10; 93 | [H,g] = objective_function(obj); 94 | [P,h] = inequality_const(obj); 95 | [C,b] = equality_const(obj); 96 | [z] = initialize(obj); 97 | nw = []; 98 | while k*length(z) >= 10e-3 99 | % fprintf('log barrier k = %d\n',k); 100 | x_opt = inf_newton_solver(H,g,P,h,C,b,k,z,nw); 101 | k = mu*k; 102 | z = x_opt; 103 | end 104 | 105 | end 106 | function [x_opt] = mpc_fixed_log(obj,k) 107 | nw = []; 108 | [z] = initialize(obj); 109 | [H,g] = objective_function(obj); 110 | [P,h] = inequality_const(obj); 111 | [C,b] = equality_const(obj); 112 | x_opt = inf_newton_solver(H,g,P,h,C,b,k,z,nw); 113 | end 114 | function [x_opt] = mpc_fixed_log_newton(obj,nw,k) 115 | [z] = initialize(obj); 116 | [H,g] = objective_function(obj); 117 | [P,h] = inequality_const(obj); 118 | [C,b] = equality_const(obj); 119 | x_opt = inf_newton_solver(H,g,P,h,C,b,k,z,nw); 120 | end 121 | function [x_opt] = mpc_fixed_newton(obj,nw) 122 | k = 1; 123 | mu = 1/10; 124 | [H,g] = objective_function(obj); 125 | [P,h] = inequality_const(obj); 126 | [C,b] = equality_const(obj); 127 | [z] = initialize(obj); 128 | while k*length(z) >= 10e-3 129 | % fprintf('log barrier k = %d\n',k); 130 | x_opt = inf_newton_solver(H,g,P,h,C,b,k,z,nw); 131 | k = mu*k; 132 | z = x_opt; 133 | end 134 | end 135 | end 136 | end -------------------------------------------------------------------------------- /Fast_MPC/backtracking_inf_newton.m: -------------------------------------------------------------------------------- 1 | function [z,nu] = backtracking_inf_newton(z,nu,del_z,del_nu,rp,rd,al,bt) 2 | t = 1; 3 | too_many_steps_counter = 1000; 4 | while norm([rp(z + t*del_z);rd(z+t*del_z,nu+t*del_nu)],2) > (1-al*t)*norm([rp(z);rd(z,nu)],2) 5 | t = bt*t; 6 | if too_many_steps_counter == 0 7 | error('line search fail - took too many iterations'); 8 | end 9 | end 10 | z = z + t*del_z; 11 | nu = nu + t*del_nu; 12 | 13 | end -------------------------------------------------------------------------------- /Fast_MPC/fast_mpc_eq_const.m: -------------------------------------------------------------------------------- 1 | function [C,b] = fast_mpc_eq_const(obj) 2 | 3 | %% Parameters 4 | A = obj.A; 5 | B = obj.B; 6 | w = obj.w; 7 | n = size(A,2); 8 | m = size(B,2); 9 | x = obj.x0; 10 | u = obj.R; 11 | T = obj.T; 12 | C = zeros(T*n,T*(n+m)); 13 | b = zeros(T*n,1); 14 | xf = obj.x_final; 15 | 16 | %% Initial condition check 17 | if isempty(A) 18 | error('Define the state dynamics/equality constrained matrix'); 19 | elseif isempty(B) 20 | error('Define the control dynamics/equality constrained matrix'); 21 | end 22 | 23 | if size(A,2) ~= size(x,1) 24 | error('The equality state dynamics matrix size does not match'); 25 | elseif size(B,2) ~= size(u,2) 26 | error('The equality control dynamics matrix size does not match'); 27 | elseif isempty(w) 28 | w = zeros(n,1); 29 | end 30 | 31 | %% Equality constraint construction 32 | 33 | for i=n:n:T*n-n+1 34 | if i==n 35 | C(i+1:i+n,i+((i/n)-1)*(n+m+n):i+n+((i/n)-1)*(n+m+n)+m+n-1) = [-A -B eye(n)]; 36 | b(i+1:i+n) = w; 37 | else 38 | C(i+1:i+n,((i/n)-1)*(n+m)+m+1:((i/n)-1)*(n+m)+m+m+n+n) = [-A -B eye(n)]; 39 | b(i+1:i+n) = w; 40 | end 41 | end 42 | C(1:n,1:m+n) = [-B eye(n)]; 43 | % C(end-n+1:end,end-(n+m+n)+1:end) = [zeros(n,n) zeros(n,m) eye(n)]; 44 | b(1:n) = A*x + w; 45 | % b(end-n+1:end) = xf; 46 | if isempty(xf)~=1 47 | b = [b;xf]; 48 | C = [C;zeros(n,size(C,2))]; 49 | end 50 | C(end-n+1:end,end-n+1:end) = eye(n); 51 | end -------------------------------------------------------------------------------- /Fast_MPC/fast_mpc_ineq_const.m: -------------------------------------------------------------------------------- 1 | function [P,h] = fast_mpc_ineq_const(obj) 2 | 3 | %% Initial condition check 4 | if (size(obj.x_min,1)~=size(obj.Q,1)) || (size(obj.x_max,1)~=size(obj.Q,1)) 5 | error('Check the state inequality constraints dimensions'); 6 | end 7 | if (size(obj.u_min,1)~=size(obj.R,1)) || (size(obj.u_max,1)~=size(obj.R,1)) 8 | error('Check cotrol iequality constraint dimension'); 9 | end 10 | 11 | %% Parameters 12 | x_min = obj.x_min; 13 | x_max = obj.x_max; 14 | u_min = obj.u_min; 15 | u_max = obj.u_max; 16 | T = obj.T; 17 | n = size(x_min,1); 18 | m = size(u_min,1); 19 | x = obj.x0; 20 | 21 | %% Inequality constraint construction 22 | P = zeros(2*T*(n+m),T*(n+m)); 23 | h = zeros(2*T*(n+m),1); 24 | 25 | for i=1:2*(m+n):size(P,1)-2*(m+n)+1 26 | if i==1 27 | P(i:i+2*(m+n)-1,i:i+(m+n)-1) = [eye(m) zeros(m,n);(-eye(m)) zeros(m,n);... 28 | zeros(n,m) eye(n); zeros(n,m) -(eye(n))]; 29 | else 30 | P(i:i+2*(m+n)-1,(i+1)/2:(i+1)/2+(m+n)-1) = [eye(m) zeros(m,n);(-eye(m)) zeros(m,n);... 31 | zeros(n,m) eye(n); zeros(n,m) -(eye(n))]; 32 | end 33 | end 34 | 35 | for i=1:2*(m+n):2*T*(m+n)-2*(m+n)+1 36 | h(i:2*(m+n)+i-1) = [u_max;-u_min;x_max;-x_min]; 37 | end 38 | 39 | end -------------------------------------------------------------------------------- /Fast_MPC/fast_mpc_init.m: -------------------------------------------------------------------------------- 1 | function [z_init] = fast_mpc_init(obj) 2 | %% Parameters 3 | T = obj.T; 4 | n = size(obj.Q,1); 5 | m = size(obj.R,1); 6 | x_min = obj.x_min; 7 | x_max = obj.x_max; 8 | u_min = obj.u_min; 9 | u_max = obj.u_max; 10 | 11 | %% Initialization step 12 | if isempty(obj.x_init) ~= 1 13 | if size(obj.x_init,1) ~= T*(n+m) 14 | error('Initialization size mismatch (T*(n+m))'); 15 | else 16 | z_init = obj.x_init; 17 | end 18 | else 19 | x_init = (x_min + x_max)/2; 20 | u_init = (u_min + u_max)/2; 21 | z_init = zeros(T*(m+n),1); 22 | for i = 1:(m+n):T*(m+n)-(m+n)+1 23 | z_init(i:i+m-1,1) = u_init; 24 | z_init(i+m:i+m+n-1,1) = x_init; 25 | end 26 | 27 | end 28 | end -------------------------------------------------------------------------------- /Fast_MPC/fast_mpc_objective.m: -------------------------------------------------------------------------------- 1 | function [H,g] = fast_mpc_objective(obj) 2 | %% Parameters 3 | T = obj.T; % Horizon time 4 | n = size(obj.Q,1); % State vector dimension 5 | m = size(obj.R,1); % Control vector dimension 6 | z = zeros(T*(n+m),1); 7 | 8 | R = obj.R; 9 | Q = obj.Q; 10 | Qf = obj.Qf; 11 | q = obj.q; 12 | r = obj.r; 13 | qf = obj.qf; 14 | x = obj.x0; 15 | 16 | %% Checking initial conditions 17 | if (size(obj.Q,1) ~= size(obj.Q,2)) ||... 18 | (size(obj.Qf,1) ~= size(obj.Qf,2)) 19 | error('State stage cost must a square matrix'); 20 | elseif size(obj.R,1) ~= size(obj.R,2) 21 | error('Control stage cost must a square matrix'); 22 | end 23 | 24 | 25 | % Checking state and control linear stage cost size 26 | if isempty(obj.q) == 0 27 | if (size(obj.q,1) ~= size(obj.Q,1)) 28 | error('Linear state cost needs to be a vector of size n'); 29 | end 30 | else 31 | q = zeros(size(Q,1),1); 32 | end 33 | 34 | if isempty(obj.r) == 0 35 | if (size(obj.r,1) ~= size(obj.R,1)) 36 | error('Linear control cost needs to be a vector of size n'); 37 | end 38 | else 39 | r = zeros(size(R,1),1); 40 | end 41 | if isempty(obj.qf) == 0 42 | if (size(obj.qf,1) ~= size(obj.Q,1)) 43 | error('State terminal linear cost needs to be a vector of size n'); 44 | end 45 | else 46 | qf = zeros(size(Q,1),1); 47 | end 48 | 49 | %% Objective construction 50 | H = zeros(T*(n+m),T*(n+m)); 51 | for i=m+1:(n+m):size(H,1)-n 52 | H(i:i+(n+m)-1,i:i+(n+m)-1) = [Q zeros(n,m); zeros(m,n) R]; 53 | end 54 | H(1:m,1:m) = R; 55 | H(end-n+1:end,end-n+1:end) = Qf; 56 | 57 | g = zeros(T*(n+m),1); 58 | for i=m+1:(n+m):size(g,1) 59 | if i == T*(n+m) -n+1 60 | g(end-n+1:end) = qf; 61 | else 62 | g(i:i+(n+m)-1) = [q;r]; 63 | end 64 | end 65 | g(1:m) = r; 66 | 67 | end -------------------------------------------------------------------------------- /Fast_MPC/finite_difference_jacob.m: -------------------------------------------------------------------------------- 1 | function [f0, J] = finite_difference_jacob( fun, x0 ) 2 | % make sure x0 is a column vector 3 | [Nx,cols] = size(x0); 4 | if cols ~= 1 5 | error('x0 needs to be a column vector'); 6 | end 7 | 8 | % make sure fun returns a column vector 9 | f0 = fun(x0); 10 | [Nf,cols] = size(f0); 11 | if cols ~= 1 12 | error('fun needs to return a column vector'); 13 | end 14 | 15 | % initialize empty J 16 | J = zeros(Nf, Nx); 17 | 18 | % perform the finite difference jacobian evaluation 19 | h = 1e-6; 20 | for k = 1:Nx 21 | x = x0; 22 | x(k) = x(k) + h; 23 | f = fun(x); 24 | grad = (f - f0)/h; 25 | J(:,k) = grad; 26 | end 27 | 28 | % % initialize empty J 29 | % J = zeros(Nf, Nx); 30 | % h = 1e-15; 31 | % for k = 1:Nx 32 | % x = x0; 33 | % x(k) = x(k) + 1i*h; 34 | % f = fun(x); 35 | % grad = imag(f)/h; 36 | % J(:,k) = grad; 37 | % end 38 | -------------------------------------------------------------------------------- /Fast_MPC/inf_newton_KKT_H.m: -------------------------------------------------------------------------------- 1 | function [H,d] = inf_newton_KKT_H(H,P,h,z,k) 2 | 3 | d_inv = (h - P*z); 4 | n = length(d_inv); 5 | D = zeros(n,n); 6 | 7 | for i=1:n 8 | D(i,i) = (1/(d_inv(i)))^2; 9 | end 10 | % D = diag((d_inv).^-2); 11 | 12 | d = 1./d_inv; 13 | H = 2*H + k*P'*D*P; 14 | 15 | end -------------------------------------------------------------------------------- /Fast_MPC/inf_newton_solver.m: -------------------------------------------------------------------------------- 1 | function x_opt = inf_newton_solver(H,g,P,h,C,b,k,z,newton) 2 | nu = rand(length(b),1); 3 | iter = 1; 4 | if isempty(newton) 5 | max_iter = 1000; 6 | else 7 | max_iter = newton; 8 | end 9 | tol = 1e-6; 10 | for i=1:max_iter 11 | [KKT_H,d] = inf_newton_KKT_H(H,P,h,z,k); 12 | rd =@(z,v)(2*H*z + g + k*P'*d + C'*v); 13 | rp =@(z)(C*z - b); 14 | norm_grad = @(x,nu)(norm([-rd(x,nu);-rp(x)],2)); 15 | tol_g = C*z-b; 16 | n_r = norm_grad(z,nu); 17 | n_g = norm(tol_g); 18 | % fprintf('iteration: %4d, norm_residual: %.2e \n',iter, n_r); 19 | if ( n_r <= tol) && (n_g <=1e-8) 20 | x_opt = z; 21 | return; 22 | end 23 | 24 | L = chol(KKT_H,'lower'); 25 | optsL.LT = true; 26 | optsU.UT = true; 27 | Schur = C*linsolve(L',linsolve(L,C',optsL),optsU); 28 | phi_inv_rd = linsolve(L',linsolve(L,rd(z,nu),optsL),optsU); 29 | Beta = -rp(z) + C*phi_inv_rd; 30 | SL = chol(Schur,'lower'); 31 | int_nu = linsolve(SL,-Beta,optsL); 32 | del_nu = linsolve(SL',int_nu,optsU); 33 | 34 | int_z = linsolve(L,-rd(z,nu)-C'*del_nu,optsL); 35 | del_z = linsolve(L',int_z,optsU); 36 | al = 10^-4; 37 | bt = 0.5; 38 | [z,nu] = backtracking_inf_newton(z,nu,del_z,del_nu,rp,rd,al,bt); 39 | iter = iter + 1; 40 | 41 | end 42 | x_opt = z; 43 | end -------------------------------------------------------------------------------- /Fast_MPC/inf_nw_test.m: -------------------------------------------------------------------------------- 1 | function [H,del_f] = inf_nw_test(z) 2 | n = length(z); 3 | H = zeros(n,n); 4 | for i=1:length(z) 5 | H(i,i) = (1/z(i)); 6 | end 7 | 8 | del_f = zeros(n,1); 9 | for i=1:n 10 | del_f(i) = log(z(i)) + 1; 11 | end 12 | 13 | end -------------------------------------------------------------------------------- /Fast_MPC/linesearch_inf_nw.m: -------------------------------------------------------------------------------- 1 | function [x,nu] = linesearch_inf_nw(r,x,nu,del_x,del_nu) 2 | t = 1; 3 | al = 0.01; 4 | beta = 0.5; 5 | too_many_steps_counter = 1000; 6 | while (r(x+t*del_x,nu+t*del_nu)) > (1-al*t)*r(x,nu) 7 | t = beta*t; 8 | too_many_steps_counter = too_many_steps_counter - 1; 9 | if too_many_steps_counter == 0 10 | error('line search fail - took too many iterations'); 11 | end 12 | end 13 | x = x + t*del_x; 14 | nu = nu + t*del_nu; 15 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fast MPC (Model predictive control) 2 | This repository contains matlab interface to convert a standard model predictive control to fast model predictive control based on the paper [Fast Model predictive control using online optimization]. The fast MPC class solves using a custom built **infeasible start newton solver** exploiting the structure of MPC. 3 | 4 | [Fast Model predictive control using online optimization]:http://stanford.edu/~boyd/papers/pdf/fast_mpc.pdf 5 | 6 | ## Description 7 | In the conventional method, a MPC problem is solved and the first control step is applied to the system, and the next integrated state forms the initial condition for the next MPC iteration. Here the structure of the MPC is exploited for accelerated results. The current implementation is performed on time invariant system dynamics (equality constraints) 8 | 9 | ![](http://latex.codecogs.com/gif.latex?%5Cbegin%7Beqnarray*%7D%20%5Cmin_%7Bx%28%5Ccdot%29%2Cu%28%5Ccdot%29%7D%20%26%20%26%20l_%7Bf%7D%28x%28t+T%29%29%20+%20%5Csum_%7Bn%3D%5Ctau%7D%5E%7B%5Ctau+T-1%7D%20l%28%20x%28%5Ctau%29%2Cu%28%5Ctau%29%20%5C%2C%5C%5C%20%5Ctextrm%7Bs.t.%7D%20%26%20%26%20x%28t+1%29%20%5C%3B%20%3D%20%5C%3B%20A%28%5C%2Cx%28t%29%29+B%28%5C%2Cu%28t%29%5C%2C%29%20+%20%5Chat%7Bw%7D%5C%5C%20%26%20%26%20x%280%29%20%5C%3B%20%3D%20%5C%3B%20x_0%2C%5C%5C%20%26%20%26%20%5Cunderline%20u%28t%29%20%5C%3B%20%5Cleq%20%5C%3B%20u%28t%29%20%5C%3B%20%5Cleq%20%5C%3B%20%5Coverline%20u%28t%29%2C%5C%5C%20%26%20%26%20%5Cunderline%20x%28t%29%20%5C%3B%20%5Cleq%20%5C%3B%20x%28t%29%20%5C%3B%20%5Cleq%20%5C%3B%20%5Coverline%20x%28t%29%2C%20%5Cquad%20%5Ctextrm%7Bfor%20all%7D%20%5C%3B%20%5C%2C%20t%20%5Cin%20%5B0%2CT%5D%5C%3B%20%5Cend%7Beqnarray*%7D) 10 | 11 | Where, 12 | 13 | ![](http://latex.codecogs.com/gif.latex?%24%24%20l_%7Bf%7D%28x%28t+T%29%29%20%3D%20x%28t+T%29%5E%5Cintercal%20Q_%7Bf%7Dx%28t+T%29%20+%20q_%7Bf%7D%5E%5Cintercal%20x%28t+T%29%20%24%24) 14 | 15 | 16 | ![](http://latex.codecogs.com/gif.latex?l%28x%28%5Ctau%29%2Cu%28%5Ctau%29%29%20%3D%20%5Cbegin%7Bbmatrix%7Dx%28t%29%20%26%20u%28t%29%20%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7DQ%20%26%20S%5E%5Cintercal%20%5C%5C%20S%20%26%20R%20%5Cend%7Bbmatrix%7D%5Cbegin%7Bbmatrix%7Dx%28t%29%20%5C%5C%20u%28t%29%20%5Cend%7Bbmatrix%7D%20+%20q%5E%5Cintercal%20x%28t%29%20+%20r%5E%5Cintercal%20u%28t%29) 17 | 18 | The above mentioned problem is converted into the following structure 19 | 20 | 21 | ![](http://latex.codecogs.com/gif.latex?minimize%20%5Cquad%20z%5E%5Cintercal%20Hz%20+%20g%5E%5Cintercal%20z%20+%20k%5Cphi%28z%29) 22 | ![](http://latex.codecogs.com/gif.latex?subject%20%5C%20to%20%5Cquad%20Cz%20%3D%20b) 23 | 24 | where, 25 | ![](http://latex.codecogs.com/gif.latex?%5Cphi%28z%29%20%3D%20%5Csum_%7Bi%3D1%7D%5E%7BlT+k%7D%20-log%28h_%7Bi%7D%20-%20p_%7Bi%7D%5E%5Cintercal%20z%29) 26 | 27 | The details of the matrices and structure are present in the paper. 28 | 29 | **Upcoming updates will include non-linear fast MPC along with the inclusions of integrators** 30 | 31 | 32 | ## Getting Started 33 | 34 | Clone or download the repository. Various inputs are needed to the FAST MPC class which are explained as follows. 35 | 36 | Q - State stage cost 37 | 38 | R - Control stage cost 39 | 40 | S - State control coupled cost 41 | 42 | Qf - Terminal state cost 43 | 44 | q - linear state cost 45 | 46 | r - linear control cost 47 | 48 | qf - terminal state linear cost 49 | 50 | x_min - lower bound on state 51 | 52 | x_max - upper bound on state 53 | 54 | u_min - lower bound on control 55 | 56 | u_max - upper bound on control 57 | 58 | T - Horizon length 59 | 60 | x0 - initial state 61 | 62 | A - State transition matrix 63 | 64 | B - Control matrix 65 | 66 | w - distrubance vector 67 | 68 | xf - final state 69 | 70 | x_init - initialization(optional) (Note: if you intend to provide your own initialization, then the entire vector length needs to be provided i.e., T*(n+m)) 71 | 72 | 73 | A constructor is also present for the class and the cronological order is as mentioned above. 74 | 75 | ### Prerequisites 76 | 77 | Matlab 2010 or higher 78 | 79 | 80 | ## Running the tests 81 | 82 | The test_fast_mpc.m contains a random system. It also compares the native matlab solver and various fast MPC methods. 83 | 84 | 85 | ## Authors 86 | 87 | * **Sandeep Banik** - [Projects](https://github.com/sandeepbanik) 88 | 89 | ## Reference 90 | 91 | **Fast Model Predictive Control Using Online Optimization** by Yang Wang and Stephen Boyd 92 | 93 | -------------------------------------------------------------------------------- /test_fast_mpc.m: -------------------------------------------------------------------------------- 1 | % Testing FAST MPC class 2 | clear; 3 | clc; 4 | close all; 5 | addpath('Fast_MPC'); 6 | %% Parameters 7 | 8 | n = 8; % Dimension of state 9 | m = 5; % Dimension of control 10 | Q = eye(n); % State stage cost 11 | R = eye(m); % Control stage cost 12 | S = []; % State control coupled cost 13 | Qf = 50*eye(n); % Terminal state cost 14 | q = []; % Linear state cost 15 | r = []; % Linear control cost 16 | qf = []; % Terminal state cost 17 | Xmax = 10; % State upper limit 18 | Umax = 2; % Control upper limit 19 | xmin = -Xmax*ones(n,1); % State lower bound 20 | xmax = Xmax*ones(n,1); % State upper bound 21 | umin = -Umax*ones(m,1); % Cotrol lower bound 22 | umax = Umax*ones(m,1); % Control upper bound 23 | 24 | high_limit = 1; 25 | low_limit = 0; 26 | A = (high_limit-low_limit).*rand(n,n) + ones(n,n)*low_limit; % Random A (State transition) matrix 27 | B = (high_limit-low_limit).*rand(n,m) + ones(n,m)*low_limit; % Random B (Control matrix) matrix 28 | 29 | A = A./(max(abs(eig(A)))); % Spectral radius of A within 1 30 | 31 | high_limit_w = 1; 32 | low_limit_w = 0; 33 | w = (high_limit_w-low_limit_w).*rand(n,1) + ones(n,1)*low_limit_w; % Random noise vector 34 | 35 | T = 10; % Horizon length 36 | x0 = rand(n,1); % Initial state (random) 37 | xf = 1*ones(n,1); % Terminal state 38 | test = Fast_MPC2(Q,R,S,Qf,q,r,qf,xmin,xmax,umin,umax,T,x0,... 39 | A,B,w,xf,[]); % Build class 40 | 41 | %% Solve 42 | 43 | % Native matlab solver 44 | tic; 45 | [x_opt_mat] = test.matlab_solve; % Solving using native matlab solver fmincon 46 | t_mat = toc; 47 | 48 | % Structured MPC full solve 49 | tic; 50 | [x_opt_full] = test.mpc_solve_full; % Solving structure problem as log barrier method with infeasible start newton 51 | t_full = toc; 52 | 53 | % Fixed log barrier method k=0.01 54 | k_fix = 0.01; 55 | tic; 56 | [x_opt_log] = test.mpc_fixed_log(k_fix); % Fixed log(k) iteration method 57 | t_log = toc; 58 | 59 | % Fixed newton step = 5 60 | n_fix = 5; 61 | tic; 62 | [x_opt_nw] = test.mpc_fixed_newton(n_fix); % Fixed newton steps(5) method 63 | t_nw = toc; 64 | 65 | % Fixed log barrier + fixed newton step 66 | tic; 67 | [x_opt_lgnw] = test.mpc_fixed_log_newton(n_fix,k_fix); 68 | t_lgnw = toc; 69 | 70 | fprintf('Matlab solver=%d sec\n',t_mat); 71 | fprintf('Infeasible start newton =%d sec\n',t_full); 72 | fprintf('Infeasible start newton with fixed k(%d) =%d sec\n',k_fix,t_log); 73 | fprintf('Infeasible start newton with fixed newton step(%d) =%d sec\n',n_fix,t_nw); 74 | fprintf('Infeasible start newton with fixed newton and barrier =%d sec\n',t_lgnw); 75 | 76 | %% Plotting 77 | 78 | x_mat = zeros(T*n,1); 79 | u_mat = zeros(T*m,1); 80 | for i=1:(m+n):length(x_opt_mat) 81 | if i==1 82 | u_mat(i:i+m-1) = x_opt_mat(i:i+m-1); 83 | x_mat(i:i+n-1) = x_opt_mat(i+m:i+m+n-1); 84 | else 85 | u_mat((i-1)/(m+n)*m+1:(i-1)/(m+n)*m+m) = x_opt_mat(i:i+m-1); 86 | x_mat((i-1)/(m+n)*n+1:(i-1)/(m+n)*n+n) = x_opt_mat(i+m:i+m+n-1); 87 | end 88 | end 89 | 90 | x_full = zeros(T*n,1); 91 | u_full = zeros(T*m,1); 92 | for i=1:(m+n):length(x_opt_full) 93 | if i==1 94 | u_full(i:i+m-1) = x_opt_full(i:i+m-1); 95 | x_full(i:i+n-1) = x_opt_full(i+m:i+m+n-1); 96 | else 97 | u_full((i-1)/(m+n)*m+1:(i-1)/(m+n)*m+m) = x_opt_full(i:i+m-1); 98 | x_full((i-1)/(m+n)*n+1:(i-1)/(m+n)*n+n) = x_opt_full(i+m:i+m+n-1); 99 | end 100 | end 101 | 102 | x_log = zeros(T*n,1); 103 | u_log = zeros(T*m,1); 104 | for i=1:(m+n):length(x_opt_log) 105 | if i==1 106 | u_log(i:i+m-1) = x_opt_log(i:i+m-1); 107 | x_log(i:i+n-1) = x_opt_log(i+m:i+m+n-1); 108 | else 109 | u_log((i-1)/(m+n)*m+1:(i-1)/(m+n)*m+m) = x_opt_log(i:i+m-1); 110 | x_log((i-1)/(m+n)*n+1:(i-1)/(m+n)*n+n) = x_opt_log(i+m:i+m+n-1); 111 | end 112 | end 113 | 114 | x_nw = zeros(T*n,1); 115 | u_nw = zeros(T*m,1); 116 | for i=1:(m+n):length(x_opt_nw) 117 | if i==1 118 | u_nw(i:i+m-1) = x_opt_nw(i:i+m-1); 119 | x_nw(i:i+n-1) = x_opt_nw(i+m:i+m+n-1); 120 | else 121 | u_nw((i-1)/(m+n)*m+1:(i-1)/(m+n)*m+m) = x_opt_nw(i:i+m-1); 122 | x_nw((i-1)/(m+n)*n+1:(i-1)/(m+n)*n+n) = x_opt_nw(i+m:i+m+n-1); 123 | end 124 | end 125 | 126 | x_lgnw = zeros(T*n,1); 127 | u_lgnw = zeros(T*m,1); 128 | for i=1:(m+n):length(x_opt_lgnw) 129 | if i==1 130 | u_lgnw(i:i+m-1) = x_opt_lgnw(i:i+m-1); 131 | x_lgnw(i:i+n-1) = x_opt_lgnw(i+m:i+m+n-1); 132 | else 133 | u_lgnw((i-1)/(m+n)*m+1:(i-1)/(m+n)*m+m) = x_opt_lgnw(i:i+m-1); 134 | x_lgnw((i-1)/(m+n)*n+1:(i-1)/(m+n)*n+n) = x_opt_lgnw(i+m:i+m+n-1); 135 | end 136 | end 137 | 138 | figure(1); 139 | stairs(x_mat);hold on; 140 | stairs(x_full) 141 | stairs(x_log); 142 | stairs(x_nw); 143 | stairs(x_lgnw); 144 | legend('Matlab solver', 'In nw method', 'fixed log, fixed newton', 'fixed log + newton'); 145 | axis tight; 146 | 147 | figure(2); 148 | stairs(u_mat);hold on; 149 | stairs(u_full) 150 | stairs(u_log); 151 | stairs(u_nw); 152 | stairs(u_lgnw); 153 | legend('Matlab solver', 'In nw method', 'fixed log, fixed newton', 'fixed log + newton'); 154 | axis tight; 155 | --------------------------------------------------------------------------------