dev

src/compute_X_LQR.m

@@ -28,10 +28,10 @@
     systemLQR.x.setConstraint = Xp;
 
     InvSet = systemLQR.invariantSet();
-    figure;
-    InvSet.plot(), alpha(0.25);
-    title('Resulting State Constraints under LQR Control'), xlabel('\Delta T_{VC}'), ylabel('\Delta T_{F1}'), zlabel('\Delta T_{F2}')
-    hold on; 
+%     figure;
+%     InvSet.plot(), alpha(0.25);
+%     title('Control Invariant Set under LQR Controller'), xlabel('\Delta T_{VC}'), ylabel('\Delta T_{F1}'), zlabel('\Delta T_{F2}')
+%     hold on; 
 
     A_x = InvSet.A;
     b_x = InvSet.b;

src/compute_controller_base_parameters.m

@@ -1,17 +1,18 @@
-function param = compute_controller_base_parameters
+function param = compute_controller_base_parameters()
     % load truck parameters
     load('system/parameters_building');
 
+
     % Task 1: continuous time dynamics in state space form
     Bdc = diag([1/building.m_VC, 1/building.m_F1, 1/building.m_F2]);
     Ac = Bdc * ...
         [-building.a_F1_VC - building.a_F2_VC - building.a_Env_VC, building.a_F1_VC, building.a_F2_VC;...
-        building.a_F1_VC, -building.a_F1_VC - building.a_F2_F1, building.a_F2_F1;...
-        building.a_F2_VC, building.a_F2_F1, -building.a_F2_VC - building.a_F2_F1];
+         building.a_F1_VC, -building.a_F1_VC - building.a_F2_F1, building.a_F2_F1;...
+         building.a_F2_VC, building.a_F2_F1, -building.a_F2_VC - building.a_F2_F1];
     Bc = Bdc * ...
         [building.b_11, building.b_12, building.b_13;...
-        building.b_21, building.b_22, building.b_23;...
-        building.b_31, building.b_32, building.b_33];
+         building.b_21, building.b_22, building.b_23;...
+         building.b_31, building.b_32, building.b_33];
     d = [building.d_VC + building.a_Env_VC * building.T_Env; building.d_F1; building.d_F2];
 
     % Task 2: discretization

src/controller_lqr.m

@@ -17,13 +17,15 @@
     param = init(Q, R);
 end
 
-[Klqr,~,~] = dlqr(param.A, param.B, Q, R);
-p = param.p_sp - Klqr * (T - param.T_sp);
+% compute control action
+[Klqr, ~, ~] = dlqr(param.A, param.B, Q, R);
+p = param.p_sp - Klqr*(T-param.T_sp);
+
 end
 
 function param = init(Q, R)
 % get basic controller parameters
-param = compute_controller_base_parameters;
+param = compute_controller_base_parameters();
 % add additional parameters if necessary, e.g.
-% param.F = ...
+% param.F = ;
 end

src/controller_mpc_1.m

@@ -18,7 +18,7 @@
     [param, yalmip_optimizer] = init(Q, R, N);
 end
 
-% evaluate control action by solving MPC problem
+% Evaluate control action by solving MPC problem
 [u_mpc, errorcode] = yalmip_optimizer(T);
 if errorcode ~= 0
     warning('MPC1 infeasible');
@@ -28,19 +28,42 @@
 
 end
 
-
 function [param, yalmip_optimizer] = init(Q, R, N)
 % get basic controller parameters
-param = compute_controller_base_parameters;
+% ...
+% get terminal cost
+% ...
 % implement your MPC using Yalmip here
-nx = size(param.A,1);
+% nx = size(param.A,1);
+% nu = size(param.B,2);
+% U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
+% X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
+% T0 = sdpvar(nx,1,'full');
+% objective = ...;
+% constraints = [...];
+% for k = 1:N-1
+%     constraints = [constraints,... ];
+%     objective = objective + ...;
+% end
+% objective = objective + ...;
+% ops = sdpsettings('verbose',0,'solver','quadprog');
+% yalmip_optimizer = optimizer(constraints,objective,ops,...,...);
+
+% get basic controller parameters
+param = compute_controller_base_parameters;
+yalmip('clear');
+
+% Implement your MPC using Yalmip here
+nx = size(param.A,2);
 nu = size(param.B,2);
 U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
 X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
 T0 = sdpvar(nx,1,'full');
+X0 = T0 - param.T_sp;
+
 objective = 0;
 constraints = [];
-constraints = [constraints, X{1} == T0 - param.T_sp];
+constraints = [constraints, X{1} == X0];
 
 for k = 1:N-1
     constraints = [constraints, X{k+1} == param.A * X{k} + param.B * U{k}];
@@ -53,8 +76,6 @@
 [~, P_inf, ~] = dlqr(param.A, param.B, Q, R);
 l_f = X{N}' * P_inf * X{N};
 objective = objective + l_f;
-
 ops = sdpsettings('verbose', 0, 'solver', 'quadprog');
 yalmip_optimizer = optimizer(constraints, objective, ops, T0, U{1});
-
 end

src/controller_mpc_1_forces.m

@@ -19,11 +19,12 @@
 end
 
 % evaluate control action by solving MPC problem
-% [u_mpc,errorcode] = forces_optimizer(...);
-% if (errorcode ~= 1)
-%     warning('MPC1 infeasible');
-% end
-% p = ...
+[u_mpc, errorcode] = forces_optimizer(T);
+if errorcode ~= 0
+    warning('MPC1_Forces infeasible');
+end
+
+p = u_mpc + param.p_sp;
 end
 
 function [param, forces_optimizer] = init(Q, R, N)
@@ -46,4 +47,33 @@
 % objective = objective + ...;
 % codeoptions = ...;
 % forces_optimizer = optimizerFORCES(...);
+% get basic controller parameters
+param = compute_controller_base_parameters;
+yalmip('clear');
+
+% Implement your MPC using Yalmip here
+nx = size(param.A,2);
+nu = size(param.B,2);
+U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
+X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
+T0 = sdpvar(nx,1,'full');
+X0 = T0 - param.T_sp;
+
+objective = 0;
+constraints = [];
+constraints = [constraints, X{1} == X0];
+
+for k = 1:N-1
+    constraints = [constraints, X{k+1} == param.A * X{k} + param.B * U{k}];
+    constraints = [constraints, param.Xcons(:,1) <= X{k+1} <= param.Xcons(:,2)];
+    constraints = [constraints, param.Ucons(:,1) <= U{k} <= param.Ucons(:,2)];
+    objective = objective + U{k}' * R * U{k} + X{k}' * Q * X{k};
+end
+
+% Task 10: get terminal cost
+[~, P_inf, ~] = dlqr(param.A, param.B, Q, R);
+l_f = X{N}' * P_inf * X{N};
+objective = objective + l_f;
+ops = getOptions('simpleMPC_solver');
+forces_optimizer = optimizerFORCES(constraints, objective, ops, T0, U{1}, {'xinit'}, {'u0'});
 end

src/controller_mpc_2.m

@@ -19,28 +19,49 @@
 end
 
 % evaluate control action by solving MPC problem
-[u_mpc, errorcode] = yalmip_optimizer(T);
-if errorcode ~= 0
+[u_mpc,errorcode] = yalmip_optimizer(T);
+if (errorcode ~= 0)
     warning('MPC2 infeasible');
 end
-
 p = u_mpc + param.p_sp;
-
 end
 
-
 function [param, yalmip_optimizer] = init(Q, R, N)
 % get basic controller parameters
-param = compute_controller_base_parameters;
+% ...
+% get terminal cost
+% ...
 % implement your MPC using Yalmip here
-nx = size(param.A,1);
+% nx = size(param.A,1);
+% nu = size(param.B,2);
+% U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
+% X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
+% T0 = sdpvar(nx,1,'full');
+% objective = ...;
+% constraints = [...];
+% for k = 1:N-1
+%     constraints = [constraints,...];
+%     objective = objective + ...;
+% end
+% constraints = [constraints, ...];
+% ops = sdpsettings('verbose',0,'solver','quadprog');
+% yalmip_optimizer = optimizer(constraints,objective,ops,...,...);
+
+% get basic controller parameters
+param = compute_controller_base_parameters;
+yalmip('clear');
+
+% Implement your MPC using Yalmip here
+nx = size(param.A,2);
 nu = size(param.B,2);
 U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
 X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
 T0 = sdpvar(nx,1,'full');
+X0 = T0 - param.T_sp;
+
 objective = 0;
 constraints = [];
-constraints = [constraints, X{1} == T0 - param.T_sp];
+constraints = [constraints, X{1} == X0];
 
 for k = 1:N-1
     constraints = [constraints, X{k+1} == param.A * X{k} + param.B * U{k}];
@@ -49,10 +70,8 @@
     objective = objective + U{k}' * R * U{k} + X{k}' * Q * X{k};
 end
 
-% terminal set constraint
-constraints = [constraints, X{N} == zeros(3,1)];
-
+constraints = [constraints, X{N} == zeros(3, 1)];
 ops = sdpsettings('verbose', 0, 'solver', 'quadprog');
 yalmip_optimizer = optimizer(constraints, objective, ops, T0, U{1});
 
-end
+end

src/controller_mpc_3.m

@@ -19,28 +19,51 @@
 end
 
 % evaluate control action by solving MPC problem
-[u_mpc, errorcode] = yalmip_optimizer(T);
-if errorcode ~= 0
+[u_mpc,errorcode] = yalmip_optimizer(T);
+if (errorcode ~= 0)
     warning('MPC3 infeasible');
 end
-
 p = u_mpc + param.p_sp;
-
 end
 
-
 function [param, yalmip_optimizer] = init(Q, R, N)
 % get basic controller parameters
-param = compute_controller_base_parameters;
+% ...
+% get terminal cost
+% ...
+% get terminal set
+% ...
 % implement your MPC using Yalmip here
-nx = size(param.A,1);
+% nx = size(param.A,1);
+% nu = size(param.B,2);
+% U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
+% X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
+% T0 = sdpvar(nx,1,'full');
+% objective = ...;
+% constraints = [...];
+% for k = 1:N-1
+%     constraints = [constraints,...];
+%     objective = objective + ...;
+% end
+% constraints = [constraints, ...];
+% objective = objective + ...;
+% ops = sdpsettings('verbose',0,'solver','quadprog');
+% yalmip_optimizer = optimizer(constraints,objective,ops,...,...);
+% get basic controller parameters
+param = compute_controller_base_parameters;
+yalmip('clear');
+
+% Implement your MPC using Yalmip here
+nx = size(param.A,2);
 nu = size(param.B,2);
 U = sdpvar(repmat(nu,1,N-1),ones(1,N-1),'full');
 X = sdpvar(repmat(nx,1,N),ones(1,N),'full');
 T0 = sdpvar(nx,1,'full');
+X0 = T0 - param.T_sp;
+
 objective = 0;
 constraints = [];
-constraints = [constraints, X{1} == T0 - param.T_sp];
+constraints = [constraints, X{1} == X0];
 
 for k = 1:N-1
     constraints = [constraints, X{k+1} == param.A * X{k} + param.B * U{k}];
@@ -49,16 +72,14 @@
     objective = objective + U{k}' * R * U{k} + X{k}' * Q * X{k};
 end
 
-% get terminal cost
 [~, P_inf, ~] = dlqr(param.A, param.B, Q, R);
 l_f = X{N}' * P_inf * X{N};
 objective = objective + l_f;
 
-% terminal set constraint
 [A_x, b_x] = compute_X_LQR(Q, R);
 constraints = [constraints, A_x * X{N} <= b_x];
-
 ops = sdpsettings('verbose', 0, 'solver', 'quadprog');
 yalmip_optimizer = optimizer(constraints, objective, ops, T0, U{1});
 
 end
+