%AFQRRLS2 Problem 1.1.1.2.10
%
%   '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);
  for l=1:L,
    Qthetafp(:,((1:L1)+(l-1)*L1))=sparse(eye(L1));
    Qtheta(:,((1:L1)+(l-1)*L1))=sparse(eye(L1));
  end
  normef=abs(x(1));
  gammap=1;
  r=zeros(L,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);
    sinthetaf=efq1/normef;
    mu=normef;
    for l=1:L,
      aux=mu;
      mu=sqrt(aux^2+(xq2(l))^2);
      costhetafp=aux/mu;
      sinthetafp=xq2(l)/mu;
      Qthetafp(l,l+(L-l)*L1)=costhetafp;
      Qthetafp(l,L1+(L-l)*L1)=-sinthetafp;
      Qthetafp(L1,l+(L-l)*L1)=sinthetafp;
      Qthetafp(L1,L1+(L-l)*L1)=costhetafp;
    end
    re=[r
        gammap*sinthetaf];
    for l=L:-1:1,
      re=Qthetafp(:,((1:L1)+(l-1)*L1))*re;
    end
    r=re(2:L1);
    gammap=1;
    for l=1:L,
      aux=gammap;
      gammap=sqrt(aux^2-(r(L1-l))^2);
      costheta=gammap/aux;
      sintheta=r(L1-l)/aux;
      Qtheta(1,1+(l-1)*L1)=costheta;
      Qtheta(1,L2-l+(l-1)*L1)=-sintheta;
      Qtheta(L2-l,1+(l-1)*L1)=sintheta;
      Qtheta(L2-l,L2-l+(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