%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% MODULE: hr1.m
%
% PURPOSE: Stationary sources
%
% AUTHOR: B. Champagne
%
% DATE: 21 Nov. 1992
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Parameters defining array and source configuration:
L = 8;
M = 2; 
SNR = [100, 100]';   % [P1,...,PM], linear scale
theta = [9, 12];     % in degrees

% Parameters controlling algorithms:
Me = 2;              % Estimated value of M
epsilon = .02;
alpha0 = .02;
alpha1 = .075;
alpha3 = .015;
mu1 = .00024;
mu2 = .00012;
mu3 = .0025;
mu4 = .0025;

% Parameters controlling simulation:
itmax = 400;
nemax = 40;
rand('normal')
rand('seed',5)

% Evaluation of signal transformation matrix:
for m = 1:M
  omega = pi * sin(pi*theta(m)/180);
  for l = 1:L
     A(l,m) = exp(j*(l-1)*omega);
  end
end

% Evaluation of true data covariance matrix:
Rs = diag(SNR);
Rn = eye(L);
Rx = A * Rs * A' + Rn;

% Eigendecomposition of the true covariance matrix:
[UL,SV,UR] = svd(Rx);
EVAL = diag(SV(1:L,1:L));
EVEC = UL(:,1:M);
PS = EVEC * EVEC';  % signal-subspace projector

% Preliminary assignments and inititialization:
AMP = sqrt(SNR/2);
JEVEC0 = zeros(1,itmax+1);
JEVECA = zeros(1,itmax+1);
JEVECB = zeros(1,itmax+1);
JEVECC = zeros(1,itmax+1);
JEVEC1 = zeros(1,itmax+1);
JEVEC2 = zeros(1,itmax+1);
JEVEC3 = zeros(1,itmax+1);
JEVEC4 = zeros(1,itmax+1);
JEVAL0 = zeros(M+1,itmax+1);
JEVALA = zeros(M+1,itmax+1);
JEVALB = zeros(M+1,itmax+1);
JEVALC = zeros(M+1,itmax+1);
TH0 = zeros(itmax+1,M);
THA = zeros(itmax+1,M);
THB = zeros(itmax+1,M);
THC = zeros(itmax+1,M);
TH1 = zeros(itmax+1,M);
TH2 = zeros(itmax+1,M);
TH3 = zeros(itmax+1,M);
TH4 = zeros(itmax+1,M);

% used to estimate steady-state value of errors:
S0 = 0;
SA = 0;
SB = 0;
SC = 0;
S1 = 0;
S2 = 0;
S3 = 0;
S4 = 0;

% Main loop over experiments
for NE = 1:nemax
   NE

%  Initialization of algorithms 
   Rhx = zeros(L,L);
   for it = 1:8
      S = AMP .* ( rand(M,1) + j * rand(M,1) );
      N = (1/sqrt(2)) .* ( rand(L,1) + j * rand(L,1) );
      X = A * S + N;
      Rhx = Rhx + X * X';
   end
   Rhx = Rhx / 8;
   R0 = Rhx;
%   T = inv(Rhx);
%   T = 100*eye(L);

   [UL,SV,UR] = svd(Rhx);

   L0 = diag(SV(1:M,1:M));
   U0 = UL(:,1:M);
   G0 = ( real(trace(Rhx)) - sum(L0) ) / (L - M);

   L0A = diag(SV);
   U0A = UL;

   L0B = diag(SV(1:Me,1:Me));
   U0B = UL(:,1:Me);
   G0B = ( real(trace(Rhx)) - sum(L0B) ) / (L - Me);

   L0C = L0B;
   U0C = U0B;
   G0C = G0B;

   W1 = U0B;
   W2 = U0B;
   W3 = U0B;
   W4 = U0B;

   TH = rmusic(U0);
   TH0(1,:) = TH0(1,:) + TH;
   THA(1,:) = THA(1,:) + TH;
   THB(1,:) = THB(1,:) + TH;
   THC(1,:) = THC(1,:) + TH;
   TH1(1,:) = TH1(1,:) + TH;
   TH2(1,:) = TH2(1,:) + TH;
   TH3(1,:) = TH3(1,:) + TH;
   TH4(1,:) = TH4(1,:) + TH;

   err = enorm(PS -  U0 * U0');
   JEVEC0(1) = err + JEVEC0(1);
   for I = 1:M
      err = abs(EVAL(I) - L0(I));
      JEVAL0(I,1) = err/EVAL(I) + JEVAL0(I,1);
   end
   err = abs(EVAL(M+1) - G0);
   JEVAL0(M+1,1) = err/EVAL(M+1)+ JEVAL0(M+1,1);

   err = enorm(PS - U0A(:,1:M) * U0A(:,1:M)');
   JEVECA(1) = err + JEVECA(1);
   for I = 1:M+1
      err = abs(EVAL(I) - L0A(I));
      JEVALA(I,1) = err/EVAL(I) + JEVALA(I,1);
   end

   err = enorm(PS -  U0B(:,1:M) * U0B(:,1:M)');
   JEVECB(1) = err + JEVECB(1);
   for I = 1:M
      err = abs(EVAL(I) - L0B(I));
      JEVALB(I,1) = err/EVAL(I) + JEVALB(I,1);
   end
   err = abs(EVAL(M+1) - G0B);
   JEVALB(M+1,1) = err/EVAL(M+1) + JEVALB(M+1,1);

   err = enorm(PS - U0C(:,1:M) * U0C(:,1:M)');
   JEVECC(1) = err + JEVECC(1);
   for I = 1:M
      err = abs(EVAL(I) - L0C(I));
      JEVALC(I,1) = err/EVAL(I) + JEVALC(I,1);
   end
   err = abs(EVAL(M+1) - G0C);
   JEVALC(M+1,1) = err/EVAL(M+1) + JEVALC(M+1,1);

   err = enorm(PS - W1(:,1:M) * W1(:,1:M)');
   JEVEC1(1) = err + JEVEC1(1);

   err = enorm(PS - W2(:,1:M) * W2(:,1:M)');
   JEVEC2(1) = err + JEVEC2(1);

   err = enorm(PS - W3(:,1:M) * W3(:,1:M)');
   JEVEC3(1) = err + JEVEC3(1);

   err = enorm(PS - W4(:,1:M) * W4(:,1:M)');
   JEVEC4(1) = err + JEVEC4(1);

% Main loop over experiments
   for it = 1:itmax
      it

%     Data generation
      S = AMP .* ( rand(M,1) + j * rand(M,1) );
      N = (1/sqrt(2)) .* ( rand(L,1) + j * rand(L,1) );
      X = A * S + N;

%     Eigendecomposition of R0:
       R0 = (1-alpha0)*R0 + (alpha0*X)*X';
       [UL,SV,UR] = svd(R0);
       L0 = diag(SV(1:M,1:M));
       G0 = ( real(trace(R0)) - sum(L0) ) / (L - M);
       U0 = UL(:,1:M);
       TH0(it+1,:) = rmusic(U0) + TH0(it+1,:);
       err = enorm(PS -  U0 * U0');
       JEVEC0(it+1) = err + JEVEC0(it+1);
       for I = 1:M
          err = abs(EVAL(I) - L0(I));
          JEVAL0(I,it+1) = err/EVAL(I) + JEVAL0(I,it+1);
       end
       err = abs(EVAL(M+1) - G0);
       JEVAL0(M+1,it+1) = err/EVAL(M+1) + JEVAL0(M+1,it+1);

%     Perturbation algorithm PA:
      [L0A,U0A] = pa(L0A,U0A,X,epsilon);
      THA(it+1,:) = rmusic(U0A(:,1:M)) + THA(it+1,:);
      TEMP = U0A(:,1:M) * U0A(:,1:M)';
      err = enorm(PS - TEMP);
      JEVECA(it+1) = err + JEVECA(it+1);
      for I = 1:M+1
         err = abs(EVAL(I) - L0A(I));
         JEVALA(I,it+1) = err/EVAL(I) + JEVALA(I,it+1);
      end

%     Perturbation algorithm PB:
      [L0B,G0B,U0B] = pb(L0B,G0B,U0B,X,epsilon);
      THB(it+1,:) = rmusic(U0B(:,1:M)) + THB(it+1,:);
      err = enorm(PS - U0B(:,1:M) * U0B(:,1:M)');
      JEVECB(it+1) = err + JEVECB(it+1);
      if (it > 200)
         SB = SB + err;
      end
      for I = 1:M
         err = abs(EVAL(I) - L0B(I));
         JEVALB(I,it+1) = err/EVAL(I) + JEVALB(I,it+1);
      end
      err = abs(EVAL(M+1) - G0B);
      JEVALB(M+1,it+1) = err/EVAL(M+1) + JEVALB(M+1,it+1);

%     Perturbation algorithm PC:
      [L0C,G0C,U0C] = pc(L0C,G0C,U0C,X,epsilon);
      THC(it+1,:) = rmusic(U0C(:,1:M)) + THC(it+1,:);
      err = enorm(PS - U0C(:,1:M) * U0C(:,1:M)');
      JEVECC(it+1) = err + JEVECC(it+1);
      for I = 1:M
         err = abs(EVAL(I) - L0C(I));
         JEVALC(I,it+1) = err/EVAL(I) + JEVALC(I,it+1);
      end
      err = abs(EVAL(M+1) - G0C);
      JEVALC(M+1,it+1) = err/EVAL(M+1) + JEVALC(M+1,it+1);

%     Adaptive algorithm YK1:
      [W1,Rhx] = yk1(W1,Rhx,X,alpha1,mu1);
      TH1(it+1,:) = rmusic(W1(:,1:M)) + TH1(it+1,:);
      err = enorm(PS - W1(:,1:M) * W1(:,1:M)');
      JEVEC1(it+1) = err + JEVEC1(it+1);
      if (it > 200)
         S1 = S1 + err;
      end

%     Adaptive algorithm YK2:
      W2 = yk2(W2,X,mu2);
      TH2(it+1,:) = rmusic(W2(:,1:M)) + TH2(it+1,:);
      err = enorm(PS - W2(:,1:M) * W2(:,1:M)');
      JEVEC2(it+1) = err + JEVEC2(it+1);
      if (it > 200)
         S2 = S2 + err;
      end

%     Adaptive algorithm YK3:
%     Inverse covariance matrix gradient algorithm (grad3):
%     Z = T*X;
%     Z = ( ( 1 / ( (1-alpha3)/alpha3 + Z'*X ) ) * Z ) * Z';
%     T = ( 1 / (1-alpha3) ) * (T - Z);
%     W3 = (eye(M) - (2*mu3)*T) * W3;
%     W3 = gorth(W3);
%     TH3(it+1,:) = rmusic(W3(:,1:M)) + TH3(it+1,:);
%     err = enorm(PS - W3(:,1:M) * W3(:,1:M)');
%     JEVEC3(it+1) = err + JEVEC3(it+1);

%     Adaptive algorithm YK4:
%     Data-projection algorithm (grad4):
%     W4 = (eye(M) + ((mu4/(X'*X))*X)*X') * W4;
%     W4 = gorth(W4);
%     TH4(it+1,:) = rmusic(W4(:,1:M)) + TH4(it+1,:);
%     err = enorm(PS - W4(:,1:M) * W4(:,1:M)');
%     JEVEC4(it+1) = err + JEVEC4(it+1);

   end

end

SB = SB / ( (itmax - 200) * nemax );
S1 = S1 / ( (itmax - 200) * nemax );
S2 = S2 / ( (itmax - 200) * nemax );

JEVEC0 = JEVEC0 / nemax;
JEVECA = JEVECA / nemax;
JEVECB = JEVECB / nemax;
JEVECC = JEVECC / nemax;
JEVEC1 = JEVEC1 / nemax;
JEVEC2 = JEVEC2 / nemax;
JEVEC3 = JEVEC3 / nemax;
JEVEC4 = JEVEC4 / nemax;

JEVAL0 = JEVAL0 / nemax;
JEVALA = JEVALA / nemax;
JEVALB = JEVALB / nemax;
JEVALC = JEVALC / nemax;

TH0 = TH0 / nemax;
THA = THA / nemax;
THB = THB / nemax;
THC = THC / nemax;
TH1 = TH1 / nemax;
TH2 = TH2 / nemax;
TH3 = TH3 / nemax;
TH4 = TH4 / nemax;
