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; % 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',2)
hold on
grid on
plot(t,y,t,yff,'g','LineWidth',2)
xlabel('time(sec)')
ylabel('Inlet Pressure[Kpa]');
legend('Setpoint','Measurement','Filtered',3);

