MATLAB Code for Offset-Free MPC Control of Heated Swimming Pool System with input constraints

mpc_with_input_constraints.m

% MATLAB Code for Offset-Free MPC Control of Heated Swimming Pool System

% Clear workspace and command window
clear; close all; clc;

% System Parameters
A = 0.7788;
B = 0.0442;
C = 1;       % Output matrix

% Constants
d_const = 15;       % Constant disturbance (ambient air temperature)
w = 20;             % Set-point temperature (desired water temperature)
N2 = 10;            % Prediction horizon
Nu = 3;             % Control horizon
QQ = 1;              % Weight on output error
RR = 0;              % Weight on control input (set to zero)

% Simulation Parameters
T_sim = 50;         % Total simulation time steps
x0 = 15;            % Initial water temperature
u0 = 0;             % Initial control input

% Augmented State-Space Matrices
M = [A, B;      % [state dynamics]
     0, 1];     % [input dynamics]
N = [B; 1];     % [Input matrix]
Q = [C, 0];     % [Output matrix]


% Observer Design (Estimator Gain L)
% Desired observer poles (distinct and within unit circle)
observer_poles = [0.3, 0.45];
L = place(M', Q', observer_poles)';

% Initialize Variables
x_hat = zeros(2, T_sim+1);       % State vector
x_hat(:,1) = [x0; u0];           % Initial state vector
u = zeros(1, T_sim);             % Control history
y = zeros(1, T_sim);             % Output history
ym = zeros(1, T_sim);            % Measured output history

% Precompute Prediction Matrices F and H
F = zeros(N2, 2);
H = zeros(N2, Nu);

% Compute F and H Matrices
for i = 1:N2
    F(i,:) = Q * (M^i);
    for j = 1:Nu
        if i >= j
            H(i,j) = Q * (M^(i-j)) * N;
        else
            H(i,j) = 0;
        end
    end
end


% Reference Trajectory
w_vec = w * ones(N2, 1);  % Reference over prediction horizon

% Simulation Loop
for t = 1:T_sim
    % Output Measurement   
    ym(t) = Q * (x_hat(:,t) + [0.2212*d_const; 0]);  % System output with disturbance

    % model observation
    y(t) = Q * x_hat(:,t);

    % State Estimation (Observer Update)
    x_hat(:,t) = x_hat(:,t) + L * (ym(t) - y(t));

    % Compute Control Law
    G = (H' * QQ * H) + (RR * eye(Nu)); %hessian
    f =  H' * QQ * (F * x_hat(:,t) - w_vec);

    % Input Constraints
    A_cum = tril(ones(Nu)); % Cumulative sum matrix
    Aineq = [A_cum; -A_cum];
    bineq = [40 * ones(Nu,1) - x_hat(2,t); x_hat(2,t) * ones(Nu,1)];
    % Solve Quadratic Program
    options = optimoptions('quadprog', 'Display', 'off');
    [delta_u, fval, exitflag, output, lambda] = quadprog(G, f, Aineq, bineq, [], [], [], [], [], options);

    % System Update
    x_hat(:,t+1) = M * x_hat(:,t) + N * delta_u(1);  % update states
    % Log Control Input
    u(t) = x_hat(2,t+1);
    disp(u(t))
end

% Plot Results
time = 0:T_sim-1;

figure;
subplot(2,1,1);
plot(time, y, 'b-', 'LineWidth', 2);
hold on;
plot(time, w * ones(size(time)), 'r--', 'LineWidth', 1.5);
hold on;
plot(time, d_const * ones(size(time)), 'g-', 'LineWidth', 1.5);
xlabel('Time Step');
ylabel('Water Temperature (°C)');
title('Water Temperature Response');
legend('Temperature', 'Set-point','Ambient Temperature');
ylim([14 23]);
grid on;

subplot(2,1,2);
stairs(time, u, 'k-', 'LineWidth', 2);
xlabel('Time Step');
ylabel('Heater Input Power');
title('Control Input');
grid on;

% Display Final Water Temperature
fprintf('Final Water Temperature: %.2f°C\n', y(end));