% Level control of a tank using>
% 1. Simple PI with anti-windup
% 2. Observer/Estimator
% 3. A PI without anti-windup for normal operation with a P-controller to
% monitor the upper limit and a P-controller to monitor the lower limit.
% 4. A PI with observer/estimation for normal operation (based on Christoph
% Backi's  model) with a P-controller to  monitor the upper limit 
% and a P-controller to monitor the lower limit.
% November 2017

clear;
close all;


%% SIMULATION TIME
t_sim = 400;                   % simulation time

%% MODEL
%To work with physical variables, we are using an interpreted Matlab
%function called SimpleTank_ODE  dh/dt=(qin-qout)/A. 
% This could be substituted with an integrator (1/s). It is left as it is
% in case future changes are required.

%Nominal values
qout0= 0.5;
qin0= 0.5;
h0=0.5;
p=1; %Area of the tank
theta=0; %Time delay in the tank

%% INIT
h_init = 0.5;           % initial value level
h_set = 0.5;            % setpoint level
h_est_init = 0.4;       % initial value level estimate
q_in_est_init = 0.4;    % initial value inflow estimate

% Use h_est for P-controllers
est=0; % est=0 -> use h_meas; est=1 -> use h_est

%% DISTURBANCES
% The disturbance is the inflow

sin_ON =1;             % add sinusoidal to nominal inflow
d1amplitude=0.05;
d1bias=0;
d1freq=1; %rad/s
d1phase=0; %rad

noise_ON= 1;    % add noise
useEKF =0 ;     % use the Extended Kalman Filter
useLSO = 1;     % use the Least Squares Observer
noise_power=1e-5;
ratelimit=10000; %to limit P controllers
tfilter=1/4; %measurement filter

d2= 0.2; %step disturbance
td2=50;%20;  

d3= 0.2;
td3=100;%40; 

d4=0.05;
td4=150;%60;

d5=-0.9;
td5=200;%75;

%% PARAMETERS PI CONTROLLER
tau_c = 3;              % PI controller tuning variable
%roc = 1;              % MV rate of change (not used)
t_delay =2;% 0.75; %2;            % MV delay

%% PARAMETERS OBSERVER
p0 = [1; 0.5; 1];
options = optimoptions('fsolve','Display','iter');

A = [0 1; 0 0];
C = [1 0];

% compute LSO feedback vector
eta = 1e0;          % tuning variable LSO
RLSO = 1e-1;        % measurement noise covariance matrix LSO
RLSOinv = 1/RLSO;

[pLSO,fvalLSO] = fsolve(@(p) LSO(p,A,C,eta,RLSOinv),p0,options);
LLSO = eta*[pLSO(1) pLSO(2); pLSO(2) pLSO(3)]*transpose(C)*RLSOinv;

% compute EKF feedback vector
Q=[1e-1 0; 0 1e0];  % process noise covariance matrix EKF
REKF = 1e-1;        % measurement noise covariance matrix EKF
REKFinv = 1/REKF;

[pEKF,fvalEKF] = fsolve(@(p) EKF(p,A,C,Q,REKFinv),p0,options);
LEKF = [pEKF(1) pEKF(2); pEKF(2) pEKF(3)]*transpose(C)*REKFinv;


% SYS = ss([0 1; 0 0],[[-1;0] eye(2,2)],[1 0],[0 [0 0]]); 
% N = 0;
% [KEST,L,P] = kalman(SYS,Q,R,N);

%% PARAMETERS P-CONTROLLERS

t_delay2 = 0.05;           % MV delay for low pass filter for P-controllers
Ymin=0.1; % minimum level
Ymax=0.9; % maximum level
Umax=1;   % minimum output (q_out,max)
Umin=0;   % maximum output (q_out,min)

%Saturation of input given u0
Uminsat=Umin-qout0;
Umaxsat=Umax-qout0;

%SIMC tuning
kprime=-1/p;
Kc=(1/kprime)*(1/(tau_c+theta));
tauI=4*(tau_c+theta);
f=20; % Factor to make P-controller gain higher than 
Kcmin=f*Kc; 
Kcmax=Kcmin;

%% SIMULATE AND PLOT ONE PI CONTROLLER with anti-windup

sim('TankPIctrl',t_sim) 
hPI=h;
q_inPI=q_in;
q_outOnePI=q_out;

hmin=[0.1,0.1];
hmax=[0.9,0.9];
hminmin=[0,0];
hmaxmax=[1,1];
hsp=[0.5,0.5];
tim=[0,t_sim];

figure(1);
subplot(211)
plot(h.time,h.signals.values,'k')
%title('One PI controller with anti-windup')
hold on
plot(tim,hmin,'--r')
plot(tim,hmax,'--r')
plot(tim,hsp,'--','Color',[0 165/255 115/255])
hold off
%legend('h','h_{min}','h_{max}','h_{sp}'); 
%legend('h','h_{min}','h_{max}','h_{sp}'); 
ylim([-2 2]); grid on; grid minor; ylabel('Level [m]')
subplot(212)
plot(q_in.time,q_in.signals.values)
hold on
plot(q_out.time,q_out.signals.values,'k')
legend('q_{in}','q_{out}'); ylim([-0.1 1.1]); grid on; grid minor
xlabel('time [s]');ylabel('Flow [m^3/s]')
hold off

% figure(1);
% subplot(311)
% plot(h.time,h.signals.values,'k')
% %title('One PI controller with anti-windup')
% hold on
% plot(tim,hmin,'--r')
% plot(tim,hmax,'--r')
% plot(tim,hsp,'--','Color',[0 165/255 115/255])
% hold off
% %legend('h','h_{min}','h_{max}','h_{sp}'); 
% legend('h','h_{min}','h_{max}','h_{sp}'); 
% ylim([-2 2]); grid on; grid minor; ylabel('h [m]')
% subplot(312)
% plot(q_in.time,q_in.signals.values,'k')
% legend('q_{in}'); ylim([-0.1 1.1]); grid on; grid minor; ylabel('Flow [m^3/s]')
% subplot(313)
% plot(q_out.time,q_out.signals.values,'k')
% legend('q_{out}'); ylim([-0.1 1.1]); grid on; grid minor
% xlabel('time [s]');ylabel('Flow [m^3/s]')

%saveas (figure (1), 'figures/OnlyPID','eps2c');

%% SIMULATE AND PLOT ONE CONTROLLER WITH OBSERVER
% sim('levelcontrolObs',t_sim)     % one controller with observer
% hObs=h;
% q_inObs=q_in;
% q_outObs=q_out;

% figure;
% subplot(311)
% plot(h.time,h.signals.values)
% title('One PI controller with estimation of q_{in}')
% hold on;
% plot(h_est.time,h_est.signals.values)
% legend('h','h_{est}')
% ylim([-3 3]); grid on; grid minor; ylabel('h [m]')
% subplot(312)
% plot(q_in.time,q_in.signals.values)
% hold on;
% plot(q_in_est.time,q_in_est.signals.values)
% legend('q_{in}','q_{in,est}')
% ylim([-0.1 1.1]); grid on; grid minor; ylabel('q [m^3/s]')
% subplot(313)
% plot(q_out.time,q_out.signals.values)
% legend('q_{out}');ylim([-0.1 1.1]); grid on; grid minor

%% SIMULATE AND PLOT ONE PI CONTROLLER AND 2 P CONTROLLERS
s=1;  %select structure with simple feedback PI controller
tic
sim('levelcontrol_3ctrl',t_sim)       % 3 controllers
toc
IAEsimple=IAEmid;
hsimple=h;
q_insimple=q_in;
q_outsimple=q_out;

figure(2);
subplot(3,1,1)
plot(h.time,h.signals.values,'k')
hold on
plot(tim,hmin,'--','Color',[255/255 165/255 0/255])
plot(tim,hmax,'--','Color',[255/255 165/255 0/255])
plot(tim,hminmin,'--r')
plot(tim,hmaxmax,'--r')
plot(tim,hsp,'--','Color',[0 165/255 115/255])
hold off
%legend('h','h_{min}','h_{max}','h_{sp}'); 
%legend('h','h_{min}','h_{max}','h_{sp}'); 
ylim([-0.1 1.1]); grid on; grid minor; ylabel('Level [m]')

subplot(3,1,2)
plot(q_in.time,q_in.signals.values)
hold on;
plot(q_out.time,q_out.signals.values,'k')
%title('1 PI with estimator and 2 P controllers')
%legend('q_{in} [m^3/s]','q_{out} [m^3/s]')
%yticks(0:0.2:1);
lgd=legend('q_{in}','q_{out}'); 
lgd.FontSize=8;
lgd.Location='northeast';
ylim([-0.1 1.1]); grid on; grid minor
xlabel('time [s]');ylabel('Flow [m^3/s]')
hold off

subplot(3,1,3)
plot(q_outmax.time,q_outmax.signals.values,'--r')  %,'Color',[105/255 56/255 27/255])
hold on;
plot(q_outmid.time,q_outmid.signals.values,'-.','Color',[50/255 205/255 50/255])
hold on;
plot(q_outmin.time,q_outmin.signals.values,'--','Color',[255/255 165/255 0/255])
hold on;
plot(q_out.time,q_out.signals.values,'k')
hold on;
%legend('q_{max,out}','q_{mid,out}','q_{min,out}','q_{out}')
a1=annotation('textarrow',[0.37 0.35],[0.23 0.23],'String','q_{max,out}');
a2=annotation('textarrow',[0.57 0.57],[0.27 0.24],'String','q_{mid,out}');
a3=annotation('textarrow',[0.68 0.65],[0.23 0.23],'String','q_{min,out}');
a1.FontSize=10;
a1.HeadLength=4;
a1.HeadWidth=8;
a2.FontSize=a1.FontSize;
a3.FontSize=a1.FontSize;
a2.HeadLength=a1.HeadLength;
a3.HeadLength=a1.HeadLength;
a2.HeadWidth=a1.HeadWidth;
a3.HeadWidth=a1.HeadWidth;
% text(140,0.45,['q_{max,out}'],...
% 	'VerticalAlignment','bottom',...
% 	'HorizontalAlignment','center',...
% 	'FontSize',10)
ylim([-0.1 1.1]); grid on; grid minor
xlabel('time [s]');ylabel('Flow [m^3/s]')

saveas (figure (2), 'figures/Case4','eps2c'); 
% figure;
% subplot(3,1,1)
% plot(q_in.time,q_in.signals.values)
% title('One PI controller and P controllers to track limits')
% hold on;
% plot(q_out.time,q_out.signals.values)
% %title('1 PI with estimator and 2 P controllers')
% legend('q_{in} [m^3/s]','q_{out} [m^3/s]')
% ylabel('q [m^3/s]')
% ylim([-0.1 1.1])
% yticks(0:0.2:1);
% grid on
% 
% subplot(3,1,2)
% plot(h.time,h.signals.values)
% legend('h [m]')
% ylim([-0.1 1.1])
% ylabel('h [m]')
% %yticks(0:0.1:1);
% xlabel('time [s]')
% grid on
% 
% subplot(3,1,3)
% plot(q_outmax.time,q_outmax.signals.values)
% hold on;
% plot(q_outmid.time,q_outmid.signals.values)
% hold on;
% plot(q_outmin.time,q_outmin.signals.values)
% hold on;
% plot(q_out.time,q_out.signals.values,'*')
% hold on;
% legend('q_{max,out}','q_{mid,out}','q_{min,out}','q_{out}')
% %ylim([-1 1])
% ylabel('q_{out} [m^3/s]')
% %yticks(-1:0.2:1);
% xlabel('time [s]')
% grid on



%% SIMULATE AND PLOT ONE PI CONTROLLER WITH OBSERVER AND 2 P CONTROLLERS
% s=2; %select structure with observer
% sim('levelcontrol_3ctrl',t_sim)       
% IAEObsPI=IAEmid;
% hObsPI=h;
% q_inObsPI=q_in;
% q_outObsPI=q_out;
% 
% figure;
% subplot(311)
% plot(h.time,h.signals.values)
% title('One PI controller with observer and P controllers to track limits')
% hold on;
% plot(h_est.time,h_est.signals.values)
% legend('h','h_{est}') ;grid on; grid minor; ylabel('h [m]')
% subplot(312)
% plot(q_in.time,q_in.signals.values)
% hold on;
% plot(q_in_est.time,q_in_est.signals.values)
% legend('q_{in}','q_{in,est}');
%  ylim([-0.1 1.1]); grid on; grid minor; ylabel('q [m^3/s]')
% subplot(313)
% plot(q_out.time,q_out.signals.values)
% legend('q_{out}');ylim([-0.1 1.1]); grid on; grid minor
% xlabel('time [s]');ylabel('q [m^3/s]')
% % figure;
% % plot(error_level.time,error_level.signals.values)
% % title('One PI controller with observer and P controllers to track limits')
% % hold on
% % plot(pi.time,pi.signals.values) % Output from PI
% figure;
% subplot(3,1,1)
% plot(q_in.time,q_in.signals.values)
% title('One PI controller with observer and P controllers to track limits')
% hold on;
% plot(q_out.time,q_out.signals.values)
% legend('q_{in} [m^3/s]','q_{out} [m^3/s]')
% ylabel('q [m^3/s]')
% ylim([-0.1 1.1])
% yticks(0:0.2:1);
% grid on
% subplot(3,1,2)
% plot(h.time,h.signals.values)
% legend('h [m]')
% ylim([-0.1 1.1])
% ylabel('h [m]')
% %yticks(0:0.1:1);
% xlabel('time [s]')
% grid on
% subplot(3,1,3)
% plot(q_outmax.time,q_outmax.signals.values)
% hold on;
% plot(q_outmid.time,q_outmid.signals.values)
% hold on;
% plot(q_outmin.time,q_outmin.signals.values)
% hold on;
% %plot(q_out.time,q_out.signals.values,'*')
% %hold on;
% %legend('q_{max,out}','q_{mid,out}','q_{min,out}','q_{out}')
% legend('q_{max,out}','q_{mid,out}','q_{min,out}')
% %ylim([-1 1])
% ylabel('q_{out} [m^3/s]')
% %yticks(-1:0.2:1);
% xlabel('time [s]')
% grid on

%% PLOT COMPARISONS

% figure; %Evolution of IAE
% subplot(211)
% plot(IAE_PI.time, IAE_PI.signals.values)
% title('Evolution of IAE')
% hold on
% plot(IAEObs.time, IAEObs.signals.values)
% hold on
% plot(IAEsimple.time, IAEsimple.signals.values)
% hold on
% plot(IAEObsPI.time, IAEObsPI.signals.values)
% hold on
% legend('PI/windup','Observer', 'PI/P', 'Observer/P', 'Location','northwest')
% ylabel('IAE')
% grid on
% subplot(212)
% plot(q_in.time,q_in.signals.values)
% ylabel('q_{in} [m^3/s]')
% xlabel('time [s]')
% grid on
% 
% figure; %compare level, q_in and q_out for the two cases with P controllers
% subplot(311)
% plot(hsimple.time,hsimple.signals.values)
% hold on
% plot(hObsPI.time,hObsPI.signals.values)
% hold on
% %title('Comparison')
% legend('PI/P','Observer/P'); 
% ylim([-0.1 1.1]); grid on; grid minor; ylabel('h [m]')
% subplot(312)
% plot(q_inPI.time,q_inPI.signals.values)
% %legend('PI', 'Observer','PI/P','Observer/P'); 
% ylim([-0.1 1.1]); grid on; grid minor; ylabel('q_{in} [m^3/s]')
% subplot(313)
% plot(q_outsimple.time,q_outsimple.signals.values)
% hold on
% plot(q_outObsPI.time,q_outObsPI.signals.values)
% hold on
% legend('PI/P','Observer/P'); 
% ylim([-0.1 1.1]); grid on; grid minor
% xlabel('time [s]');ylabel('q_{out} [m^3/s]')
% 
% % figure; %compare level, q_in and q_out for the two cases without P controllers
% % subplot(311)
% % plot(hPI.time,hPI.signals.values)
% % hold on
% % plot(hObs.time,hObs.signals.values)
% % hold on
% % %title('Comparison')
% % legend('PI', 'Observer'); 
% % ylim([-3 3]); grid on; grid minor; ylabel('h [m]')
% % subplot(312)
% % plot(q_inPI.time,q_inPI.signals.values)
% % %legend('PI', 'Observer','PI/P','Observer/P'); 
% % ylim([-0.1 1.1]); grid on; grid minor; ylabel('q_{in} [m^3/s]')
% % subplot(313)
% % plot(q_outOnePI.time,q_outOnePI.signals.values)
% % hold on
% % plot(q_outObs.time,q_outObs.signals.values)
% % legend('PI', 'Observer'); 
% % ylim([-0.1 1.1]); grid on; grid minor
% % xlabel('time [s]');ylabel('q_{out} [m^3/s]')


