%SWIMPOOL Runs swimming pool example
%
%     [wtemp,power,tvector] = swimpool(W,V,Tplant,kplant,ulim,Ts)
%
% Implements solution to Example 7.4 in the book:
% 'Predictive Control with Constraints'
% by J.M.Maciejowski (Pearson Education).
%
% Simulates control of pool water temperature, when air temperature
% varies sinusoidally and acts as an unmeasured disturbance. 
% An internal model of the disturbance, and a stabilising observer
% gain, are used.
%
% Input arguments:
%  W      Artificial state noise variance, used to find stabilising
%          observer. Default: 1 
%  V      Artificial measurement noise, as above.  Default: 1e-3
%  Tplant Time constant of pool (Model has 1 hour). Default: 1
%  kplant Heater gain of pool (Model has 0.2 degC/kW). Default: 0.2
%  ulim   Input constraints. Default: [-inf, inf, 1e6]
%  Ts     Sampling time. Default: 0.25 hour
%
% MPC assumes Q=1, R=0, Hp=10, Hu=3.
%
% Requires MPC TOOLBOX and CONTROL TOOLBOX for Matlab.

% J.M.Maciejowski, 12 September 2000.
% Copyright (C) 2000. All Rights reserved.

function [wtemp,power,tvector] = swimpool(W,V,Tplant,kplant,ulim,Ts)

     % Handle input arguments, set defaults:

if nargin < 6, Ts = 0.25; end    % hour, sampling interval, default
if nargin < 5, ulim = [-inf,inf,1e6]; end % input constraints, default
if nargin < 4, Tplant = 1; end   % hour, time constant, default
if nargin < 3, kplant = 0.2; end % degC/kW, heater gain, default
if nargin < 2, V =1e-3; end      % artificial noise covariance, default
if nargin < 1, W =1; end         % artificial process noise, default

% Define parameters of internal model:

Tmodel = 1;   % hour, time constant
kmodel = 0.2; % degC/kW, heater gain

% Weights for MPC cost function:
Q=1; R=0;
% Horizons for MPC:
Hp=10; Hu=3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Define plant:

aplantc = -1/Tplant;  bplantc = [kplant, 1]/Tplant; % continuous-time
cplant = 1;           dplant = [0, 0];
plantc = ss(aplantc,bplantc,cplant,dplant);  % LTI object

plantd = c2d(plantc,Ts);  % discrete-time equivalent
[aplantd,bplantd] = ssdata(plantd); % A and B matrices. (C and D stay unchanged)

plantinfo = [Ts,1,1,0,1,1,0]; % information for MOD format
                          % (1 state, SISO, 1 unmeasured disturbance)
plant = ss2mod(aplantd,bplantd,cplant,dplant,plantinfo); % plant in MOD format

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Define controller's internal model:

% Disturbance dynamics (diurnal variation):
aairc = [0, 1; -(2*pi/24)^2, 0]; % Air temp A matrix, continuous-time
% Complete continuous-time model, with state vector =
%   [water temp, air temp, air temp derivative]':
amodelc = [-1/Tmodel, 1/Tmodel, 0;
            zeros(2,1),  aairc    ];
bmodelc = [kmodel/Tmodel; 0; 0];
cmodel = [1, 0, 0];
dmodel = 0;
modelc = ss(amodelc,bmodelc,cmodel,dmodel);  % LTI object

modeld = c2d(modelc,Ts); % discrete-time equivalent
[amodeld,bmodeld] = ssdata(modeld); % A and B matrices. (C and D stay unchanged)

modinfo = [Ts,3,1,0,0,1,0];
model = ss2mod(amodeld,bmodeld,cmodel,dmodel,modinfo); % model in MOD format

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Compute observer gain which gives asymptotically stable observer:

% First need slightly different model, with artificial process noise:
% Use same 'A' matrix, different 'B' matrix.
bobs = [bmodeld, [0;0;1]];
obsinfo = [Ts,3,1,0,1,1,0]; % minfo vector for MOD format
modobs = ss2mod(amodeld,bobs,cmodel,[0,0],obsinfo); % model for observer

% Now compute observer gain:
Kest = smpcest(modobs,W,V);

% Compute and display observer poles:
[auga,augb,augc]=mpcaugss(amodeld,bobs,cmodel); % augmented model
obspoles = eig(auga*(eye(4)-Kest*augc)); % pole computation
disp('   Observer poles are:'), disp(obspoles)
if any(abs(obspoles)>=1),
  disp('Warning: Observer not asymptotically stable')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Now simulate pool with predictive control:

tend = 50; % end time for simulation
tvector = [0:0.25:tend]';  % time vector for plots etc
setpoint =  20; % deg C, setpoint for water temperature

airtemp = 15 + 10*sin(2*pi*tvector/24); % sine wave, period 24 hours

[wtemp,power] = scmpc(plant,model,Q,R,Hu,Hp,tend,setpoint,ulim,[],Kest,...
                      [],[],airtemp,[]);

% Display results:
figure  % New figure
plotall(wtemp,power,tvector);
subplot(211), grid
hold on
plot(tvector,airtemp,'--')
xlabel('Time (hours)'), ylabel('Temperature (deg C)')
title('Water (solid) and Air (broken) Temperatures')
subplot(212), grid
xlabel('Time (hours)'), ylabel('Heater power (kW)')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%% END OF FUNCTION 'SWIMPOOL'