%FQRRLS3T Problem 3.4
%
%   'ifile.mat' - input file containing:
%      I - members of ensemble
%      K - iterations
%      s - deterministic part of signal to predict
%      sigman - standard deviation of noise in signal to predict
%      ND - delayer order
%      N - filter order
%      lambda - forgetting factor
% 
%   'ofile.mat' - output file containing:
%      ind - sample indexes 
%      MSE - mean-square error

clear all	% clear memory
load ifile;	% read input variables
LD=ND+1;	% delayer length
L=N+1;		% filter length
LD1=LD+1;
L1=L+1;			
L2=L1+1;	% auxiliary constants
MSE=zeros(K,1);	% prepare to accumulate MSE*I

for i=1:I,		% ensemble
  D=zeros(LD,1);	% initial delayer memory
  X=zeros(L,1);		% initial memory
  n=randn(K,1)*sigman;	% noise in signal to predict
  xq2=zeros(L,1);
  dq2=zeros(L,1);
  Qthetaf=sparse(eye(L2)); 
  for l=1:L,
    Qthetabp(:,((1:L1)+(l-1)*L1))=sparse(eye(L1));
    Qtheta(:,((1:L1)+(l-1)*L1))=sparse(eye(L1));
  end
  d=s(1)+n(1);		
  D=[d
     D(1:ND)];		% initial values
  normef=abs(D(LD));
  for k=1:LD,		% iterations
    d=s(k+1)+n(k+1);	% sample of signal to predict
    D=[d 
       D(1:ND)];	% new delay vector
    x=D(LD);		% new input sample
    AUX=[x
         lambda^(1/2)*xq2];
    for l=1:L,
      AUX=Qtheta(:,((1:L1)+(l-1)*L1))*AUX;
    end
    efq1=AUX(1);
    xq2=AUX(2:L1);
    aux=normef;
    normef=sqrt(lambda*aux^2+efq1^2);
    Qthetaf(1,1)=1;
    Qthetaf(1,L2)=0;
    Qthetaf(L2,1)=0;
    Qthetaf(L2,L2)=1;
    c=[1
       zeros(L,1)];
    for l=L:-1:1,
      c=Qthetabp(:,((1:L1)+(l-1)*L1))*c;
    end
    ce=[0
        c];
    for l=1:L,
      ce(1:L1)=Qtheta(:,((1:L1)+(l-1)*L1))*ce(1:L1);
    end    
    ce=Qthetaf*ce;
    c=ce(2:L2);
    alpha=c(1);  
    for l=1:L,
      aux=alpha;
      alpha=sqrt(aux^2+(c(l+1))^2);
      Qthetabp(1,1+(l-1)*L1)=1;
      Qthetabp(1,l+1+(l-1)*L1)=0;
      Qthetabp(l+1,1+(l-1)*L1)=0;
      Qthetabp(l+1,l+1+(l-1)*L1)=1;
    end
    AUX=[1
         zeros(L1,1)];
    for l=1:L,
      AUX(1:L1)=Qtheta(:,((1:L1)+(l-1)*L1))*AUX(1:L1);
    end
    AUX=Qthetaf*AUX;
    re=AUX(2:L2);
    for l=1:L,
      re=(Qthetabp(:,((1:L1)+(l-1)*L1)))'*re;
    end
    r=re(2:L1);
    gammap=1;
    for l=1:L,
      aux=gammap;
      gammap=sqrt(aux^2-(r(l))^2);
      Qtheta(1,1+(l-1)*L1)=1;
      Qtheta(1,l+1+(l-1)*L1)=0;
      Qtheta(l+1,1+(l-1)*L1)=0;
      Qtheta(l+1,l+1+(l-1)*L1)=1;
    end
    X=[x
       X(1:N)];		% new input vector
    AUX=[d
         lambda^(1/2)*dq2];
    for l=1:L,
      AUX=Qtheta(:,((1:L1)+(l-1)*L1))*AUX;
    end
    eq1=AUX(1);
    dq2=AUX(2:L1);
    ep=eq1/gammap;	% error sample
    MSE(k+1)=MSE(k+1)+ep^2;	% accumulate MSE*I
  end
  for k=LD1:(K-1),	% iterations
    d=s(k+1)+n(k+1);	% sample of signal to predict
    D=[d 
       D(1:ND)];	% new delay vector
    x=D(LD);		% new input sample
    AUX=[x
         lambda^(1/2)*xq2];
    for l=1:L,
      AUX=Qtheta(:,((1:L1)+(l-1)*L1))*AUX;
    end
    efq1=AUX(1);
    xq2=AUX(2:L1);
    aux=normef;
    normef=sqrt(lambda*aux^2+efq1^2);
    costhetaf=lambda^(1/2)*aux/normef;
    sinthetaf=efq1/normef;
    Qthetaf(1,1)=costhetaf;
    Qthetaf(1,L2)=-sinthetaf;
    Qthetaf(L2,1)=sinthetaf;
    Qthetaf(L2,L2)=costhetaf;
    c=[1
       zeros(L,1)];
    for l=L:-1:1,
      c=Qthetabp(:,((1:L1)+(l-1)*L1))*c;
    end
    ce=[0
        c];
    for l=1:L,
      ce(1:L1)=Qtheta(:,((1:L1)+(l-1)*L1))*ce(1:L1);
    end    
    ce=Qthetaf*ce;
    c=ce(2:L2);
    alpha=c(1);  
    for l=1:L,
      aux=alpha;
      alpha=sqrt(aux^2+(c(l+1))^2);
      costhetabp=aux/alpha;
      sinthetabp=-c(l+1)/alpha;
      Qthetabp(1,1+(l-1)*L1)=costhetabp;
      Qthetabp(1,l+1+(l-1)*L1)=sinthetabp;
      Qthetabp(l+1,1+(l-1)*L1)=-sinthetabp;
      Qthetabp(l+1,l+1+(l-1)*L1)=costhetabp;
    end
    AUX=[1
         zeros(L1,1)];
    for l=1:L,
      AUX(1:L1)=Qtheta(:,((1:L1)+(l-1)*L1))*AUX(1:L1);
    end
    AUX=Qthetaf*AUX;
    re=AUX(2:L2);
    for l=1:L,
      re=(Qthetabp(:,((1:L1)+(l-1)*L1)))'*re;
    end
    r=re(2:L1);
    gammap=1;
    for l=1:L,
      aux=gammap;
      gammap=sqrt(aux^2-(r(l))^2);
      costheta=gammap/aux;
      sintheta=r(l)/aux;
      Qtheta(1,1+(l-1)*L1)=costheta;
      Qtheta(1,l+1+(l-1)*L1)=-sintheta;
      Qtheta(l+1,1+(l-1)*L1)=sintheta;
      Qtheta(l+1,l+1+(l-1)*L1)=costheta;
    end
    X=[x
       X(1:N)];		% new input vector
    AUX=[d
         lambda^(1/2)*dq2];
    for l=1:L,
      AUX=Qtheta(:,((1:L1)+(l-1)*L1))*AUX;
    end
    eq1=AUX(1);
    dq2=AUX(2:L1);
    ep=eq1/gammap;	% error sample
    MSE(k+1)=MSE(k+1)+ep^2;	% accumulate MSE*I
  end
end

ind=0:(K-1);	% sample indexes
MSE=MSE/I;	% calculate MSE
save ofile ind MSE;	% write output variables