├── .gitignore ├── D_mult.m ├── Functions ├── D_mult.m ├── calculate_mu_max.m ├── compute_wbar.m ├── controller_PE.m ├── prestab_controller.m └── state_tube.m ├── Plots ├── cont_params.m ├── controller_nom.m ├── model_nom.m ├── nominal_MPC.m ├── plot_script.m └── plotregionLine.m ├── boundedComplexity.m ├── calc_bounds.mat ├── controller.m ├── controller_expl.m ├── controller_expl_pre.m ├── controller_pre.m ├── model.m ├── script.m ├── system_desc.m └── updateParameters.m /.gitignore: -------------------------------------------------------------------------------- 1 | *.asv 2 | calc_bounds.mat 3 | *.eps 4 | Plots/nom_data.mat 5 | -------------------------------------------------------------------------------- /D_mult.m: -------------------------------------------------------------------------------- 1 | function D = D_mult(sys,x,u) 2 | x_mult = reshape(sys.Ap,[sys.n*sys.p,sys.n])*x; 3 | x_mult = reshape(x_mult,[sys.n,sys.p]); 4 | u_mult = reshape(sys.Bp,[sys.n*sys.p,sys.m])*u; 5 | u_mult = reshape(u_mult,[sys.n,sys.p]); 6 | D = x_mult + u_mult; 7 | end -------------------------------------------------------------------------------- /Functions/D_mult.m: -------------------------------------------------------------------------------- 1 | function D = D_mult(sys,x,u) 2 | x_mult = reshape(sys.Ap,[sys.n*sys.p,sys.n])*x; 3 | x_mult = reshape(x_mult,[sys.n,sys.p]); 4 | u_mult = reshape(sys.Bp,[sys.n*sys.p,sys.m])*u; 5 | u_mult = reshape(u_mult,[sys.n,sys.p]); 6 | D = x_mult + u_mult; 7 | end -------------------------------------------------------------------------------- /Functions/calculate_mu_max.m: -------------------------------------------------------------------------------- 1 | function mu = calculate_mu_max(sys) 2 | % 1/mu >= sup_(x,u) ||D(x,u)||^2 3 | % uses the corner points of x and u to get the supremum (approximation). 4 | % The optimization to be solved is concave. Hence this apx is used. 5 | % This gives the maximum value of mu that can be used for the system 6 | 7 | x_test = []; 8 | u_test = []; 9 | D_val = []; 10 | for i = 1:length(sys.Box_x_v) 11 | for j = 1:length(sys.Box_u_v) 12 | x_test = [x_test sys.Box_x_v(:,i)]; 13 | u_test = [u_test sys.Box_u_v(:,j)]; 14 | D_val(i,j) = norm(D_mult(sys, sys.Box_x_v(:,i), sys.Box_u_v(:,j)),2)^2; 15 | end 16 | end 17 | D_max = max(max(D_val)); 18 | 19 | % mu <= 1/D_max 20 | mu = 1/D_max; 21 | end -------------------------------------------------------------------------------- /Functions/compute_wbar.m: -------------------------------------------------------------------------------- 1 | function w_bar = compute_wbar(sys,cont) 2 | %% Computation of w_bar 3 | nHw = size(sys.H_w,1); 4 | nHx = size(cont.H_x,1); 5 | % max Hx_i w, s.t. H_w w <= h_w 6 | % min h_w' li, s.t. H_w' li == Hx_i, li>=0 7 | lam = sdpvar(nHw,nHx,'full'); 8 | Constraints = [lam>=0]; 9 | J = 0; 10 | for i = 1:nHx 11 | Constraints = [Constraints,... 12 | sys.H_w'*lam(:,i)==cont.H_x(i,:)']; 13 | J = J+sys.h_w'*lam(:,i); 14 | end 15 | options = sdpsettings('solver','gurobi','verbose',0); 16 | diagnostics = optimize(Constraints,J,options); 17 | if diagnostics.problem 18 | error(['Calculating w_bar:',diagnostics.info]); 19 | end 20 | 21 | for i = 1:nHx 22 | w_bar(i,1) = value(sys.h_w'*lam(:,i)); 23 | end 24 | 25 | 26 | end -------------------------------------------------------------------------------- /Functions/controller_PE.m: -------------------------------------------------------------------------------- 1 | function u = controller_PE(sys,cont,xk,U_past,PE) 2 | 3 | %% setup optimization problem 4 | 5 | % Declare independent variables 6 | % l goes from 1:N 7 | % j goes from 1:nx_v 8 | z_lk = sdpvar(sys.n,cont.N+1,'full'); 9 | v_lk = sdpvar(sys.m,cont.N,'full'); 10 | alpha_lk = sdpvar(1,cont.N+1,'full'); 11 | Lambda_jlk = sdpvar(cont.nHx,sys.nHtheta,cont.nx_v,cont.N,'full'); 12 | 13 | % define cost variables 14 | x_hat = xk; 15 | u_hat = []; 16 | J = 0; 17 | for l =1:cont.N 18 | u_hat = [u_hat cont.K*x_hat(:,l) + v_lk(:,l)] ; 19 | x_hat = [x_hat cont.A_est*x_hat(:,l) + cont.B_est*u_hat(:,l)]; 20 | J = J + x_hat(:,l)'*cont.Q*x_hat(:,l) + u_hat(:,l)'*cont.R*u_hat(:,l); 21 | end 22 | J = J + x_hat(:,cont.N+1)'*cont.P*x_hat(:,cont.N+1); 23 | 24 | % define state dependent variables 25 | x_jlk = []; 26 | u_jlk = []; 27 | d_jlk = []; 28 | D_jlk = []; 29 | for l = 1:cont.N 30 | x_jlk = cat(3,x_jlk,repmat(z_lk(:,l),1,cont.nx_v)+alpha_lk(:,l)*cont.x_v); 31 | u_jlk = cat(3,u_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_jlk(:,:,l)); 32 | 33 | d_jlk = cat(3,d_jlk,sys.A0*x_jlk(:,:,l)+sys.B0*u_jlk(:,:,l) - repmat(z_lk(:,l+1),1,cont.nx_v)); 34 | D_temp = []; 35 | for j = 1:cont.nx_v 36 | D_temp = cat(3,D_temp,D_mult(sys,x_jlk(:,j,l),u_jlk(:,j,l))); 37 | end 38 | D_jlk = cat(4,D_jlk,D_temp); 39 | end 40 | 41 | % define constraints. 42 | Constraints = [Lambda_jlk(:)>=0]; 43 | Constraints = [Constraints,-cont.H_x*z_lk(:,1) - alpha_lk(:,1)*ones(cont.nHx,1) <= -cont.H_x*xk]; 44 | for l = 1:cont.N 45 | Constraints = [Constraints, ... 46 | (sys.F+sys.G*cont.K)*z_lk(:,l) + sys.G*v_lk(:,l) + alpha_lk(:,l)*cont.f_bar <= ones(sys.nc,1)]; 47 | 48 | for j = 1:cont.nx_v 49 | Constraints = [Constraints, ... 50 | Lambda_jlk(:,:,j,l)*cont.h_theta_k + cont.H_x*d_jlk(:,j,l) - alpha_lk(:,l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 51 | 52 | Constraints = [Constraints, ... 53 | cont.H_x*D_jlk(:,:,j,l) == Lambda_jlk(:,:,j,l)*cont.H_theta]; 54 | end 55 | end 56 | 57 | % define terminal constraints 58 | Constraints = [Constraints, ... 59 | z_lk(:,cont.N+1) == zeros(sys.n,1), ... 60 | cont.h_T*alpha_lk(cont.N+1) <= 1 ]; 61 | 62 | options = sdpsettings('solver','gurobi','verbose',0,'debug',0); 63 | 64 | % persistency of excitation (for scalars) 65 | if sys.m>1 66 | error('Persistency of excitation can be implemented for single input systems only.\n'); 67 | end 68 | 69 | omega_bar_PE = -cont.rho_PE*eye(sys.n); 70 | for k =0:sys.n 71 | omega_bar_PE = omega_bar_PE + U_past(k+1:k+sys.n)*U_past(k+1:k+sys.n)'; 72 | end 73 | 74 | % assert(all(eig(omega_bar_PE)>0),'reduce rho_PE value, or change initialization') 75 | 76 | alpha_PE = 1-U_past(1:sys.n)'*(omega_bar_PE\U_past(1:sys.n)); 77 | beta_PE = 0; 78 | u_phi_vec = 0; 79 | for j = 1:sys.n 80 | beta_PE = beta_PE - U_past(j)*U_past(j+1:j+sys.n)'*(omega_bar_PE\U_past(1:sys.n)); 81 | u_phi_vec = u_phi_vec + U_past(j)*U_past(j+1:j+sys.n); 82 | end 83 | gamma_PE = U_past(1:sys.n)'*U_past(1:sys.n) - cont.rho_PE - u_phi_vec'*(omega_bar_PE\u_phi_vec); 84 | 85 | bounds = roots([alpha_PE,2*beta_PE,gamma_PE]); 86 | bounds = sort(bounds); 87 | try 88 | if alpha_PE < 0 89 | % stay within the bounds 90 | Constraints0 = [Constraints,... 91 | bounds(1) <= u_hat(1) <= bounds(2) ]; 92 | diagnostics = optimize(Constraints0,J,options); 93 | if diagnostics.problem 94 | error('Infeasibility due to PE') 95 | end 96 | u = value(u_hat(:,1)); 97 | else 98 | % stay out of the bounds 99 | Constraints1 = [Constraints,... 100 | u_hat(1) <= bounds(1) ]; 101 | Constraints2 = [Constraints,... 102 | u_hat(1) >= bounds(2) ]; 103 | 104 | 105 | diagnostics1 = optimize(Constraints1,J,options); 106 | if ~diagnostics1.problem 107 | J1 = value(J); 108 | u1 = value(u_hat(:,1)); 109 | else 110 | J1 = Inf; 111 | u1 = [NaN]; 112 | end 113 | 114 | 115 | diagnostics2 = optimize(Constraints2,J,options); 116 | if ~diagnostics2.problem 117 | J2 = value(J); 118 | u2 = value(u_hat(:,1)); 119 | else 120 | J2 = Inf; 121 | u2 = [NaN]; 122 | end 123 | 124 | if diagnostics2.problem && diagnostics1.problem 125 | error('Infeasibility due to PE') 126 | end 127 | 128 | if J11) 44 | error('Gain computed is not stabilizing') 45 | end 46 | end 47 | 48 | % Compute minimum value of alpha 49 | alpha_min = value(alpha_bar); 50 | for k = 1:size(sys.H_theta_v,2) 51 | for j = 1:size(cont.x_v,2) 52 | alpha_min = min(alpha_min,min(cont.w_bar./(cont.vec_1_x-cont.H_x*(Ac(:,:,k)+Bc(:,:,k)*K_val)*cont.x_v(:,j)))); 53 | end 54 | end 55 | 56 | end 57 | 58 | %% LMI with P and K included together 59 | % [Pinv Pinv*Ac(:,:,k)'+Y'*Bc(:,:,k)' Pinv*cont.Q_L Y'*cont.R_L 60 | % Ac(:,:,k)*Pinv+Bc(:,:,k)*Y Pinv zeros(sys.n) zeros(sys.n,sys.m) 61 | % cont.Q_L*Pinv zeros(sys.n) gamma*eye(sys.n) zeros(sys.n,sys.m) 62 | % cont.R_L*Y zeros(sys.m,sys.n) zeros(sys.m,sys.n) gamma*eye(sys.m) 63 | % ] >= 0]; -------------------------------------------------------------------------------- /Functions/state_tube.m: -------------------------------------------------------------------------------- 1 | function X0 = state_tube(sys,cont) 2 | % Construct initial tube shape for the system 3 | 4 | % We start with a symmetric shape: ||x||<=1, and then construct output 5 | % admissible sets for 2-3 time steps. This should improve performance over 6 | % the initial set, without a complicated set description 7 | 8 | warning('Function hardcoded for n = 2'); 9 | assert(sys.n==2); 10 | 11 | % initial constraint set 12 | A_nom = sys.A0;%+sys.B0*cont.K; 13 | 14 | nSteps = 10; 15 | A_temp = sys.Box_x; 16 | b_temp = sys.box_x; 17 | for i = 1:nSteps 18 | 19 | % shrink the disturbance space 20 | for j = 1:length(sys.box_x) 21 | 22 | cvx_begin quiet 23 | variable w(sys.n,1) 24 | variable J 25 | 26 | maximize J 27 | subject to 28 | J == sys.Box_x(j,:)*A_nom^i*w; 29 | sys.H_w*w <= sys.h_w; 30 | 31 | cvx_end 32 | 33 | max_val(j,i) = J; 34 | clear J w 35 | end 36 | A_temp = [A_temp ; sys.Box_x*A_nom^i]; 37 | b_temp = [b_temp ; sys.box_x-sum(max_val(:,1:i),2)]; 38 | end 39 | 40 | figure; plotregion(-A_temp,-b_temp); 41 | X0.A = A_temp; 42 | X0.b = b_temp; 43 | 44 | % Need to compute vertices of X0 45 | 46 | 47 | end -------------------------------------------------------------------------------- /Plots/cont_params.m: -------------------------------------------------------------------------------- 1 | function cont = cont_params(sys) 2 | % prediction horizon 3 | cont.N = 8; 4 | 5 | % cost matrices 6 | cont.Q = eye(sys.n); 7 | cont.R = eye(sys.m); 8 | cont.P = [2.8 0.56; 9 | 0.56 2.5]; 10 | cont.Q_L = chol(cont.Q); 11 | cont.R_L = chol(cont.R); 12 | 13 | 14 | % block size for updating h_theta_k 15 | cont.blk = 10; 16 | 17 | % Define state tube shape : H_x*x <= vec_1_x, vertices: x_v(:,i) 18 | cont.H_x = [eye(sys.n); -eye(sys.n)]; 19 | cont.vec_1_x = ones(length(cont.H_x),1); 20 | cont.nHx = size(cont.H_x,1); % denoted u in Lorenzen(2019) 21 | cont.x_v = [1 1; 1 -1; -1 1; -1 -1 ]'; 22 | cont.nx_v = length(cont.x_v); % number of vertices 23 | 24 | % prestabilizing gain 25 | cont.w_bar = compute_wbar(sys,cont); 26 | [cont.K,cont.alpha_bar,cont.alpha_min] = prestab_controller(sys,cont); 27 | 28 | % Define terminal constraint: z_N|k = 0; h_T*alpha_N|k <= 1; 29 | cont.f_bar = max((sys.F+sys.G*cont.K)*cont.x_v,[],2); 30 | 31 | % Bounded complexity 32 | cont.H_theta = sys.H_theta; 33 | cont.h_theta_k = sys.h_theta; 34 | cont.h_theta_0 = sys.h_theta; 35 | 36 | end -------------------------------------------------------------------------------- /Plots/controller_nom.m: -------------------------------------------------------------------------------- 1 | function optProb = controller_nom(sys,cont) 2 | 3 | %% setup optimization problem 4 | 5 | x_pre = sdpvar(sys.n,1,'full'); 6 | theta_pre = sdpvar(sys.p,1,'full'); 7 | 8 | %% NOTE: HARDCODED FOR 2 dimensional parameters 9 | A_true = sys.A0+ sys.Ap(:,:,1)*theta_pre(1)+ sys.Ap(:,:,2)*theta_pre(2); 10 | B_true = sys.B0+ sys.Bp(:,:,1)*theta_pre(1)+ sys.Bp(:,:,2)*theta_pre(2); 11 | 12 | % Declare independent variables 13 | % l goes from 1:N 14 | % j goes from 1:nx_v 15 | x_lk = sdpvar(sys.n,cont.N+1,'full'); 16 | v_lk = sdpvar(sys.n,cont.N,'full'); 17 | Constraints = [x_lk(:,1) == x_pre]; 18 | for l = 1:cont.N 19 | % constraints 20 | Constraints = [Constraints, ... 21 | (sys.F+sys.G*cont.K)*x_lk(:,l) + sys.G*v_lk(:,l) <= ones(sys.nc,1)]; 22 | % dynamics 23 | Constraints = [Constraints, ... 24 | x_lk(:,l+1) == (A_true+ B_true *cont.K)*x_lk(:,l) + B_true *v_lk(:,l)]; 25 | end 26 | 27 | % define terminal constraints 28 | Constraints = [Constraints, ... 29 | cont.H_x *x_lk(:,cont.N+1) <= cont.vec_1_x*cont.alpha_bar;]; 30 | 31 | options = sdpsettings('solver','gurobi','verbose',0,'debug',0); 32 | 33 | %% Cost function definition 34 | 35 | J = 0; 36 | 37 | for l = 1:cont.N 38 | J = J + (norm(cont.Q_L*x_lk(:,l),'inf') + norm(cont.R_L*(cont.K*x_lk(:,l)+v_lk(:,l)),'inf')); 39 | end 40 | J = J + (norm(cont.Q_L*x_lk(:,l),'inf') + norm(cont.R_L*(cont.K*x_lk(:,l)),'inf')); 41 | 42 | optProb = optimizer(Constraints,J,options,[x_pre;theta_pre],cont.K*x_pre+v_lk(:,1)); 43 | 44 | 45 | end 46 | 47 | -------------------------------------------------------------------------------- /Plots/model_nom.m: -------------------------------------------------------------------------------- 1 | classdef model_nom 2 | properties 3 | theta_true 4 | A_true 5 | B_true 6 | w_bound 7 | x 8 | end 9 | 10 | 11 | methods 12 | % Constructor 13 | function obj = model_nom(sys,theta0,x0) 14 | obj.theta_true = theta0; 15 | obj.A_true = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(obj.theta_true,[1,1,sys.p])),3); 16 | obj.B_true = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(obj.theta_true,[1,1,sys.p])),3); 17 | 18 | obj.w_bound = sys.w_bound; 19 | obj.x = x0; 20 | end 21 | 22 | % simulate 23 | function obj = simulate(obj,u) 24 | % works only for uniform w bounds [not general polytope Hw*w<=1] 25 | w = -obj.w_bound + 2*obj.w_bound*rand(length(obj.x),1); 26 | 27 | obj.x = obj.A_true*obj.x + obj.B_true*u+w; 28 | end 29 | 30 | end 31 | 32 | end -------------------------------------------------------------------------------- /Plots/nominal_MPC.m: -------------------------------------------------------------------------------- 1 | % script to run nominal MPC for all the systems in the parameter space 2 | 3 | clc; 4 | clear; 5 | 6 | addpath('..\Functions','..\') 7 | sys = system_desc(); 8 | cont = cont_params(sys); 9 | 10 | Tsim = 10; 11 | %% 12 | 13 | % range of thetas to be considered 14 | pts = 51; 15 | theta1 = linspace(-0.4,0.7,pts); 16 | theta2 = linspace(-0.5,1,pts); 17 | J_all = NaN*ones(length(theta1)); 18 | u1_all = NaN*ones(length(theta1)); 19 | u2_all = NaN*ones(length(theta1)); 20 | 21 | 22 | tic 23 | optProb = controller_nom(sys,cont); 24 | toc 25 | 26 | for t1 = 1:length(theta1) 27 | for t2 = 1:length(theta2) 28 | %% Simulate for each set of thetas 29 | % initialize states and inputs 30 | x = NaN*ones(sys.n,Tsim+1); 31 | u = NaN*ones(sys.m,Tsim); 32 | 33 | J = 0; 34 | 35 | % initial condition 36 | x(:,1) = sys.x0; 37 | 38 | % model initialization 39 | true_sys = model_nom(sys,[theta1(t1);theta2(t2)],x(:,1)); 40 | 41 | rng(10,'twister'); 42 | % simulate 43 | for k = 1:Tsim 44 | 45 | % calculate control input 46 | [u(:,k)] = optProb([x(:,k);theta1(t1);theta2(t2)]); 47 | % runtimes(:,k) = a5.solvertime; 48 | 49 | % Apply to true system 50 | true_sys = true_sys.simulate(u(:,k)); 51 | x(:,k+1) = true_sys.x; 52 | J = J + norm(cont.Q_L*x(:,k+1),'inf') + norm(cont.R_L*u(:,k),'inf'); 53 | 54 | end 55 | %% Store relevant data 56 | 57 | J_all(t1,t2) = J; 58 | u1_all(t1,t2) = u(1,1); 59 | u2_all(t1,t2) = u(2,1); 60 | 61 | clear true_sys 62 | end 63 | toc 64 | end 65 | 66 | %% Save data 67 | save('nom_data.mat','J_all','u1_all','u2_all') -------------------------------------------------------------------------------- /Plots/plot_script.m: -------------------------------------------------------------------------------- 1 | %% Script to generate plots for RAMPC 2 | 3 | clc; 4 | clear; 5 | 6 | addpath('..\Functions','..\') 7 | sys = system_desc(); 8 | cont_P = cont_params(sys); 9 | 10 | Tsim = 10; 11 | 12 | %% Simulate PAMPC 13 | 14 | % initialize states and inputs 15 | x_P = NaN*ones(sys.n,Tsim+1); 16 | u_P = NaN*ones(sys.m,Tsim); 17 | runtimes_P = NaN*ones(Tsim,1); 18 | 19 | J_P = 0; 20 | % regressors 21 | Dk_P = zeros(sys.n*cont_P.blk,sys.p); 22 | dk_P = zeros(sys.n*cont_P.blk,1); 23 | 24 | 25 | % initial condition 26 | x_P(:,1) = sys.x0; 27 | 28 | % model initialization 29 | true_sys = model(sys,x_P(:,1)); 30 | 31 | % Estimation parameters % Unused in PAMPC (needed in sim only) 32 | cont_P.mu = 2; 33 | cont_P.theta_hat = [0.5;0.5;]; 34 | cont_P.x_hat_k = sys.x0; 35 | cont_P.A_est = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(cont_P.theta_hat,[1,1,sys.p])),3); 36 | cont_P.B_est = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(cont_P.theta_hat,[1,1,sys.p])),3); 37 | 38 | optProb_P = controller_pre(sys,cont_P); 39 | 40 | rng(10,'twister'); 41 | % simulate 42 | for k = 1:Tsim 43 | % update feasible set and parameter estimate 44 | cont_P = updateParameters(sys,cont_P,x_P(:,k),Dk_P,dk_P); 45 | 46 | % calculate control input 47 | [u_P(:,k),a1,~,~,~,a5] = optProb_P([x_P(:,k);cont_P.h_theta_k]); 48 | runtimes_P(:,k) = a5.solvertime; 49 | 50 | % Apply to true system 51 | true_sys = true_sys.simulate(u_P(:,k)); 52 | x_P(:,k+1) = true_sys.x; 53 | J_P = J_P + norm(cont_P.Q_L*x_P(:,k+1),'inf') + norm(cont_P.R_L*u_P(:,k),'inf'); 54 | 55 | % update regressors 56 | new_Dk = zeros(sys.n,sys.p); 57 | for i= 1:sys.p 58 | new_Dk(:,i) = sys.Ap(:,:,i)*x_P(:,k) + sys.Bp(:,:,i)*u_P(:,k); 59 | end 60 | new_dk = -x_P(:,k+1)+sys.A0*x_P(:,k)+sys.B0*u_P(:,k); 61 | 62 | Dk_P = [Dk_P(sys.n+1:end,:);new_Dk]; 63 | dk_P = [dk_P(sys.n+1:end);new_dk]; 64 | 65 | end 66 | 67 | %% Simulate DAMPC 1 68 | cont_D1 = cont_params(sys); 69 | 70 | % initialize states and inputs 71 | x_D1 = NaN*ones(sys.n,Tsim+1); 72 | u_D1 = NaN*ones(sys.m,Tsim); 73 | runtimes_D1 = NaN*ones(Tsim,1); 74 | 75 | J_D1 = 0; 76 | % regressors 77 | Dk_D1 = zeros(sys.n*cont_D1.blk,sys.p); 78 | dk_D1 = zeros(sys.n*cont_D1.blk,1); 79 | 80 | % initial condition 81 | x_D1(:,1) = sys.x0; 82 | 83 | % model initialization 84 | true_sys = model(sys,x_D1(:,1)); 85 | 86 | % Estimation parameters 87 | cont_D1.mu = 2; 88 | cont_D1.theta_hat = [0.5;0.5;]; 89 | cont_D1.x_hat_k = sys.x0; 90 | cont_D1.A_est = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(cont_D1.theta_hat,[1,1,sys.p])),3); 91 | cont_D1.B_est = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(cont_D1.theta_hat,[1,1,sys.p])),3); 92 | 93 | % Exploration: number of predictions 94 | cont_D1.nPred_theta = 1; 95 | cont_D1.nPred_X = 2; 96 | 97 | optProb_D1 = controller_expl_pre(sys,cont_D1); 98 | 99 | rng(10,'twister'); 100 | % simulate 101 | for k = 1:Tsim 102 | % update feasible set and parameter estimate 103 | cont_D1 = updateParameters(sys,cont_D1,x_D1(:,k),Dk_D1,dk_D1); 104 | 105 | % calculate control input 106 | [u_D1(:,k),a1,~,~,~,a5] = optProb_D1([x_D1(:,k);cont_D1.h_theta_k;cont_D1.theta_hat]); 107 | runtimes_D1(k) = a5.solvertime; 108 | 109 | % Apply to true system 110 | true_sys = true_sys.simulate(u_D1(:,k)); 111 | x_D1(:,k+1) = true_sys.x; 112 | J_D1 = J_D1 + norm(cont_D1.Q_L*x_D1(:,k+1),'inf') + norm(cont_D1.R_L*u_D1(:,k),'inf'); 113 | 114 | % update regressors 115 | new_Dk = zeros(sys.n,sys.p); 116 | for i= 1:sys.p 117 | new_Dk(:,i) = sys.Ap(:,:,i)*x_D1(:,k) + sys.Bp(:,:,i)*u_D1(:,k); 118 | end 119 | new_dk = -x_D1(:,k+1)+sys.A0*x_D1(:,k)+sys.B0*u_D1(:,k); 120 | 121 | Dk_D1 = [Dk_D1(sys.n+1:end,:);new_Dk]; 122 | dk_D1 = [dk_D1(sys.n+1:end);new_dk]; 123 | end 124 | 125 | %% Simulate DAMPC 2 126 | 127 | cont_D2 = cont_params(sys); 128 | 129 | % initialize states and inputs 130 | x_D2 = NaN*ones(sys.n,Tsim+1); 131 | u_D2 = NaN*ones(sys.m,Tsim); 132 | runtimes_D2 = NaN*ones(Tsim,1); 133 | 134 | J_D2 = 0; 135 | % regressors 136 | Dk_D2 = zeros(sys.n*cont_D2.blk,sys.p); 137 | dk_D2 = zeros(sys.n*cont_D2.blk,1); 138 | 139 | % initial condition 140 | x_D2(:,1) = sys.x0; 141 | 142 | % model initialization 143 | true_sys = model(sys,x_D2(:,1)); 144 | 145 | % Estimation parameters 146 | cont_D2.mu = 2; 147 | cont_D2.theta_hat = [0.5;0.5;]; 148 | cont_D2.x_hat_k = sys.x0; 149 | cont_D2.A_est = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(cont_D2.theta_hat,[1,1,sys.p])),3); 150 | cont_D2.B_est = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(cont_D2.theta_hat,[1,1,sys.p])),3); 151 | 152 | % Exploration: number of predictions 153 | cont_D2.nPred_theta = 1; 154 | cont_D2.nPred_X = 5; 155 | 156 | optProb_D2 = controller_expl_pre(sys,cont_D2); 157 | 158 | rng(10,'twister'); 159 | % simulate 160 | for k = 1:Tsim 161 | % update feasible set and parameter estimate 162 | cont_D2 = updateParameters(sys,cont_D2,x_D2(:,k),Dk_D2,dk_D2); 163 | 164 | % calculate control input 165 | [u_D2(:,k),a1,~,~,~,a5] = optProb_D2([x_D2(:,k);cont_D2.h_theta_k;cont_D2.theta_hat]); 166 | runtimes_D2(k) = a5.solvertime; 167 | 168 | % Apply to true system 169 | true_sys = true_sys.simulate(u_D2(:,k)); 170 | x_D2(:,k+1) = true_sys.x; 171 | J_D2 = J_D2 + norm(cont_D2.Q_L*x_D2(:,k+1),'inf') + norm(cont_D2.R_L*u_D2(:,k),'inf'); 172 | 173 | % update regressors 174 | new_Dk = zeros(sys.n,sys.p); 175 | for i= 1:sys.p 176 | new_Dk(:,i) = sys.Ap(:,:,i)*x_D2(:,k) + sys.Bp(:,:,i)*u_D2(:,k); 177 | end 178 | new_dk = -x_D2(:,k+1)+sys.A0*x_D2(:,k)+sys.B0*u_D2(:,k); 179 | 180 | Dk_D2 = [Dk_D2(sys.n+1:end,:);new_Dk]; 181 | dk_D2 = [dk_D2(sys.n+1:end);new_dk]; 182 | 183 | end 184 | 185 | %% PLOTS START 186 | %% Plot the trajectories 187 | 188 | set(0,'DefaultFigureWindowStyle','normal','defaultLineLineWidth',1.5,'DefaultLineMarkerSize',3) 189 | 190 | f = 80; 191 | h = figure(f);clf; f = f+1; 192 | h.Units = 'centimeters'; 193 | h.WindowStyle = 'normal'; 194 | h.Position = [2 2 8 10]; 195 | ha = tight_subplot(4,1,[0.03 0.05],[.09 .01],[.1 .03]); 196 | 197 | i = 1; 198 | axes(ha(i));hold on; 199 | plot(0:Tsim,x_P(1,:),'b-^') 200 | plot(0:Tsim,x_D1(1,:),'r-.^') 201 | plot(0:Tsim,x_D2(1,:),'k:^') 202 | ylabel(ha(i),'$x_1$') 203 | ylim(ha(i),[-0.2 1.5]) 204 | yticks(ha(i),0:1) 205 | legend('PAMPC','DAMPC$_2$','DAMPC$_5$',... 206 | 'Orientation','Vertical','Location','Northeast') 207 | 208 | i = i+1; 209 | axes(ha(i));hold on; 210 | plot(0:Tsim,x_P(2,:),'b-^') 211 | plot(0:Tsim,x_D1(2,:),'r-.^') 212 | plot(0:Tsim,x_D2(2,:),'k:^') 213 | ylabel(ha(i),'$x_2$') 214 | ylim(ha(i),[-0.2 1.5]) 215 | yticks(ha(i),0:1) 216 | 217 | i = i+1; 218 | axes(ha(3));hold on; 219 | plot(0:Tsim-1,-0.5*ones(Tsim,1),'m--') 220 | plot(0:Tsim-1,u_P(1,:),'b-^') 221 | plot(0:Tsim-1,u_D1(1,:),'r-.^') 222 | plot(0:Tsim-1,u_D2(1,:),'k:^') 223 | xlim([0 Tsim]) 224 | ylabel(ha(i),'$u_1$') 225 | yticks(ha(i),-1:0) 226 | ylim(ha(i),[-1.2 0.2]) 227 | legend('Input constraint','Location','Southeast') 228 | 229 | i = i+1; 230 | axes(ha(i));hold on; 231 | plot(0:Tsim-1,u_P(2,:),'b-^') 232 | plot(0:Tsim-1,u_D1(2,:),'r-.^') 233 | plot(0:Tsim-1,u_D2(2,:),'k:^') 234 | xlim([0 Tsim]) 235 | ylabel(ha(i),'$u_2$') 236 | yticks(ha(i),-1:0) 237 | ylim(ha(i),[-1.2 0.2]) 238 | 239 | yticklabels(ha,'auto') 240 | xticklabels(ha(4),'auto') 241 | xlabel(ha(4),'Timesteps') 242 | 243 | export_fig trajCL.eps 244 | % saveas(f-1,'trajCL','epsc') 245 | 246 | %% Plot parameter set after 10 time steps 247 | 248 | h = figure(f);clf; f = f+1; 249 | h.Units = 'centimeters'; 250 | h.WindowStyle = 'normal'; 251 | h.Position = [2 2 8 7]; 252 | 253 | hold on; 254 | plotregionLine(-cont_P.H_theta,-cont_P.h_theta_k,[],[],'b',[],[],'-'); 255 | plotregionLine(-cont_D1.H_theta,-cont_D1.h_theta_k,[],[],'r',[],[],'-.'); 256 | plotregionLine(-cont_D2.H_theta,-cont_D2.h_theta_k,[],[],'k',[],[],'--'); 257 | plot(0.95,0.3,'ms','MarkerSize',3,'MarkerFaceColor','red') 258 | xlim([-1 1]);ylim([-1 1]); 259 | xlabel('$\theta_1$') 260 | ylabel('$\theta_2$') 261 | legend('PAMPC','DAMPC$_2$','DAMPC$_5$',... 262 | 'Orientation','Vertical','Location','southeast') 263 | 264 | export_fig parameterSet.eps 265 | %% Plot heatmap of nominal costs 266 | 267 | nom = load('nom_data.mat'); 268 | 269 | h = figure(f);clf; f = f+1; 270 | h.Units = 'centimeters'; 271 | h.WindowStyle = 'normal'; 272 | h.Position = [2 2 8 5.5]; 273 | hold on; 274 | t1b = [-0.5,1]; 275 | t2b = [-0.4;0.71]; 276 | pts = 51; 277 | surf(linspace(t1b(1),t1b(2),pts),linspace(t2b(1),t2b(2),pts),nom.J_all'-10); 278 | colormap summer 279 | shading interp 280 | plotregionLine(-cont_P.H_theta,-cont_P.h_theta_k,[],[],'b',[],[],'-'); 281 | plotregionLine(-cont_D1.H_theta,-cont_D1.h_theta_k,[],[],'r',[],[],'-.'); 282 | plotregionLine(-cont_D2.H_theta,-cont_D2.h_theta_k,[],[],'k',[],[],'--'); 283 | plot(0.95,0.3,'ms','MarkerSize',3,'MarkerFaceColor','red') 284 | 285 | view([0 0 1]) 286 | xlim(t1b);ylim(t2b); 287 | colorbar('Ticks',[-7.5,-7,-6.5,-6,-5.5,-5,-4.5],... 288 | 'TickLabels',{'2.5','3','3.5','4','4.5','5','5.5'}); 289 | xlabel('$\theta_1$') 290 | ylabel('$\theta_2$') 291 | % title('Nominal Costs') 292 | export_fig nominalCost.eps 293 | -------------------------------------------------------------------------------- /Plots/plotregionLine.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/anilparsi/RobustAdaptiveMPC/4544b27ae9eaa6bfb24e749af23a29c232ccf3e1/Plots/plotregionLine.m -------------------------------------------------------------------------------- /boundedComplexity.m: -------------------------------------------------------------------------------- 1 | function sys = boundedComplexity(sys) 2 | %% add constraints on parameters with bounded complexity overall 3 | 4 | %% Decide on directions to add constraints 5 | % number of parameters 6 | p = size(sys.H_theta,2); 7 | 8 | % Each plane in parameter space can be specified by a direction vector 9 | % chosen. The directions involving two zeros and 1 are chosen already. Now, 10 | % we vary each parameter in the space between -a and b, with n_values 11 | % number of grid points in between. 12 | 13 | n_values = 3; 14 | values = linspace(-1.1,1,n_values)'; 15 | l_values = length(values); % length of the values vector 16 | 17 | new_dir = kron(values,ones(l_values,1)); 18 | for i =1:p-1 19 | new_dir = [kron(new_dir,ones(l_values,1)),kron(ones(l_values^(i+1),1),values)]; 20 | end 21 | 22 | n_new_dir = size(new_dir,1); 23 | %% Decide on lower and upper bounds for these directions 24 | 25 | % Check if already calculated bounds are valid 26 | try 27 | % load and check length 28 | matrices = load('calc_bounds.mat'); 29 | calc_flag = length(matrices.lb_new)~=n_new_dir; 30 | 31 | % check equality 32 | if any(any(new_dir-matrices.new_dir)) 33 | % not same matrices 34 | calc_flag = 1; 35 | end 36 | 37 | catch 38 | calc_flag = 1; 39 | end 40 | 41 | 42 | if calc_flag 43 | % calculate upper and lower bounds again 44 | lb_new = NaN*ones(n_new_dir,1); 45 | ub_new = NaN*ones(n_new_dir,1); 46 | for i = 1:n_new_dir 47 | % calculate upper bound 48 | cvx_begin quiet 49 | variable x_max(p,1) 50 | variable J_max 51 | maximize J_max 52 | 53 | subject to 54 | J_max == new_dir(i,:)*x_max; 55 | sys.H_theta*x_max <= sys.h_theta; 56 | cvx_end 57 | 58 | if (isnan(J_max)) 59 | warning(cvx_status) 60 | flag = 1; 61 | return; 62 | end 63 | ub_new(i) = J_max; 64 | 65 | % calculate lower bound 66 | cvx_begin quiet 67 | variable x_min(p,1) 68 | variable J_min 69 | minimize J_min 70 | 71 | subject to 72 | J_min == new_dir(i,:)*x_min; 73 | sys.H_theta*x_min <= sys.h_theta; 74 | cvx_end 75 | 76 | if (isnan(J_min)) 77 | warning(cvx_status) 78 | flag = 1; 79 | return; 80 | end 81 | lb_new(i) = J_min; 82 | 83 | end 84 | else % use loaded matrices 85 | warning('Bounded complexity matrices are being loaded from calc_bounds.mat.') 86 | ub_new = matrices.ub_new; 87 | lb_new = matrices.lb_new; 88 | end 89 | 90 | save('calc_bounds.mat','new_dir','lb_new','ub_new'); 91 | 92 | sys.H_theta = [sys.H_theta; new_dir; -new_dir]; 93 | sys.h_theta = [sys.h_theta; ub_new; -lb_new]; 94 | end 95 | -------------------------------------------------------------------------------- /calc_bounds.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/anilparsi/RobustAdaptiveMPC/4544b27ae9eaa6bfb24e749af23a29c232ccf3e1/calc_bounds.mat -------------------------------------------------------------------------------- /controller.m: -------------------------------------------------------------------------------- 1 | function u = controller(sys,cont,xk) 2 | 3 | %% setup optimization problem 4 | 5 | % Declare independent variables 6 | % l goes from 1:N 7 | % j goes from 1:nx_v 8 | z_lk = sdpvar(sys.n,cont.N+1,'full'); 9 | v_lk = sdpvar(sys.m,cont.N,'full'); 10 | alpha_lk = sdpvar(1,cont.N+1,'full'); 11 | Lambda_jlk = sdpvar(cont.nHx,sys.nHtheta,cont.nx_v,cont.N-1,'full'); 12 | Lambda_1k = sdpvar(cont.nHx,sys.nHtheta,'full'); 13 | 14 | % define state dependent variables 15 | x_jlk = []; 16 | u_jlk = []; 17 | d_jlk = []; 18 | D_jlk = []; 19 | for l = 2:cont.N 20 | x_jlk = cat(3,x_jlk,repmat(z_lk(:,l),1,cont.nx_v)+alpha_lk(:,l)*cont.x_v); 21 | u_jlk = cat(3,u_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_jlk(:,:,l-1)); 22 | 23 | d_jlk = cat(3,d_jlk,sys.A0*x_jlk(:,:,l-1)+sys.B0*u_jlk(:,:,l-1) - repmat(z_lk(:,l+1),1,cont.nx_v)); 24 | D_temp = []; 25 | for j = 1:cont.nx_v 26 | D_temp = cat(3,D_temp,D_mult(sys,x_jlk(:,j,l-1),u_jlk(:,j,l-1))); 27 | end 28 | D_jlk = cat(4,D_jlk,D_temp); 29 | end 30 | 31 | % define constraints. 32 | Constraints = [Lambda_jlk(:)>=0,Lambda_1k(:)>=0]; 33 | Constraints = [Constraints,z_lk(:,1)==xk,alpha_lk(1)==0]; 34 | Constraints = [Constraints,... 35 | (sys.F+sys.G*cont.K)*xk + sys.G*v_lk(:,1) <= ones(sys.nc,1),... 36 | Lambda_1k*cont.h_theta_k + cont.H_x*(sys.A0*xk+sys.B0*(cont.K*xk+v_lk(:,1))-z_lk(:,2)) - alpha_lk(2)*ones(cont.nHx,1) <= -cont.w_bar,... 37 | cont.H_x*D_mult(sys,xk,cont.K*xk+v_lk(:,1)) == Lambda_1k*cont.H_theta 38 | ]; 39 | for l = 2:cont.N 40 | Constraints = [Constraints, ...s 41 | (sys.F+sys.G*cont.K)*z_lk(:,l) + sys.G*v_lk(:,l) + alpha_lk(:,l)*cont.f_bar <= ones(sys.nc,1)]; 42 | 43 | for j = 1:cont.nx_v 44 | Constraints = [Constraints, ... 45 | Lambda_jlk(:,:,j,l-1)*cont.h_theta_k + cont.H_x*d_jlk(:,j,l-1) - alpha_lk(:,l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 46 | 47 | Constraints = [Constraints, ... 48 | cont.H_x*D_jlk(:,:,j,l-1) == Lambda_jlk(:,:,j,l-1)*cont.H_theta]; 49 | end 50 | end 51 | 52 | % define terminal constraints 53 | Constraints = [Constraints, ... 54 | z_lk(:,cont.N+1) == zeros(sys.n,1), ... 55 | alpha_lk(cont.N+1) <= cont.alpha_bar... 56 | alpha_lk(cont.N+1) >= cont.alpha_min]; 57 | 58 | options = sdpsettings('solver','gurobi','verbose',0,'debug',0); 59 | 60 | 61 | %% Cost function definition 62 | 63 | % define cost variables 64 | stage_cost_max = sdpvar(cont.N+1,1,'full'); 65 | J = sum(stage_cost_max); 66 | 67 | %% define cost constraints 68 | 69 | % define cost variables 70 | stage_cost_max = sdpvar(cont.N+1,1,'full'); 71 | J = sum(stage_cost_max); 72 | 73 | costConstraints = [norm(cont.R_L*(cont.K*xk+v_lk(:,1)),'inf')<=stage_cost_max(1)]; 74 | 75 | for l = 2:cont.N 76 | for j = 1:cont.nx_v 77 | costConstraints = [costConstraints, ... 78 | norm(cont.Q_L*x_jlk(:,j,l-1),'inf') + norm(cont.R_L*(cont.K*x_jlk(:,j,l-1)+v_lk(:,l)),'inf') <= stage_cost_max(l) 79 | ]; 80 | end 81 | end 82 | % terminal cost 83 | for j = 1:cont.nx_v 84 | costConstraints = [costConstraints, ... 85 | alpha_lk(cont.N+1)*(norm(cont.Q_L*cont.x_v(:,j),'inf') + norm(cont.R_L*cont.K*cont.x_v(:,j),'inf')) <= stage_cost_max(cont.N+1) 86 | ]; 87 | end 88 | 89 | diagnostics = optimize([Constraints,costConstraints],J,options); 90 | if diagnostics.problem 91 | error(diagnostics.info); 92 | end 93 | u = value(cont.K*xk + v_lk(:,1)); 94 | 95 | 96 | end 97 | 98 | -------------------------------------------------------------------------------- /controller_expl.m: -------------------------------------------------------------------------------- 1 | function u = controller_expl(sys,cont,xk) 2 | 3 | %% Part for standard constraints 4 | % Declare independent variables 5 | % l goes from 1:N 6 | % j goes from 1:nx_v 7 | z_lk = sdpvar(sys.n,cont.N+1,'full'); 8 | v_lk = sdpvar(sys.m,cont.N,'full'); 9 | alpha_lk = sdpvar(1,cont.N+1,'full'); 10 | Lambda_jlk = sdpvar(cont.nHx,sys.nHtheta,cont.nx_v,cont.N-1,'full'); 11 | Lambda_1k = sdpvar(cont.nHx,sys.nHtheta,'full'); 12 | % define state dependent variables 13 | x_jlk = []; 14 | u_jlk = []; 15 | d_jlk = []; 16 | D_jlk = []; 17 | for l = 2:cont.N 18 | x_jlk = cat(3,x_jlk,repmat(z_lk(:,l),1,cont.nx_v)+alpha_lk(l)*cont.x_v); 19 | u_jlk = cat(3,u_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_jlk(:,:,l-1)); 20 | 21 | d_jlk = cat(3,d_jlk,sys.A0*x_jlk(:,:,l-1)+sys.B0*u_jlk(:,:,l-1) - repmat(z_lk(:,l+1),1,cont.nx_v)); 22 | D_temp = []; 23 | for j = 1:cont.nx_v 24 | D_temp = cat(3,D_temp,D_mult(sys,x_jlk(:,j,l-1),u_jlk(:,j,l-1))); 25 | end 26 | D_jlk = cat(4,D_jlk,D_temp); 27 | end 28 | 29 | % define constraints. 30 | stdConstraints = [Lambda_jlk(:)>=0,Lambda_1k(:)>=0]; 31 | stdConstraints = [stdConstraints,z_lk(:,1)==xk,alpha_lk(1)==0]; 32 | stdConstraints = [stdConstraints,... 33 | (sys.F+sys.G*cont.K)*xk + sys.G*v_lk(:,1) <= ones(sys.nc,1),... 34 | Lambda_1k*cont.h_theta_k + cont.H_x*(sys.A0*xk+sys.B0*(cont.K*xk+v_lk(:,1))-z_lk(:,2)) - alpha_lk(2)*ones(cont.nHx,1) <= -cont.w_bar,... 35 | cont.H_x*D_mult(sys,xk,cont.K*xk+v_lk(:,1)) == Lambda_1k*cont.H_theta 36 | ]; 37 | for l = 2:cont.N 38 | stdConstraints = [stdConstraints, ... 39 | (sys.F+sys.G*cont.K)*z_lk(:,l) + sys.G*v_lk(:,l) + alpha_lk(l)*cont.f_bar <= ones(sys.nc,1)]; 40 | 41 | for j = 1:cont.nx_v 42 | stdConstraints = [stdConstraints, ... 43 | Lambda_jlk(:,:,j,l-1)*cont.h_theta_k + cont.H_x*d_jlk(:,j,l-1) - alpha_lk(l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 44 | 45 | stdConstraints = [stdConstraints, ... 46 | cont.H_x*D_jlk(:,:,j,l-1) == Lambda_jlk(:,:,j,l-1)*cont.H_theta]; 47 | end 48 | end 49 | 50 | % define terminal constraints 51 | stdConstraints = [stdConstraints, ... 52 | z_lk(:,cont.N+1) == zeros(sys.n,1), ... 53 | alpha_lk(cont.N+1) <= cont.alpha_bar ... 54 | alpha_lk(cont.N+1) >= cont.alpha_min]; 55 | %% Part for exploration 56 | 57 | % Declare variables for exploration 58 | if cont.nPred_X>1 59 | Lambda_til_jlk = sdpvar(cont.nHx,sys.nHtheta+cont.nPred_theta*sys.nHw,cont.nx_v,cont.nPred_X-1,'full'); 60 | else 61 | Lambda_til_jlk = []; 62 | end 63 | Lambda_til_1k = sdpvar(cont.nHx,sys.nHtheta+cont.nPred_theta*sys.nHw); 64 | z_til_lk = sdpvar(sys.n,cont.nPred_X+1,'full'); 65 | alpha_til_lk = sdpvar(1,cont.nPred_X+1,'full'); 66 | 67 | % predict state evolution 68 | x_hat = xk; 69 | for l = 1:cont.nPred_theta-1 70 | x_hat = [x_hat (cont.A_est+cont.B_est*cont.K)*x_hat(:,l) + cont.B_est*v_lk(:,l)]; 71 | end 72 | 73 | % predict new constraints on theta 74 | H_pred = [sys.H_w*D_mult(sys,xk,cont.K*xk+v_lk(:,1))]; 75 | h_pred = [sys.h_w + sys.H_w*D_mult(sys,xk,cont.K*xk+v_lk(:,1))*cont.theta_hat]; 76 | for l = 2:cont.nPred_theta 77 | H_pred = [H_pred; 78 | sys.H_w*D_mult(sys,x_hat(:,l),cont.K*x_hat(:,l)+v_lk(:,l))]; 79 | h_pred = [h_pred; 80 | sys.h_w + sys.H_w*D_mult(sys,x_hat(:,l),cont.K*x_hat(:,l)+v_lk(:,l))*cont.theta_hat]; 81 | end 82 | 83 | % Append to H_theta 84 | H_theta_til = [cont.H_theta; H_pred]; 85 | h_theta_til = [cont.h_theta_k; h_pred]; 86 | 87 | % define exploration variables 88 | x_til_jlk = []; 89 | u_til_jlk = []; 90 | d_til_jlk = []; 91 | D_til_jlk = []; 92 | 93 | for l = 2:cont.nPred_X+1 94 | x_til_jlk = cat(3,x_til_jlk,repmat(z_til_lk(:,l),1,cont.nx_v)+alpha_til_lk(l)*cont.x_v); 95 | u_til_jlk = cat(3,u_til_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_til_jlk(:,:,l-1)); 96 | end 97 | for l = 2:cont.nPred_X 98 | d_til_jlk = cat(3,d_til_jlk,sys.A0*x_til_jlk(:,:,l-1)+sys.B0*u_til_jlk(:,:,l-1) - repmat(z_til_lk(:,l+1),1,cont.nx_v)); 99 | D_temp = []; 100 | for j = 1:cont.nx_v 101 | D_temp = cat(3,D_temp,D_mult(sys,x_til_jlk(:,j,l-1),u_til_jlk(:,j,l-1))); 102 | end 103 | D_til_jlk = cat(4,D_til_jlk,D_temp); 104 | end 105 | 106 | % define exploration constraints 107 | exploreConstraints = [Lambda_til_jlk(:)>=0,Lambda_til_1k(:)>=0]; 108 | exploreConstraints = [exploreConstraints,z_til_lk(:,1)==xk,alpha_til_lk(1)==0]; 109 | exploreConstraints = [exploreConstraints,... 110 | Lambda_til_1k*h_theta_til + cont.H_x*(sys.A0*xk+sys.B0*(cont.K*xk+v_lk(:,1))-z_til_lk(:,2)) - alpha_til_lk(2)*ones(cont.nHx,1) <= -cont.w_bar,... 111 | cont.H_x*D_mult(sys,xk,cont.K*xk+v_lk(:,1)) == Lambda_til_1k*H_theta_til 112 | ]; 113 | % implement containment conditions of tubes 114 | for l = 2:cont.nPred_X 115 | exploreConstraints = [exploreConstraints, ... 116 | (sys.F+sys.G*cont.K)*z_til_lk(:,l) + sys.G*v_lk(:,l) + alpha_til_lk(l)*cont.f_bar <= ones(sys.nc,1) 117 | ]; 118 | for j = 1:cont.nx_v 119 | exploreConstraints = [exploreConstraints, ... 120 | Lambda_til_jlk(:,:,j,l-1)*h_theta_til + cont.H_x*d_til_jlk(:,j,l-1) - alpha_til_lk(l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 121 | 122 | exploreConstraints = [exploreConstraints, ... 123 | cont.H_x*D_til_jlk(:,:,j,l-1) == Lambda_til_jlk(:,:,j,l-1)*H_theta_til]; 124 | end 125 | end 126 | % at the end, ensure the X_til_lk tube is inside the X_lk tube 127 | for j = 1:cont.nx_v 128 | exploreConstraints = [exploreConstraints, ... 129 | cont.H_x*(z_til_lk(:,end)+alpha_til_lk(end)*cont.x_v(:,j) - z_lk(:,cont.nPred_X+1)) <= alpha_lk(cont.nPred_X+1)*cont.vec_1_x; ]; 130 | end 131 | %% Cost function definition 132 | 133 | % define cost variables 134 | stage_cost_max = sdpvar(cont.N+1,1,'full'); 135 | J = sum(stage_cost_max); 136 | 137 | % define cost constraints 138 | costConstraints = [norm(cont.R_L*(cont.K*xk+v_lk(:,1)),'inf')<=stage_cost_max(1)]; 139 | 140 | for l = 2:cont.N 141 | for j = 1:cont.nx_v 142 | if l<=cont.nPred_X+1 143 | costConstraints = [costConstraints, ... 144 | norm(cont.Q_L*x_til_jlk(:,j,l-1),'inf') + norm(cont.R_L*(cont.K*x_til_jlk(:,j,l-1)+v_lk(:,l)),'inf') <= stage_cost_max(l) 145 | ]; 146 | else 147 | costConstraints = [costConstraints, ... 148 | norm(cont.Q_L*x_jlk(:,j,l-1),'inf') + norm(cont.R_L*(cont.K*x_jlk(:,j,l-1)+v_lk(:,l)),'inf') <= stage_cost_max(l) 149 | ]; 150 | end 151 | end 152 | end 153 | % terminal cost 154 | for j = 1:cont.nx_v 155 | costConstraints = [costConstraints, ... 156 | alpha_lk(cont.N+1)*(norm(cont.Q_L*cont.x_v(:,j),'inf') + norm(cont.R_L*cont.K*cont.x_v(:,j),'inf')) <= stage_cost_max(cont.N+1) 157 | ]; 158 | end 159 | 160 | %% Solve problem 161 | Constraints = [stdConstraints;exploreConstraints;costConstraints]; 162 | options = sdpsettings('solver','ipopt','verbose',0); 163 | diagnostics = optimize(Constraints,J,options); 164 | if diagnostics.problem 165 | error(diagnostics.info) 166 | end 167 | u = value(cont.K*xk + v_lk(:,1)); 168 | 169 | end 170 | 171 | -------------------------------------------------------------------------------- /controller_expl_pre.m: -------------------------------------------------------------------------------- 1 | function optProb = controller_expl_pre(sys,cont) 2 | % function to generate optimizer using yalmip 3 | 4 | % define presolve variables 5 | x_pre = sdpvar(sys.n,1,'full'); 6 | h_pre = sdpvar(sys.nHtheta,1,'full'); 7 | theta_pre = sdpvar(sys.p,1,'full'); 8 | 9 | A_pre = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(cont.theta_hat,[1,1,sys.p])),3); 10 | B_pre = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(cont.theta_hat,[1,1,sys.p])),3); 11 | %% Part for standard constraints 12 | % Declare independent variables 13 | % l goes from 1:N 14 | % j goes from 1:nx_v 15 | z_lk = sdpvar(sys.n,cont.N+1,'full'); 16 | v_lk = sdpvar(sys.m,cont.N,'full'); 17 | alpha_lk = sdpvar(1,cont.N+1,'full'); 18 | Lambda_jlk = sdpvar(cont.nHx,sys.nHtheta,cont.nx_v,cont.N-1,'full'); 19 | Lambda_1k = sdpvar(cont.nHx,sys.nHtheta,'full'); 20 | % define state dependent variables 21 | x_jlk = []; 22 | u_jlk = []; 23 | d_jlk = []; 24 | D_jlk = []; 25 | for l = 2:cont.N 26 | x_jlk = cat(3,x_jlk,repmat(z_lk(:,l),1,cont.nx_v)+alpha_lk(l)*cont.x_v); 27 | u_jlk = cat(3,u_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_jlk(:,:,l-1)); 28 | 29 | d_jlk = cat(3,d_jlk,sys.A0*x_jlk(:,:,l-1)+sys.B0*u_jlk(:,:,l-1) - repmat(z_lk(:,l+1),1,cont.nx_v)); 30 | D_temp = []; 31 | for j = 1:cont.nx_v 32 | D_temp = cat(3,D_temp,D_mult(sys,x_jlk(:,j,l-1),u_jlk(:,j,l-1))); 33 | end 34 | D_jlk = cat(4,D_jlk,D_temp); 35 | end 36 | 37 | % define constraints. 38 | stdConstraints = [Lambda_jlk(:)>=0,Lambda_1k(:)>=0]; 39 | stdConstraints = [stdConstraints,z_lk(:,1)==x_pre,alpha_lk(1)==0]; 40 | stdConstraints = [stdConstraints,... 41 | (sys.F+sys.G*cont.K)*x_pre + sys.G*v_lk(:,1) <= ones(sys.nc,1),... 42 | Lambda_1k*h_pre + cont.H_x*(sys.A0*x_pre+sys.B0*(cont.K*x_pre+v_lk(:,1))-z_lk(:,2)) - alpha_lk(2)*ones(cont.nHx,1) <= -cont.w_bar,... 43 | cont.H_x*D_mult(sys,x_pre,cont.K*x_pre+v_lk(:,1)) == Lambda_1k*cont.H_theta 44 | ]; 45 | for l = 2:cont.N 46 | stdConstraints = [stdConstraints, ... 47 | (sys.F+sys.G*cont.K)*z_lk(:,l) + sys.G*v_lk(:,l) + alpha_lk(l)*cont.f_bar <= ones(sys.nc,1)]; 48 | 49 | for j = 1:cont.nx_v 50 | stdConstraints = [stdConstraints, ... 51 | Lambda_jlk(:,:,j,l-1)*h_pre + cont.H_x*d_jlk(:,j,l-1) - alpha_lk(l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 52 | 53 | stdConstraints = [stdConstraints, ... 54 | cont.H_x*D_jlk(:,:,j,l-1) == Lambda_jlk(:,:,j,l-1)*cont.H_theta]; 55 | end 56 | end 57 | 58 | % define terminal constraints 59 | stdConstraints = [stdConstraints, ... 60 | z_lk(:,cont.N+1) == zeros(sys.n,1), ... 61 | alpha_lk(cont.N+1) <= cont.alpha_bar... 62 | alpha_lk(cont.N+1) >= cont.alpha_min]; 63 | 64 | %% Part for exploration 65 | 66 | % Declare variables for exploration 67 | if cont.nPred_X>1 68 | Lambda_til_jlk = sdpvar(cont.nHx,sys.nHtheta+cont.nPred_theta*sys.nHw,cont.nx_v,cont.nPred_X-1,'full'); 69 | else 70 | Lambda_til_jlk = []; 71 | end 72 | Lambda_til_1k = sdpvar(cont.nHx,sys.nHtheta+cont.nPred_theta*sys.nHw); 73 | z_til_lk = sdpvar(sys.n,cont.nPred_X+1,'full'); 74 | alpha_til_lk = sdpvar(1,cont.nPred_X+1,'full'); 75 | 76 | % predict state evolution 77 | x_hat = x_pre; 78 | for l = 1:cont.nPred_theta-1 79 | x_hat = [x_hat (A_pre+B_pre*cont.K)*x_hat(:,l) + B_pre*v_lk(:,l)]; 80 | end 81 | 82 | % predict new constraints on theta 83 | H_pred = [sys.H_w*D_mult(sys,x_pre,cont.K*x_pre+v_lk(:,1))]; 84 | h_pred = [sys.h_w + sys.H_w*D_mult(sys,x_pre,cont.K*x_pre+v_lk(:,1))*theta_pre]; 85 | for l = 2:cont.nPred_theta 86 | H_pred = [H_pred; 87 | sys.H_w*D_mult(sys,x_hat(:,l),cont.K*x_hat(:,l)+v_lk(:,l))]; 88 | h_pred = [h_pred; 89 | sys.h_w + sys.H_w*D_mult(sys,x_hat(:,l),cont.K*x_hat(:,l)+v_lk(:,l))*theta_pre]; 90 | end 91 | 92 | % Append to H_theta 93 | H_theta_til = [cont.H_theta; H_pred]; 94 | h_theta_til = [h_pre; h_pred]; 95 | 96 | % define exploration variables 97 | x_til_jlk = []; 98 | u_til_jlk = []; 99 | d_til_jlk = []; 100 | D_til_jlk = []; 101 | 102 | for l = 2:cont.nPred_X+1 103 | x_til_jlk = cat(3,x_til_jlk,repmat(z_til_lk(:,l),1,cont.nx_v)+alpha_til_lk(l)*cont.x_v); 104 | u_til_jlk = cat(3,u_til_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_til_jlk(:,:,l-1)); 105 | end 106 | for l = 2:cont.nPred_X 107 | d_til_jlk = cat(3,d_til_jlk,sys.A0*x_til_jlk(:,:,l-1)+sys.B0*u_til_jlk(:,:,l-1) - repmat(z_til_lk(:,l+1),1,cont.nx_v)); 108 | D_temp = []; 109 | for j = 1:cont.nx_v 110 | D_temp = cat(3,D_temp,D_mult(sys,x_til_jlk(:,j,l-1),u_til_jlk(:,j,l-1))); 111 | end 112 | D_til_jlk = cat(4,D_til_jlk,D_temp); 113 | end 114 | 115 | % define exploration constraints 116 | exploreConstraints = [Lambda_til_jlk(:)>=0,Lambda_til_1k(:)>=0]; 117 | exploreConstraints = [exploreConstraints,z_til_lk(:,1)==x_pre,alpha_til_lk(1)==0]; 118 | exploreConstraints = [exploreConstraints,... 119 | Lambda_til_1k*h_theta_til + cont.H_x*(sys.A0*x_pre+sys.B0*(cont.K*x_pre+v_lk(:,1))-z_til_lk(:,2)) - alpha_til_lk(2)*ones(cont.nHx,1) <= -cont.w_bar,... 120 | cont.H_x*D_mult(sys,x_pre,cont.K*x_pre+v_lk(:,1)) == Lambda_til_1k*H_theta_til 121 | ]; 122 | % implement containment conditions of tubes 123 | for l = 2:cont.nPred_X 124 | % exploreConstraints = [exploreConstraints, ... 125 | % (sys.F+sys.G*cont.K)*z_til_lk(:,l) + sys.G*v_lk(:,l) + alpha_til_lk(l)*cont.f_bar <= ones(sys.nc,1) 126 | % ]; 127 | for j = 1:cont.nx_v 128 | exploreConstraints = [exploreConstraints, ... 129 | Lambda_til_jlk(:,:,j,l-1)*h_theta_til + cont.H_x*d_til_jlk(:,j,l-1) - alpha_til_lk(l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 130 | 131 | exploreConstraints = [exploreConstraints, ... 132 | cont.H_x*D_til_jlk(:,:,j,l-1) == Lambda_til_jlk(:,:,j,l-1)*H_theta_til]; 133 | end 134 | end 135 | % at the end, ensure the X_til_lk tube is inside the X_lk tube 136 | for j = 1:cont.nx_v 137 | exploreConstraints = [exploreConstraints, ... 138 | cont.H_x*(z_til_lk(:,end)+alpha_til_lk(end)*cont.x_v(:,j) - z_lk(:,cont.nPred_X+1)) <= alpha_lk(cont.nPred_X+1)*cont.vec_1_x; ]; 139 | end 140 | %% Cost function definition 141 | 142 | % define cost variables 143 | stage_cost_max = sdpvar(cont.N+1,1,'full'); 144 | J = sum(stage_cost_max); 145 | 146 | % define cost constraints 147 | costConstraints = [norm(cont.R_L*(cont.K*x_pre+v_lk(:,1)),'inf')<=stage_cost_max(1)]; 148 | 149 | for l = 2:cont.N 150 | for j = 1:cont.nx_v 151 | if l<=cont.nPred_X+1 152 | costConstraints = [costConstraints, ... 153 | norm(cont.Q_L*x_til_jlk(:,j,l-1),'inf') + norm(cont.R_L*(cont.K*x_til_jlk(:,j,l-1)+v_lk(:,l)),'inf') <= stage_cost_max(l) 154 | ]; 155 | else 156 | costConstraints = [costConstraints, ... 157 | norm(cont.Q_L*x_jlk(:,j,l-1),'inf') + norm(cont.R_L*(cont.K*x_jlk(:,j,l-1)+v_lk(:,l)),'inf') <= stage_cost_max(l) 158 | ]; 159 | end 160 | end 161 | end 162 | % terminal cost 163 | for j = 1:cont.nx_v 164 | costConstraints = [costConstraints, ... 165 | norm(cont.Q_L*cont.x_v(:,j),'inf') + norm(cont.R_L*cont.K*cont.x_v(:,j),'inf') <= stage_cost_max(cont.N+1) 166 | ]; 167 | end 168 | 169 | 170 | %% Generate optimizer 171 | Constraints = [stdConstraints;exploreConstraints;costConstraints]; 172 | options = sdpsettings('solver','ipopt','usex0',1); 173 | 174 | optProb = optimizer(Constraints,J,options,[x_pre;h_pre;theta_pre],cont.K*x_pre+v_lk(:,1)); 175 | 176 | end 177 | 178 | -------------------------------------------------------------------------------- /controller_pre.m: -------------------------------------------------------------------------------- 1 | function optProb = controller_pre(sys,cont) 2 | 3 | % function to generate optimizer using yalmip 4 | 5 | % define presolve variables 6 | x_pre = sdpvar(sys.n,1,'full'); 7 | h_pre = sdpvar(sys.nHtheta,1,'full'); 8 | %% setup optimization problem 9 | 10 | % Declare independent variables 11 | % l goes from 1:N 12 | % j goes from 1:nx_v 13 | z_lk = sdpvar(sys.n,cont.N+1,'full'); 14 | v_lk = sdpvar(sys.m,cont.N,'full'); 15 | alpha_lk = sdpvar(1,cont.N+1,'full'); 16 | Lambda_jlk = sdpvar(cont.nHx,sys.nHtheta,cont.nx_v,cont.N-1,'full'); 17 | Lambda_1k = sdpvar(cont.nHx,sys.nHtheta,'full'); 18 | 19 | % define state dependent variables 20 | x_jlk = []; 21 | u_jlk = []; 22 | d_jlk = []; 23 | D_jlk = []; 24 | for l = 2:cont.N 25 | x_jlk = cat(3,x_jlk,repmat(z_lk(:,l),1,cont.nx_v)+alpha_lk(:,l)*cont.x_v); 26 | u_jlk = cat(3,u_jlk,repmat(v_lk(:,l),1,cont.nx_v)+cont.K*x_jlk(:,:,l-1)); 27 | 28 | d_jlk = cat(3,d_jlk,sys.A0*x_jlk(:,:,l-1)+sys.B0*u_jlk(:,:,l-1) - repmat(z_lk(:,l+1),1,cont.nx_v)); 29 | D_temp = []; 30 | for j = 1:cont.nx_v 31 | D_temp = cat(3,D_temp,D_mult(sys,x_jlk(:,j,l-1),u_jlk(:,j,l-1))); 32 | end 33 | D_jlk = cat(4,D_jlk,D_temp); 34 | end 35 | 36 | % define constraints. 37 | Constraints = [Lambda_jlk(:)>=0,Lambda_1k(:)>=0]; 38 | Constraints = [Constraints,z_lk(:,1)==x_pre,alpha_lk(1)==0]; 39 | Constraints = [Constraints,... 40 | (sys.F+sys.G*cont.K)*x_pre + sys.G*v_lk(:,1) <= ones(sys.nc,1),... 41 | Lambda_1k*h_pre + cont.H_x*(sys.A0*x_pre+sys.B0*(cont.K*x_pre+v_lk(:,1))-z_lk(:,2)) - alpha_lk(2)*ones(cont.nHx,1) <= -cont.w_bar,... 42 | cont.H_x*D_mult(sys,x_pre,cont.K*x_pre+v_lk(:,1)) == Lambda_1k*cont.H_theta 43 | ]; 44 | for l = 2:cont.N 45 | Constraints = [Constraints, ... 46 | (sys.F+sys.G*cont.K)*z_lk(:,l) + sys.G*v_lk(:,l) + alpha_lk(:,l)*cont.f_bar <= ones(sys.nc,1)]; 47 | 48 | for j = 1:cont.nx_v 49 | Constraints = [Constraints, ... 50 | Lambda_jlk(:,:,j,l-1)*h_pre + cont.H_x*d_jlk(:,j,l-1) - alpha_lk(:,l+1)*ones(cont.nHx,1) <= -cont.w_bar]; 51 | 52 | Constraints = [Constraints, ... 53 | cont.H_x*D_jlk(:,:,j,l-1) == Lambda_jlk(:,:,j,l-1)*cont.H_theta]; 54 | end 55 | end 56 | 57 | % define terminal constraints 58 | Constraints = [Constraints, ... 59 | z_lk(:,cont.N+1) == zeros(sys.n,1), ... 60 | alpha_lk(cont.N+1) <= cont.alpha_bar ... 61 | alpha_lk(cont.N+1) >= cont.alpha_min]; 62 | 63 | 64 | 65 | %% Cost function definition 66 | 67 | % define cost variables 68 | stage_cost_max = sdpvar(cont.N+1,1,'full'); 69 | J = sum(stage_cost_max); 70 | 71 | costConstraints = [norm(cont.R_L*(cont.K*x_pre+v_lk(:,1)),'inf')<=stage_cost_max(1)]; 72 | 73 | for l = 2:cont.N 74 | for j = 1:cont.nx_v 75 | costConstraints = [costConstraints, ... 76 | norm(cont.Q_L*x_jlk(:,j,l-1),'inf') + norm(cont.R_L*(cont.K*x_jlk(:,j,l-1)+v_lk(:,l)),'inf') <= stage_cost_max(l) 77 | ]; 78 | end 79 | end 80 | % terminal cost 81 | for j = 1:cont.nx_v 82 | costConstraints = [costConstraints, ... 83 | norm(cont.Q_L*cont.x_v(:,j),'inf') + norm(cont.R_L*cont.K*cont.x_v(:,j),'inf') <= stage_cost_max(cont.N+1) 84 | ]; 85 | end 86 | 87 | 88 | options = sdpsettings('solver','gurobi','verbose',0,'debug',0); 89 | 90 | 91 | optProb = optimizer([Constraints,costConstraints],J,options,[x_pre;h_pre],cont.K*x_pre+v_lk(:,1)); 92 | 93 | end -------------------------------------------------------------------------------- /model.m: -------------------------------------------------------------------------------- 1 | classdef model 2 | properties (Constant) 3 | theta_true = [0.95; 0.3]; 4 | end 5 | properties 6 | A_true 7 | B_true 8 | w_bound 9 | x 10 | end 11 | 12 | 13 | methods 14 | % Constructor 15 | function obj = model(sys,x0) 16 | obj.A_true = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(obj.theta_true,[1,1,sys.p])),3); 17 | obj.B_true = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(obj.theta_true,[1,1,sys.p])),3); 18 | 19 | obj.w_bound = sys.w_bound; 20 | obj.x = x0; 21 | end 22 | 23 | % simulate 24 | function obj = simulate(obj,u) 25 | % works only for uniform w bounds [not general polytope Hw*w<=1] 26 | w = -obj.w_bound + 2*obj.w_bound*rand(length(obj.x),1); 27 | 28 | obj.x = obj.A_true*obj.x + obj.B_true*u+w; 29 | end 30 | 31 | end 32 | 33 | end -------------------------------------------------------------------------------- /script.m: -------------------------------------------------------------------------------- 1 | %% Script to perform Robust Adaptive MPC 2 | 3 | clc; 4 | clear; 5 | 6 | sys = system_desc(); 7 | %% Define controller parameters 8 | 9 | % prediction horizon 10 | cont.N = 8; 11 | 12 | % cost matrices 13 | cont.Q = eye(sys.n); 14 | cont.R = eye(sys.m); 15 | cont.P = [2.8 0.56; 16 | 0.56 2.5]; 17 | cont.Q_L = chol(cont.Q); 18 | cont.R_L = chol(cont.R); 19 | 20 | 21 | % block size for updating h_theta_k 22 | cont.blk = 10; 23 | 24 | % Define state tube shape : H_x*x <= vec_1_x, vertices: x_v(:,i) 25 | cont.H_x = [eye(sys.n); -eye(sys.n)]; 26 | cont.vec_1_x = ones(length(cont.H_x),1); 27 | cont.nHx = size(cont.H_x,1); % denoted u in Lorenzen(2019) 28 | cont.x_v = [1 1; 1 -1; -1 1; -1 -1 ]'; 29 | cont.nx_v = length(cont.x_v); % number of vertices 30 | 31 | % prestabilizing gain 32 | addpath('Functions') 33 | cont.w_bar = compute_wbar(sys,cont); 34 | [cont.K,cont.alpha_bar,cont.alpha_min] = prestab_controller(sys,cont); 35 | 36 | % parameter bounds 37 | cont.H_theta = sys.H_theta; 38 | cont.h_theta_k = sys.h_theta; 39 | cont.h_theta_0 = sys.h_theta; 40 | 41 | % Estimation parameters 42 | cont.mu = 2; 43 | cont.theta_hat = [0.5;0.5;]; 44 | cont.x_hat_k = sys.x0; 45 | cont.A_est = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(cont.theta_hat,[1,1,sys.p])),3); 46 | cont.B_est = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(cont.theta_hat,[1,1,sys.p])),3); 47 | 48 | 49 | % Define terminal constraint: z_N|k = 0; h_T*alpha_N|k <= 1; 50 | cont.f_bar = max((sys.F+sys.G*cont.K)*cont.x_v,[],2); 51 | 52 | % Exploration: number of predictions 53 | cont.nPred_theta = 1; 54 | cont.nPred_X = 7; 55 | %% Define simulation parameters 56 | 57 | Tsim = 10; 58 | 59 | % initialize states and inputs 60 | x = NaN*ones(sys.n,Tsim+1); 61 | u = NaN*ones(sys.m,Tsim); 62 | u_std = NaN*ones(sys.m,Tsim); 63 | 64 | theta_hats = NaN*ones(sys.p,Tsim); 65 | J = 0; 66 | % regressors 67 | Dk = zeros(sys.n*cont.blk,sys.p); 68 | dk = zeros(sys.n*cont.blk,1); 69 | 70 | % initial condition 71 | x(:,1) = sys.x0; 72 | 73 | % model initialization 74 | true_sys = model(sys,x(:,1)); 75 | 76 | tic 77 | presolve = 1; 78 | if presolve 79 | % optProb1 = controller_pre(sys,cont); 80 | optProb2 = controller_expl_pre(sys,cont); 81 | end 82 | toc 83 | 84 | rng(10,'twister'); 85 | % simulate 86 | for k = 1:Tsim 87 | if any(k == [15, 30, 50]) 88 | figure; plotregion(-cont.H_theta,-cont.h_theta_k); 89 | xlim([-1 1]);ylim([-1 1]);zlim([-1 1]); 90 | end 91 | % update feasible set and parameter estimate 92 | cont = updateParameters(sys,cont,x(:,k),Dk,dk); 93 | theta_hats(:,k) = cont.theta_hat; 94 | 95 | % calculate control input 96 | tic 97 | if presolve 98 | % [u(:,k),a1,~,~,~,a5] = optProb1([x(:,k);cont.h_theta_k]); 99 | [u(:,k),a1,~,~,~,a5] = optProb2([x(:,k);cont.h_theta_k;cont.theta_hat]); 100 | if a1 101 | error(a5.infostr) 102 | end 103 | else 104 | u_std(:,k) = controller(sys,cont,x(:,k)); 105 | u(:,k) = controller_expl(sys,cont,x(:,k)); 106 | end 107 | toc 108 | 109 | % update state estimate 110 | cont.x_hat_k = cont.A_est*x(:,k)+cont.B_est*u(:,k); 111 | 112 | % Apply to true system 113 | true_sys = true_sys.simulate(u(:,k)); 114 | x(:,k+1) = true_sys.x; 115 | J = J + norm(cont.Q_L*x(:,k+1),'inf') + norm(cont.R_L*u(:,k),'inf'); 116 | 117 | % update regressors 118 | new_Dk = zeros(sys.n,sys.p); 119 | for i= 1:sys.p 120 | new_Dk(:,i) = sys.Ap(:,:,i)*x(:,k) + sys.Bp(:,:,i)*u(:,k); 121 | end 122 | new_dk = -x(:,k+1)+sys.A0*x(:,k)+sys.B0*u(:,k); 123 | 124 | Dk = [Dk(sys.n+1:end,:);new_Dk]; 125 | dk = [dk(sys.n+1:end);new_dk]; 126 | 127 | end 128 | toc 129 | %% 130 | f = 60; 131 | h = figure(f); f=f+1; 132 | subplot(411) 133 | hold on; 134 | plot(0:Tsim,x(1,:),'-*') 135 | ylabel('$x_1$') 136 | 137 | subplot(412) 138 | hold on; 139 | plot(0:Tsim,x(2,:),'-*') 140 | ylabel('$x_2$') 141 | 142 | subplot(413) 143 | hold on; 144 | plot(0:Tsim-1,u(1,:),'-*') 145 | xlim([0 Tsim]) 146 | ylabel('$u_1$') 147 | 148 | subplot(414) 149 | hold on; 150 | plot(0:Tsim-1,u(2,:),'-*') 151 | xlim([0 Tsim]) 152 | ylabel('$u_2$') 153 | % h = figure(f);f=f+1; 154 | % plotregion(-cont.H_theta,-cont.h_theta_k); 155 | % xlim([-1 1]);ylim([-1 1]);zlim([-1 1]); -------------------------------------------------------------------------------- /system_desc.m: -------------------------------------------------------------------------------- 1 | function sys = system_desc() 2 | 3 | % define matrices 4 | sys.A0 = [ 0.85 0.5; 5 | 0.2 0.6 ]; 6 | sys.Ap(:,:,1) = [0.1 0 7 | 0 0.1]; 8 | sys.Ap(:,:,2) = zeros(2,2); 9 | 10 | sys.B0 = [1.0 0.4; 11 | 0.2 0.4]; 12 | sys.Bp(:,:,1) = zeros(2,2); 13 | sys.Bp(:,:,2) = [0 0.5; 0 0.4]; 14 | 15 | % define dimensions 16 | sys.n = size(sys.Bp,1); 17 | sys.m = size(sys.Bp,2); 18 | sys.p = size(sys.Bp,3); 19 | 20 | % define bounds on theta: H_theta*theta <= h_theta 21 | sys.H_theta = [eye(sys.p);-eye(sys.p)]; 22 | sys.h_theta = [ones(sys.p,1);ones(sys.p,1)]; 23 | sys.H_theta_v = [1 1; 1 -1; -1 1; -1 -1]'; 24 | 25 | 26 | % Generate H_theta with a predetermined complexity 27 | sys = boundedComplexity(sys); 28 | sys.nHtheta = length(sys.h_theta); 29 | 30 | % define disturbance bounds: H_w*w <= h_w 31 | sys.w_bound = 0.1; 32 | sys.H_w = [eye(sys.n);-eye(sys.n)]/sys.w_bound; 33 | sys.h_w = [ones(sys.n,1);ones(sys.n,1)]; 34 | sys.nHw = length(sys.h_w); 35 | 36 | 37 | % define state and input constraints: F*x + G*u <= vec_1_cons 38 | sys.F = [-0.1 0; 39 | 0 -0.1; 40 | zeros(4,2)]; 41 | sys.G = [zeros(2,2); 1 0; -1/0.5 0; 0 1/2; 0 -1/2]; 42 | sys.nc = size(sys.F,1); 43 | sys.vec_1_cons = ones(sys.nc,1); 44 | 45 | % define box constraint on x to find mu: Box_x_v: vertices of box 46 | sys.Box_x = [eye(sys.n);-eye(sys.n)]; 47 | sys.box_x = 5*[ones(sys.n,1);ones(sys.n,1)]; 48 | sys.Box_x_v = 5*[1,1; 1,-1; -1,1; -1,-1]'; 49 | sys.Box_u_v = [1; -1]'; 50 | 51 | sys.x0 = [1;1.5]; 52 | end 53 | 54 | -------------------------------------------------------------------------------- /updateParameters.m: -------------------------------------------------------------------------------- 1 | function cont = updateParameters(sys,cont,xk,Dk,dk) 2 | 3 | H_til = cont.H_theta; 4 | h_til = cont.h_theta_k; 5 | 6 | %% update parameter bounds 7 | for i = 1:cont.blk 8 | idx1 = sys.n*(i-1)+1; 9 | idx2 = sys.n*i; 10 | 11 | H_til = [H_til 12 | -sys.H_w*Dk(idx1:idx2,:);]; 13 | 14 | h_til = [h_til; 15 | ones(length(sys.h_w),1) + sys.H_w*dk(idx1:idx2)]; 16 | end 17 | 18 | r = length(cont.h_theta_k); 19 | r2 = length(h_til); 20 | 21 | cvx_begin quiet 22 | variable h(r,1) 23 | variable Lam(r,r2) 24 | 25 | minimize(sum(h)) 26 | 27 | subject to 28 | Lam(:)>=0; 29 | Lam*H_til==cont.H_theta; 30 | Lam*h_til<=h; 31 | cvx_end 32 | 33 | if ~strcmp(cvx_status,'Solved') 34 | warning(cvx_status) 35 | flag = 1; 36 | u = []; 37 | return; 38 | end 39 | 40 | % update bounds of polytope 41 | cont.h_theta_k = h; 42 | 43 | %% Estimate of theta 44 | theta_til = cont.theta_hat + cont.mu * Dk(end-sys.n+1:end,:)'*(xk-cont.x_hat_k); 45 | 46 | if any(cont.H_theta*theta_til>=cont.h_theta_k) 47 | % theta_til outside the initial bounds, use projection 48 | cvx_begin 49 | variable theta_hat_p(sys.p,1) 50 | minimize sum((theta_hat_p-theta_til).^2) 51 | 52 | subject to 53 | cont.H_theta*theta_hat_p <= cont.h_theta_k; 54 | 55 | cvx_end 56 | 57 | if ~all(cvx_status=='Solved') 58 | warning(cvx_status) 59 | flag = 1; 60 | u = []; 61 | return; 62 | else 63 | cont.theta_hat = theta_hat_p; 64 | end 65 | 66 | else 67 | cont.theta_hat = theta_til; 68 | end 69 | 70 | cont.A_est = sys.A0+ sum(bsxfun(@times,sys.Ap,reshape(cont.theta_hat,[1,1,sys.p])),3); 71 | cont.B_est = sys.B0+ sum(bsxfun(@times,sys.Bp,reshape(cont.theta_hat,[1,1,sys.p])),3); 72 | end --------------------------------------------------------------------------------