clc
clear all
format long

% sys = tf([1],[1.65 1]);
% 
% sys = minreal(sys);
% 
% sys.outputdelay=0.66;
% 
% sys_d = c2d(sys,1,'zoh');
% 
% a1 = 0.5455; b1 = 0.1852; b2 = 0.2683;
% 
% param = [a1 b1 b2];
% 
% np = 3;

% cas deux

sys = tf([10],[1 2 10]);

sys = minreal(sys);

% sys.outputdelay=0.66;

s = tf('s');

sys_d = c2d(sys,0.01,'zoh')
sys_d = minreal(sys_d);    

a1 = -1.979; a2 = 0.9802; b1 = 0.0004966; b2 = 0.0004933;

% a1 = 0.7284; a2 = 0.1353; b1 = 1.347; b2 = 0.5168; %discretisation avec T = 1s
% a1 = -0.15; a2 = -0.27; a3 =0.43; b1 = 3; b2 = 7; b3 = 0.73;

%tracee de la reponse indicielle
% 
% sortie = sys/s;
% 
% syms s
% 
% [num,den] = tfdata(sortie,'v');
% [r,p,k] = residue(num,den);
% sortie = ilaplace(r(1)/(s-p(1)))+ilaplace(r(2)/(s-p(2)))+ilaplace(r(3)/(s-p(3)));
% t = 0:0.01:10;
% sortie = subs(sortie);
% 
% figure(1)
% % plot(t,sortie);

param = [a1 a2 b1 b2];

np = 4;

N=1000;

u = 1:10;
u = (-1).^u;

for i=11:N
    u(i) = -u(i-7)*u(i-10);
end

P = 1e8*eye(np);

teta = zeros(3,np)';

%  v = randn(1,N);

for i=1:N
    v(i)=0;
end
    
    
 y = [0 0];

for i=3:N
    phi = [y(i-2) y(i-1) u(i-1) u(i-2)]';
     y(i) = param*phi + v(i);
    teta(:,i) = teta(:,i-1)+P*phi*(y(i)-phi'*teta(:,i-1))/(1+phi'*P*phi);
    P = P-(P*phi*phi'*P)/(1+phi'*P*phi);
end

teta_final = teta(:,i)

color = {'k','g','b','r','m','c'};
 
figure(2);
  
for i=1:np
    plot(1:N,teta(i,:),color{i})
    hold on
end

grid on

z = tf('z',0.01);

figure(3)

gz = (teta_final(3)*(z^(-1))+teta_final(4)*(z^(-2)))/(1+teta_final(1)*(z^(-1))+teta_final(2)*(z^(-2)));

%gz = (1/z^1)*((teta_final(3)*+teta_final(4)*(1/z^1))/(1+teta_final(1)*(1/z^1)+teta_final(2)*(1/z^2)));

%gz = (0.0004966*(1/z^1)+0.0004933*(1/z^2))/(1-1.979*(1/z^1)+0.9802*(1/z^2));
%gz = (teta_final(3)*z+teta_final(4))/(z^2+teta_final(1)*z+teta_final(2))

step(sys_d,'r',gz,'b',0:0.01:10);


grid on