%% Initial behavior
clear

T = 100; % Total time
k = 2;
num = [1 1];
den = [1 -2 1];
G = tf(k*num, den,-1); % Transfer function

u_1 = sin(linspace(0,2*pi)); % Initial input signal

r = [zeros(T/2,1) ; 6000*ones(T/2,1)]; % Reference signal (desired output)

% The function "simul" simulates the output of a filter G with input u
y_1 = simul(G,u_1); % System output
e_1 = r - y_1; % Initial error

% Plotting results:
plot(y_1)
hold on
plot(r)
plot(e_1)
legend('output y(t)','reference r(t)','error e(t)')

%% ILC

% Filters:
num_L = den;
den_L = num;
k_L = 1/10;
L = tf(k_L*num_L,den_L,-1);

iter = 10; % Number of ILC iterations

% Store output:
u = zeros(T,iter+1);
u(:,1) = u_1;
y = zeros(T,iter+1);
y(:,1) = y_1;
e = zeros(T,iter+1);
e(:,1) = e_1;

for k = 1:iter
Le_k = simul(L,e(:,k));
u(:,k+1) = u(:,k) + Le_k;
y(:,k+1) = simul(G,u(:,k+1));
e(:,k+1) = r - y(:,k+1);
end

% Plotting the result:
for i = 1:iter
clf
plot(y(:,i));
hold on
plot(e(:,i));
plot(r)
pause();
end