controller_mpc_3.m

% BRIEF:
%   Controller function template. Input and output dimension MUST NOT be
%   modified.
% INPUT:
%   Q: State weighting matrix, dimension (3,3)
%   R: Input weighting matrix, dimension (3,3)
%   T: Measured system temperatures, dimension (3,1)
%   N: MPC horizon length, dimension (1,1)
% OUTPUT:
%   p: Heating and cooling power, dimension (3,1)

function p = controller_mpc_3(Q, R, T, N, ~)
    % controller variables
    %N = 30;

    persistent param yalmip_optimizer

    % initialize controller, if not done already
    if isempty(param)
        [param, yalmip_optimizer] = init(Q, R, N);
    end

% evaluate control action by solving MPC problem
    [u_mpc,errorcode] = yalmip_optimizer(T-param.T_sp);
    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
    [K , P] = dlqr(param.A , param.B , Q , R);
    % get terminal set
    clear compute_X_LQR
    [A_x, b_x] = compute_X_LQR(Q, R);

    % implement your MPC using Yalmip here
    %https://www.mpt3.org/Main/CustomMPC
    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 = 0; 
    constraints = [ X{1} == T0];
    
    for k = 1:N-1
        % assing domain
        constraints = [constraints; X{k+1} == param.A*X{k} + param.B*U{k} ];

        % input constraints
        constraints = [constraints; param.Ucons(:,1) <= U{k} <= param.Ucons(:,2) ];

        % state constraints
        constraints = [constraints; param.Xcons(:,1) <= X{k+1} <= param.Xcons(:,2)];
        
        % objective (updating the cost function bu summing up stage costs)
        objective = objective +  X{k}'*Q*X{k} + U{k}'*R*U{k};
    end
    
    
    
    % terminal set
    constraints = [constraints, A_x * X{end} <= b_x];
    
    objective = objective + X{end}' * P * X{end}; 
    ops = sdpsettings('verbose',0,'solver','quadprog');
    yalmip_optimizer = optimizer(constraints,objective,ops,T0,U{1});
        
end