clc
warning OFF
clear all
close all


Kc0 = -250;
dy_s = 7;
t_init = 362;
u_init = 30;
dt = 0.002;

load Data
t = Data (:,1);
r = Data (:,2);
y = Data (:,3);
n = length(y);

%%

%%%Low pass filter
n = length(y);
yf1 = zeros(1,n);
yf = zeros(1,n);
yff = zeros(1,n);
alpha = 0.25; % alpha=1 means no filtering
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',3)
hold on
grid on
plot(t,y,t,yff,'g','LineWidth',3)
xlabel('time(sec)')
ylabel('Inlet Pressure[Kpa]');
xlim ([0 650])
legend('Setpoint','Measurement','Filtered',3);

