%FQRRLS2 Problem 1.1.1.2.9
%
%   'ifile.mat' - input file containing:
%      I - members of ensemble
%      K - iterations
%      a1 - coefficient of input AR process
%      sigmax - standard deviation of input
%      Wo - coefficient vector of plant
%      sigman - standard deviation of measurement noise
%      lambda - forgetting factor
% 
%   'ofile.mat' - output file containing:
%      ind - sample indexes 
%      M - misadjustment 

clear all	% clear memory
load ifile;	% read input variables
sigmav=sigmax*sqrt(1-a1^2);	
		% standard deviation of input to AR process
L=length(Wo);		% plant and filter length
L1=L+1;			
L2=L1+1;		% auxiliary constants
N=L-1;			% plant and filter order
MSE=zeros(K,1);		% prepare to accumulate MSE*I
MSEmin=zeros(K,1);	% prepare to accumulate MSEmin*I

for i=1:I,		% ensemble
  X=zeros(L,1);		% initial memory
  v=randn(K,1)*sigmav;		% input to AR process
  x=filter([1,0],[1,a1],v);	% input 
  n=randn(K,1)*sigman;		% measurement noise for i=1:I,
  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
  normef=abs(x(1));
  for k=1:(K-1),	% iterations
    AUX=[x(k+1)
         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(k+1)
       X(1:N)];		% new input vector
    d=Wo'*X+n(k+1);	% noisy desired signal sample
    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;	% a priori error sample
    MSE(k+1)=MSE(k+1)+ep^2;	% accumulate MSE*I
    MSEmin(k+1)=MSEmin(k+1)+(n(k+1))^2;	% accumulate MSEmin*I
  end
end

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