clc
clear all
close all

Kc0 = -220;
dy_s = 21;
t_init = 320;
u_init = 30;
dt = 0.002;

load step_test4
y = step_test4 (:,1);
n = length(y);
t = 0:dt:(n-1)*dt;
r = [149*ones(1,t_init/dt) 170*ones(1,n-t_init/dt)];
 
%%%Low pass filter
n = length(y);
yf1 = zeros(1,n);
yf = zeros(1,n);
yff = zeros(1,n);
alpha = 0.001; 
y_rev = zeros(1,n);
for k=1:n
    if (k==1)
        yf1(k) = y(k);
    elseif (k==2)
        yf1(:,k) = alpha*y(k) + (1-alpha)*yf1(:,k-1);
    else
        yf1(:,k) = alpha^2*y(k) + 2*(1-alpha)*yf1(:,k-1) - (1-alpha)^2*yf1(:,k-2);
    end
    y_rev(1,n-k+1) = yf1(:,k);
end


for k=1:n
    if (k==1)
        yf(k) = y_rev(k);
    elseif (k==2)
        yf(:,k) = alpha*y_rev(k) + (1-alpha)*yf(:,k-1);
    else
        yf(:,k) = alpha^2*y_rev(k) + 2*(1-alpha)*yf(:,k-1) - (1-alpha)^2*yf(:,k-2);
    end
    yff(1,n-k+1) = yf(:,k);
end

% End of low-pass filter

figure(1)
clf

plot(t,r,'r--','LineWidth',2);
hold on
plot(t,y,t,yff,'g','LineWidth',2);
xlabel('time(sec)');
ylabel('Inlet pressure [kPa]');
title('Closed-loop step response');

i_init = round(mean(find(t==t_init)));
y_init = yff(i_init-100/dt);
t_plant = t(i_init:end);
y_plant = yff(i_init:end);
yp1 = max(y_plant);
dy_p1 = abs(yp1 - y_init);
i_yp1 = find(y_plant==yp1);
t_p1 = mean(t_plant(i_yp1));
yu = min(y_plant(i_yp1:10*i_yp1));
dy_u = abs(yu - y_init);
i_yu = find(y_plant==yu);
t_u = mean(t_plant(i_yu));
y_inf = y_plant(end);
dy_inf = abs(y_inf - y_init);
D0 = (dy_p1 - dy_inf)/dy_inf;
deltaT = t_u - t_p1;
v = (dy_inf - dy_u)/(dy_p1 - dy_inf);
z = -log(v)/sqrt(pi^2+(log(v))^2);
Tau = (deltaT/pi)*sqrt(1-z^2);
K = dy_inf/(dy_inf-dy_s);
K2 = K/(K-1);
alpha = (K+1)/(K-1);
Tau1 = 2*z*Tau*(K-1)+sqrt(4*z^2*Tau^2*(K-1)^2+(K+1)*(K-1)*Tau^2);
tp = t_p1 - t_init;
Phi = atan((1-z^2)/z)-tp*sqrt(1-z^2)/Tau;
E = sqrt(1-z^2)/Tau;
D1 = D0/(exp(-z*(tp)/Tau)*sin(E*(tp)+Phi));
Tauz = z*Tau + sqrt(z^2*Tau^2-Tau^2*(1-D1^2*(1-z^2)));
s=tf('s');
disp('The identified closed loop model:')
G2 = K2*(1+Tauz*s)*exp(-0*s)/(Tau^2*s^2 + 2*z*Tau*s + 1)
u = r-r(1);
y1 = lsim(G2,u,t);

figure (1)
plot(t,y1+y_init,'--k','LineWidth',2)
xlim([250 700])
legend('set-point','Exp. data','Filtered','identified',4)



A0 = 1/Tau^2;
A1 = 2*z/Tau;
B0 = K2/Tau^2;
B1 = K2*Tauz/Tau^2;

Kp = dy_inf/(Kc0*abs(dy_s-dy_inf));
a0 = A0/(1+Kc0*Kp);
b0 = -Kp*a0;
b1 = -B1/Kc0;
a1 = -A1-Kc0*b1; 

s = tf('s');
disp('Identified model:')
Ge = (-b1*s-b0)/(s^2-a1*s+a0)
gcl = feedback(Kc0*Ge,1);

%% Internal Model Controller (IMC) 
% Plan Information
[Zero,Pole,Gain,Ts] = zpkdata(Ge,'v');

indRHPzero = (real(Zero)>0);                        % indices of open RHP zeros
indRHPpole = (real(Pole)>0);                        % indices of open RHP poles
RHPpoles = Pole(indRHPpole);                        % RHP poles
NumRHPzeros = sum(indRHPzero);                      % number of open RHP zeros 
NumRHPpoles = sum(indRHPpole);                      % number of open RHP poles 

Tauc = 20;                                          % Tuning parameter: time constant of the closed-loop system

q_tilde = zpk(Pole,Zero,1/Gain); 
k = NumRHPpoles+1;                                  % since Vm always contains an pole at origin for step input
m = max(length(zero(q_tilde))-length(pole(q_tilde)),1); % make sure q=q_tilde*f is proper
filterOrder = m+k-1;

% 3. calculating filter as sum(a(k)s^k)/(tau*s+1)^filterOrder
coefficients = ones(1,k);

if NumRHPpoles>0
    A = zeros(NumRHPpoles,NumRHPpoles);
    for ctRHPpole = 1:length(RHPpoles)
        A(ctRHPpole,:) = RHPpoles(ctRHPpole).^(1:NumRHPpoles);
    end
    b = (Tauc*RHPpoles+1).^filterOrder-coefficients(1);
    coefficients(2:end) = (real(A\b))';
end

%Computing f
num = fliplr(coefficients);
den = fliplr(poly(repmat(-Tauc,1,filterOrder)));
f = tf(num,den);

q = minreal(q_tilde*f);
C = feedback(q,Ge,+1);
disp('The IMC controller:')
C = minreal(C)
L1 = C*Ge;
allmargin(L1)

%% Convert to PID Controller
disp('IMC based PID tuning:')

[Kc_PID,Ki_PID,Kd_PID,Tf_PID] = piddata(C);

Ti_PID = Kc_PID/Ki_PID;
Td_PID = Kd_PID/Kc_PID;


disp(['Kp = ' num2str(Kc_PID)])
disp(['Ti = ' num2str(Ti_PID)])
disp(['Td = ' num2str(Td_PID)])
disp(['Tf = ' num2str(Tf_PID)])
disp(['alpha = ' num2str(Tf_PID/Td_PID)])

disp('FEED TO  EXPERIMENTS AND CLOSE THE LOOP!')

C2 = Kc_PID*( 1 + 1/(Ti_PID*s) + Td_PID*s/(Tf_PID*s+1))
L2 = C2*Ge;
allmargin(L2)

%% Reduce to PI Controller

Kc_PI = coefficients(3)/(Gain*Tauc^3);
Ti_PI = abs(Zero)*coefficients(3);
disp('IMC based PI tuning:')
disp(['Kp = ' num2str(Kc_PI)])
disp(['Ti = ' num2str(Ti_PI)])

disp('FEED TO EXPERIMENTS AND CLOSE THE LOOP!')
