%% Mínimos Cuadrados Recursivos
% (c) 2017, 2019 Antonio Sala Piqueras, Universitat Politècnica de València. Todos los derechos reservados.
% Vídeo-presentación en: http://personales.upv.es/asala/YT/V/mcr2m.html
%
% modelo y=k*x+ruido. Adivinar "k".
%% ESTATICO, ganancia constante, se acumula información siempre:
%

%estiamdo inicial
modelo=@(x) 4*x+randn();
kmedio=0; kvarianza=1e5;
Nmuestras=150;
listak=zeros(1,Nmuestras); listadt=zeros(1,Nmuestras);
varruido=1;
for i=1:Nmuestras
   x=randn()*3;
   y=modelo(x);
   varianzay=x*kvarianza*x+varruido;
   covarianza_y_th=kvarianza*x';
   cosa=covarianza_y_th*inv(varianzay);
    kmedio=kmedio+cosa*(y-kmedio*x);
    kvarianza=kvarianza-cosa*covarianza_y_th';%no depende de "y", sí de x.
    listak(i)=kmedio;
    listadt(i)=sqrt(kvarianza);
end
plot(listak)

plot(listadt)
ylabel('Desviación típica'), xlabel('muestras')

plot([listak+listadt*3; listak; listak-listadt*3]')
legend('99% inf','media','99% sup'), grid on

%% CON OLVIDO, para seguir una ganancia "variante en el tiempo" (monitorización):

%estimado inicial
ganancia=@(t) 4+0.9*sign(cos(t/220));%ganancia no es constante
modelo=@(x,tiempo) ganancia(tiempo)*x+randn();
kmedio=0; kvarianza=1e6;
Nmuestras=4000;
listak=zeros(1,Nmuestras); listadt=zeros(1,Nmuestras);
listakreal=zeros(1,Nmuestras);
varruido=1;
olvido=0.99;%olvido=1;

grafx=zeros(1,Nmuestras);
grafy=zeros(1,Nmuestras);

for i=1:Nmuestras
    listakreal(i)=ganancia(i);%esto, claro, es desconocido!
   if(i<2000) 
       x=randn()*3; 
   elseif (i<3000) 
       x=randn()*0.04;
   else 
       x=randn()*4;
   end
   y=modelo(x,i);%sólo x, y son conocidos. 
   grafx(i)=x;
   grafy(i)=y;
   kvarianza=kvarianza/olvido;
   cosa=kvarianza*x'*inv(x*kvarianza*x+varruido);
    kmedio=kmedio+cosa*(y-kmedio*x);
    kvarianza=kvarianza-cosa*x*kvarianza;%no depende de "y", sí de x.
    listak(i)=kmedio;
    listadt(i)=sqrt(kvarianza);
   end
   plot(grafy,'.')
   xlabel('Muestras')
   hold on
   plot(grafx)
  hold off
  
   xlabel('Muestras')
plot(listadt)
ylabel('Desviación típica'), xlabel('muestras')
plot([listak+listadt*3; listak; listak-listadt*3; listakreal]')
legend('99% sup','media','99% inf','real')
axis([0 Nmuestras -3 13]), grid on
ylabel('Ganancia')
xlabel('Muestras')

% Con olvido 0.99, el tiempo que tardamos en estimar medianamente la nueva ganancia es del orden de 400 muestras.
%
%% Con olvido Filtro de Kalman, parámetros que cambian "poco a poco" (deriva lenta):

%estimado inicial
modelo=@(x,ganancia) ganancia*x+randn();
kmedio=0; kvarianza=1e6;
Nmuestras=3500;
listak=zeros(1,Nmuestras); listadt=zeros(1,Nmuestras);
listakreal=zeros(1,Nmuestras);
varruido=1;

stdincrementoparametros=0.003;%paseo incrementando k 0.001 por muestra...
%o sea que stdincrementoparametros*sqrt(Nmuestras) es la desv. dipica del "cambio esperado del parametro"
gananciareal=4;%desconocida
for i=1:Nmuestras
    
   %simulamos medir datos
   x=randn()*2.5;
   %vamos a poner un trocito sin excitación
   if((i>=2000)&&(i<3000))
       x=0; 
   end
   y=modelo(x,gananciareal);%sólo x, y son conocidos. 
   
   
   %minimos cuadrados recursivos 
   kvarianza=kvarianza+stdincrementoparametros^2;
   cosa=kvarianza*x'*inv(x*kvarianza*x+varruido);
    kmedio=kmedio+cosa*(y-kmedio*x);
    kvarianza=kvarianza-cosa*x*kvarianza;%no depende de "y", sí de x.
    listak(i)=kmedio;
    listadt(i)=sqrt(kvarianza);
    
    
    %en la siguiente muestra, la ganancia real cambia un poco... 
    listakreal(i)=gananciareal;
    gananciareal=gananciareal+randn()*stdincrementoparametros+1e-4;%esto, claro, es desconocido para los mín. cuadrados.
end
plot(listadt)
ylabel('Desviación típica'), xlabel('muestras')
axis([0 Nmuestras 0 0.4]), grid on
plot([listak+listadt*2; listak; listak-listadt*2; listakreal]')
legend('95% sup','media','95% inf','real')
axis([0 Nmuestras 2.5 5.5]), grid on

%
%
%%