% Simulation of a linear quadruple tank lab process controlled by
% an MPC controller using MPCSim.
%
% Copyright Johan ?kesson 2006
%

clear all

% Model for the four tank process
%%
% Process parameters
A1=30; % cm
A2=30;
A3=30;
A4=30;

a1=0.065; % cm
a2=0.065;
a3=0.065;
a4=0.065;

kc=0.5; % V/cm
g=981;  % cm/s^2

v10_nmp=3.00; % V
v20_nmp=3.00;

k1_nmp=3.33; % cm^3/Vs
k2_nmp=3.33; % cm^3/Vs

g1_nmp=0.3; % ratio of allocated pump capacity between lower and
            % upper tank
g2_nmp=0.3;

% If symbolic toolbox is installed, this code could be used to
% calculate the stationary values
% Stationary values, non-minimum phase
S = solve('-a1/A1*sqrt(2*g*h10_nmp)+a3/A1*sqrt(2*g*h30_nmp)+g1_nmp*k1_nmp/A1*v10_nmp',...
	  '-a2/A2*sqrt(2*g*h20_nmp)+a4/A2*sqrt(2*g*h40_nmp)+g2_nmp*k2_nmp/A2*v20_nmp',...
	  '-a3/A3*sqrt(2*g*h30_nmp)+(1-g2_nmp)*k2_nmp/A3*v20_nmp',...
	  '-a4/A4*sqrt(2*g*h40_nmp)+(1-g1_nmp)*k1_nmp/A4*v10_nmp',...
	  'h10_nmp,h20_nmp,h30_nmp,h40_nmp');

h10_nmp = eval(S.h10_nmp)
h20_nmp = eval(S.h20_nmp)
h30_nmp = eval(S.h30_nmp)
h40_nmp = eval(S.h40_nmp)

%%
% 
% h10_nmp = 8.24441415257276;
% h20_nmp = 19.01629576919927;
% h30_nmp = 4.31462580236556;
% h40_nmp = 8.80652939083585;


% Build state space model, minimum phase
T1_nmp=A1/a1*sqrt(2*h10_nmp/g);
T2_nmp=A2/a2*sqrt(2*h20_nmp/g);
T3_nmp=A3/a3*sqrt(2*h30_nmp/g);
T4_nmp=A4/a4*sqrt(2*h40_nmp/g);

A_nmp=[-1/T1_nmp 0 A3/(A1*T3_nmp) 0;
      0 -1/T2_nmp 0 A4/(A2*T4_nmp);
      0 0 -1/T3_nmp 0;
      0 0 0 -1/T4_nmp];

B_nmp=[g1_nmp*k1_nmp/A1 0;
      0 g2_nmp*k2_nmp/A2;
      0 (1-g2_nmp)*k2_nmp/A3;
      (1-g1_nmp)*k1_nmp/A4 0];

C_nmp=[kc 0 0 0; % Notice the measured signals are given in Volts!
      0 kc 0 0];

D_nmp=zeros(2,2);

sys=ss(A_nmp,B_nmp,C_nmp,D_nmp)




