%% EXERC?CIO AULA 28/08/2023

% Slide extra

% Dados do exercicio:
% u = sen(t + pi/7) + 0.5cos(2t)

% TODOS OS PARAMETROS DESCONHECIDOS:
% y = (   b_1 s + b_0   ) u
%     (s^2 + 2 s + 1)

close all
clear all

% Dados e simulacao
N   = 10000;
dt = .01;
t = 0 : dt : N;
u = (sin(t + pi/7) + 0.5*cos(2*t));
%u = ones(1,length(t));
a1 = 2;
a0 = 1;
b1 = 3;
b0 = 2;

ft1 = tf([b1 b0],[1 a1 a0]); % Funcao original
ft20 = tf([1 0], [  1 2 1]); % Funcao que multiplicada p/ y, da z
ft21 = tf([0 1],[  1 2 1]); % Funcao que multiplicada p/ y, da phi1



% Parametros adaptativos
gama = [.02 0;0 1]; % 2*eye(4); % Ganho adaptativo
theta(:,1) = [0 ;0];


% Respostas no tempo:
phi1 = lsim(ft20,u,t);
phi2 = lsim(ft21,u,t);
y = lsim(ft1,u,t) ;
phi = [phi1 phi2]';
alpha = 0.1;

for k = 1 : length(t)-1
    zest(k) = theta(:,k)' * phi(:,k);
    ms(k) = 1 + alpha*phi(:,k)'* phi(:,k);
    erro(k,:) = (y(k) - zest(k))/ms(k)^2;
    theta(:,k+1) = dt*gama*erro(k)*phi(:,k) + theta(:,k);
end


figure(1)
plot(t,theta,'LineWidth',4)
legend('Parametro a','Parametro b')
xlabel('Amostras')
ylabel('Parametros Identificado')
title('Metodo do Gradiente')
ax =gca;
ax.FontSize = 24;


figure(2)
plot(t,u,'LineWidth',4)
xlabel('Amostras')
ylabel('Entrada')

ax =gca;
ax.FontSize = 24;

figure(3)
t1=dt:dt:N;
plot(t1,erro,'LineWidth',4)
xlabel('Amostras')
ylabel('Erro de Saida')

ax = gca;
ax.FontSize = 24;