%% Mínimos Cuadrados Output Error (OE)
% (c) 2020 Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Video-presentacion en http://personales.upv.es/asala/YT/V/oeml.html
% Vamos a generar unos datos de un proceso:

s=tf('s');
G=9/(s^2+3*s+1); Ts=0.1;
Gd=c2d(G,Ts,'zoh');
Gd
truenumerador_params=Gd.num{:}(2:end);
truedenom_params=Gd.den{:}(2:end);
%generamos datos
Nmuestras=20000;
rng(1234);%siempre mismo ruido, por repetir/comparar diferentes algoritmos
u=randn(Nmuestras,1)*2;
desvtipexcit=0.1;
ruido=randn(Nmuestras,1)*desvtipexcit;

ruidoarx=0;ruidoaditivo=1;
if(ruidoarx)    
  Gd2=[Gd tf(1,Gd.den,Ts)];
  y=lsim(Gd2,[u ruido]);
end
if(ruidoaditivo)
  ylimpia=lsim(Gd,u);
  y=ylimpia+ruido;% añadimos ruido de medida
end

%
%% Calculemos el modelo identificado no recursivo usando la system ID toolbox

nf=2;nb=2;nk=1;
todoslosdatos=iddata(y,u,Ts);
th=oe(todoslosdatos,[nb nf nk]);
present(th)
th.cov
th.NoiseVariance
resid(todoslosdatos,th)

%para gráficas de luego.
dsvtipparams=sqrt(diag(th.cov))';
indiceNUM=[1 2]; indiceDEN=[3 4];
norecursivoparsnumerador=th.b(2:end);
norecursivoparsdenominador=th.f(2:end);
dsvtipnum=dsvtipparams(indiceNUM);
dsvtipden=dsvtipparams(indiceDEN);

%
%%
%% Implementación recursiva

num_inicial = [0 0.03 0.03]; den_inicial = [1 -1.8 0.9];
estimador = recursiveOE([nb nf nk],num_inicial,den_inicial);
estimador.InitialParameterCovariance = 1e6*[5 5 5 5].^2;
estimador.ForgettingFactor = 1;%-1e-3;

plotmedia=zeros(4,Nmuestras);
plotIntervConf=zeros(8,Nmuestras);
for i=1:Nmuestras
  [p1,p2,yhat]=estimador(y(i),u(i));
  
  %guardemos datos para gráficas posteriores
  covv=estimador.ParameterCovariance;
  %parameter covariance se calcula suponiendo 
  % que el resíduo (ruido de medida oe) es de varianza 1.
  %corregimos:
  covv=covv*desvtipexcit^2; 
  %esto es complicado de saber en proceso práctico! pero la covarianza se usa poco.
  desvtippars=sqrt(diag(covv));
  xa=[p1(2:end)';p2(2:end)'];%el primero es 0 en num, 1 en den, no lo guardamos.
  plotmedia(:,i)=xa;  plotIntervConf(:,i)=[xa+2*desvtippars; xa-2*desvtippars];
end



%
%% Graficas de resultados
%
%
%
% Dibujemos los parámetros estimados y su intervalo de conf...
% NUMERADOR

%dibujemos estimado de parámetros del numerador
for coefic=1:2
    figure()
    plot(plotmedia(indiceNUM(coefic),:)','k')
    hold on
    plot(plotIntervConf([indiceNUM(coefic) indiceNUM(coefic)+4],:)','g')
    plot(ones(Nmuestras,1)*truenumerador_params(coefic),'r')
    
    %plot del resultado NO recursivo
    plot(Nmuestras,norecursivoparsnumerador(coefic),'xb')
    plot(Nmuestras,norecursivoparsnumerador(coefic)+dsvtipnum(coefic)*2,'xg')
    plot(Nmuestras,norecursivoparsnumerador(coefic)-dsvtipnum(coefic)*2,'xg')
    hold off
    axis([0 Nmuestras -.05 .2])
    legend('media','95% sup','95% inf','real','no recursivo')
    title(sprintf('Numerador, coeficiente %d',coefic))
    grid on
    
end

% Denominador:


plot(plotmedia(indiceDEN,:)','k')
hold on
plot(plotIntervConf([indiceDEN indiceDEN+4],:)','g')
plot(ones(Nmuestras,2)*diag(truedenom_params),'r')
plot(Nmuestras,norecursivoparsdenominador,'xb')
    plot(Nmuestras,(norecursivoparsdenominador+dsvtipden*2)','xg')
    plot(Nmuestras,(norecursivoparsdenominador-dsvtipden*2)','xg')
hold off
title('Estimación coeficientes denominador')
axis([0 Nmuestras -2 1.])