%_______________________________________________________________________________________________
%
%    annihilator_method3b1
%
%    Subspace estimation with irregularly spaced frequencies. 
%
%    This is fairly close to standard techniques for creating the data matrix with an
%    extended observable subspace as its range space.   The vandermonde factorization
%    work shows that this can be done to achieve a Hankel structure in the data matrix.
% 
%    annihilator_method3:  The nuclear norm optimization is invariant to constant offsets
%    in the noise vector.  Try including the minimization of the noise in the objective
%    to see if it makes a numerical difference.
%
%    annihilator_method3b: Add in zero mean noise constraint.  The formulation should
%    be invariant to this but it may remove an unnecessary degree of freedom.
%
%    annihilator_method3b1:  Extract state-space matrices via Ho method.  We form the
%    Hankel [real, imag] matrix for the state-space.
%
%    Roy Smith,  11/November/2012.
%
%_______________________________________________________________________________________________

clear

h = get(0,'CurrentFigure');   % If no figures exist create the first 5
if isempty(h),
  fprintf('Position figures 1 to 5 to make them visible on the screen\n')

  figure(1)  % Bode magnitude: data, true system and fit
  figure(2)  % Bode phase: data, true system and fit
  figure(3)  % Nuclear norm as a function of noise level
  figure(4)  % Rank of the O*X*Wperp matrix vs. noise level
  figure(5)  % Singular values for each rank calculation
  disp('Paused: push any key to continue:');pause
end
    
r2d = 180/pi;
figptr = 1;

%  Test system.

flg_randsys = 1;

if flg_randsys,
  fprintf('generating random system\n');

  ncmplx_pairs = 2;
  nreal = 2;
  nx = 2*ncmplx_pairs+nreal;

  A = [];
  for p = 1:ncmplx_pairs,
    cp = rand*exp(sqrt(-1)*rand*pi/2);
    Ablk = 0.95*[real(cp), imag(cp); -imag(cp), real(cp)];
    A = blkdiag(A,Ablk);
  end

  for p = 1:nreal,
    rp = 0.95*rand;
    A = blkdiag(A,rp);
  end

  X = rand(nx,nx);
  A = X*A*inv(X);
  eA = eig(A);

  fprintf('\nSystem poles:\n');
  disp(eA);

  C = randn(1,nx);
  B = randn(nx,1);
  D = rand-0.5;
else
  fprintf('loading saved system\n');
  load sys_data1
  A = 0.9*A;
  eA = eig(A);
  fprintf('\nSystem poles:\n');
  disp(eA);
end
      
[nx,nx] = size(A);
[ny,nu] = size(D);
sys = ss(A,B,C,D,-1);
sys_hinf_norm = norm(sys,inf);      % normalize to unit infinity norm
sys = (1/sys_hinf_norm)*sys;
[A,B,C,D] = ssdata(sys);

M = 40;                              % number of positive frequency samples (must be even)

%  generate a random frequency vector.  This should not have points that are too close
%  together.

mingap = pi/M/5;
omega_p = rand(1,M)*pi;
omega_p = sort(omega_p);

fprintf('Selecting frequency grid: %i samples, minimum spacing = %g rad/sec.\n',...
      M, mingap);
badidx = find(diff(omega_p)<mingap);
itcnt = 1;
fprintf('  Iteration %i: removing %i freq values closer than %g rad/sec.\n',itcnt,length(badidx),mingap);
while(length(badidx) > 0);
  goodidx = 1:M;
  goodidx(badidx) = [];
  omega_p = omega_p(goodidx);
  while length(omega_p) < M,
    omega_p = [omega_p, pi*rand];
  end
  itcnt = itcnt+1;
  omega_p = sort(omega_p);
  badidx = find(diff(omega_p)<mingap);
  fprintf('  Iteration %i: removing %i freq values closer than %g rad/sec.\n',itcnt,length(badidx),mingap);
end
fprintf('\n');

nvar = 0.002;                          % noise variance

%   calculate a noise vector to add

noise = randn(M,1).*exp(sqrt(-1)*2*pi*rand(M,1));

%  scale noise variance

noise = (sqrt(nvar)/var(noise))*noise;

Gl_p = zeros(M,1);          % frequency response (positive)
Gln_p = zeros(M,1);         % noisy frequency response (positive)
for l = 1:M,
  Gtfw = C*inv(exp(sqrt(-1)*omega_p(l))*eye(nx,nx)-A)*B + D;
  Gl_p(l) = Gtfw;
  Gln_p(l) = Gtfw + noise(l);
end

%   Plot the frequency response

figure(1);
loglog(omega_p,abs(squeeze(Gl_p)),'bx-',omega_p,abs(squeeze(Gln_p)),'rx-.')
grid
xlabel('Freq: [rad/sec]')
ylabel('Magnitude')
axisdat = axis;
axisdat(3) = 0.01;
axisdat(4) = 2;
axis(axisdat);
legend('G_k','G_k noisy','Location','Best')
tstr = sprintf('Frequency response comparison');
title(tstr)

figure(2)
semilogx(omega_p,r2d*angle(squeeze(Gl_p)),'bx-.',...
         omega_p,r2d*angle(squeeze(Gln_p)),'rx-.')
grid
legend('G_k','G_k noisy','Location','Best')
xlabel('Freq: [rad/sec]')
ylabel('Phase [deg]')
drawnow

%  --------------------------------------------------------------------------------------

%  Set the row dimension (following applies only to SISO systems)

rank_ub = 20;      % upper bound on order to be estimated.

q = min(rank_ub,M - rank_ub);

fprintf('Setting # rows to %i\n',q);

%  form the Wc  matrix (c refers to complex-valued)

%  Make this an q x M block matrix

Wc = zeros(q,M);
for i = 1:q,
  Wc(i,:) = exp(1i*(i-1)*omega_p);
end

fprintf('Wc has dimensions %i x %i\n',q,M);
Wr = [real(Wc), imag(Wc)];
fprintf('Wr has dimensions %i x %i\n',q,2*M);

%  Now characterize the null space of Wr

[QwrT,RwrT] = qr(Wr');
Wrperp = QwrT(:,q+1:2*M);

fprintf('Annihilator: norm(Wr * Wrperp) = %g\n',norm(Wr*Wrperp));

%  Check rank conditions with data matrix (noise = 0).

Gc = Wc*diag(Gln_p);       % form complex data matrix.
Gr = [real(Gc), imag(Gc)];

fprintf('\nAnnihilator rank tests (raw data case)\n')
fprintf('  Rank(Gc) = %i\n',rank(Gc))
fprintf('  Rank(Gr) = %i\n',rank(Gr))
fprintf('  Rank(Gr*Wrperp) = %i\n',rank(Gr*Wrperp))

svdGr = svd(Gr);    % calculate singular values for standard approach.

%  --------------------------------------------------------------------------------------

%   The gamma scale chosen is a rough estimate of what is usually required.

%   These are scaled by the number of frequency samples.

log_gamma_max = 3*log10(norm(Gln_p));
log_gamma_max = ceil(10*log_gamma_max)/10;
log_gamma_min = log_gamma_max-2;

Npts = 35;
gamma_vec = logspace(log_gamma_min,log_gamma_max,Npts);
gamma_vec = fliplr(gamma_vec);   % order largest first

nnorm_vec = zeros(Npts,1);
OXrank_vec = zeros(Npts,1);
w_vec = zeros(M,Npts);   % each column is the noise vector

%  The cummulative spectrum is weighted to account for the
%  irregular spacing.  The weight applied is the length of the
%  interval between the midpoints of the surrounding frequencies.
%  It is then normalized.   Because we want to weight w.^2 we take
%  the square root.

w_weight = diff([0, omega_p])/2 + diff([omega_p, pi])/2;
w_weight = w_weight';     
w_weight = M*w_weight/sum(w_weight);
w_weight = sqrt(w_weight);

nmaxrank = 0;            % number of instances of maximum rank.
maxranklimit = 3;        % number of times the max. rank is found
                         % before stopping gamma iteration.

eta = 0.025;             % "whiteness" tolerance.

for vecptr = 1:Npts,     

  gamma = gamma_vec(vecptr);
  fprintf('\nGamma index: %i,  gamma = %g\n',vecptr,gamma);

  cvx_begin
  cvx_precision('high');
  cvx_solver sedumi

  variable w(M) complex;          % freq. domain noise

  minimize norm_nuc([real(Wc*diag(Gln_p - w)), imag(Wc*diag(Gln_p - w))]*Wrperp);

  subject to

    sum(w) == 0;             % remove invariant 

    for l = 1:M,
      [ eye(l,l), (w_weight(1:l) .* w(1:l));
       (w_weight(1:l) .* w(1:l))', ((l/(2*M)+eta))*gamma] == hermitian_semidefinite(l+1);
    end

  cvx_end

  if isnan(cvx_optval),       %  occurs if optimization failed - continue
    nnorm_vec(vecptr) = NaN;
    OXrank_vec(vecptr) = NaN;
    w_vec(:,vecptr) = NaN(M,1);
  elseif (cvx_optval == Inf),   % occurs if infeasibility detected
    fprintf('\nAborting:  infeasible problem for this noise level\n\n');
    break                       % abort as all lower gamma will also be infeasible

  else

    OXWperp = [real(Wc*diag(Gln_p - w)), imag(Wc*diag(Gln_p - w))]*Wrperp;
    %  store data vectors

    w_vec(:,vecptr) = w;
    nnorm_vec(vecptr) = norm_nuc(OXWperp);
    OXrank_vec(vecptr) = rank(OXWperp,1e-7);

  end

  fprintf('\nNorm(O*X*Wperp) = %g\n',nnorm_vec(vecptr));
  fprintf('\nrank(O*X*Wperp) = %g\n',OXrank_vec(vecptr));

  w_model = w_vec(:,vecptr);    % extract the noise
  Gmodel = Gln_p - w_model;

  figure(1)
  loglog(omega_p,abs(squeeze(Gl_p)),'bx-',...
         omega_p,abs(squeeze(Gln_p)),'rx-.',...
         omega_p,abs(squeeze(Gmodel)),'go-')
  grid
  xlabel('Freq: [rad/sec]')
  ylabel('Magnitude')
  axisdat = axis;
  axisdat(3) = 0.01;
  axisdat(4) = 2;
  axis(axisdat);
  legend('G_k','G_k noisy','Gmodel','Location','Best')
  tstr = sprintf('Frequency response comparison');
  title(tstr)

  figure(2)
  semilogx(omega_p,r2d*angle(squeeze(Gl_p)),'bx-.',...
           omega_p,r2d*angle(squeeze(Gln_p)),'rx-.',...
           omega_p,r2d*angle(squeeze(Gmodel)),'go-')
  grid
  legend('G_k','G_k noisy','Gmodel','Location','Best')
  xlabel('Freq: [rad/sec]')
  ylabel('Phase [deg]')

  figure(3)
  semilogx(gamma_vec(1:vecptr)',OXrank_vec(1:vecptr),'bx--')
  axisdat = axis;
  axisdat(3) = 0;
  axisdat(4) = q;
  axis(axisdat);
  legend('rank(O*X*Wperp)')
  xlabel('\gamma bound')
  ylabel('rank(O*X*Wperp')
  grid
  title('rank/noise bound Pareto curve')

  stmp = svd(OXWperp);

  idx1 = [1:length(stmp)]';
  idx2 = [1:length(svdGr)]';
  figure(4)
  semilogy(idx1,stmp,'bx-',idx2,svdGr,'rx-')
  legend('svd(OXWperp)','SVD(H)','Location','NorthEast')
  yl = ylim;
  yl(2) = 10;
  ylim(yl);
  xlabel('Singular value index')
  ylabel('Singular values')
  grid
  drawnow

  %   Now look at the spectrum and cummulative spectrum of the residual

  figure(5);
  subplot(2,1,1)
  plot(omega_p',w.*conj(w),'bx-',omega_p',noise.*conj(noise),'cx-')
  grid
  xlim([0,pi]);
  legend('w (opt. noise)','expt. noise','Location','Best')
  xlabel('Frequency [rad/sec]')
  ylabel('Magnitude')
  title('Spectrum of w and noise');

  subplot(2,1,2)
  plot(omega_p',cumsum(w.*conj(w)),'bx-',...
       omega_p',cumsum(noise.*conj(noise)),'cx-',...
       [0;pi],[gamma/(M-1),gamma],'r-');
  grid
  xlim([0,pi]);
  legend('w (opt. noise)','expt. noise','Location','Best')
  xlabel('Frequency [rad/sec]')
  ylabel('Magnitude')
  title('cummulative spectrum of w and noise');
  drawnow

%  figure(5)
%  subplot(2,1,1)
%  semilogx(gamma_vec(1:vecptr)',nnorm_vec(1:vecptr),'bx-')
%  xlabel('\gamma bound')
%  ylabel('|O*X*Wperp|_N')
%  grid
%  title('Nuclear norm/noise bound Pareto curve')
%  drawnow

  if OXrank_vec(vecptr) >= rank_ub,
    nmaxrank = nmaxrank + 1;
  end
  if nmaxrank >= maxranklimit,
   fprintf('\n\n ** Maximal rank limit reached\n\n');
   break
  end

end

%%   Now look for a target rank on the basis of the above analysis.

incr_idx = find(diff(OXrank_vec)>0);    % find changes in rank.

model_ranks = OXrank_vec(incr_idx);
idx = find(model_ranks > 0 & model_ranks < rank_ub);   % get useful range of ranks
model_ranks = model_ranks(idx);

model_idx = zeros(length(model_ranks),1);
for i = 1:length(model_ranks),
  model_idx(i) = max(find(OXrank_vec == model_ranks(i)));
end

fprintf('Finding models for rank values:\n')
for i = 1:length(model_ranks),
  fprintf('  Index = %i, rank = %i, gamma = %g\n',...
              model_idx(i),model_ranks(i),gamma_vec(model_idx(i)));
end

%%   ----------- Check the properties of each solution -------------------------

fprintf('\nReconstructing state-space and checking properties.\n');

%  Create the Hankel structure for the [r, i; -i, r] parametrization
%  of the Vandermonde matrix.

WMri = zeros(2*M,2*M);
for i = 1:2*M;
  WMri(i,:) = [ real(exp(1i*(i-1)*omega_p)), imag(exp(1i*(i-1)*omega_p))];
end
Wqri = WMri(1:q,:);

%  find the triangular decomposition.

[QMriT,RMriT] = qr(WMri');

Ari = QMriT'*[diag(real(exp(1i*omega_p))), diag(imag(exp(1i*omega_p)));
            -diag(imag(exp(1i*omega_p))), diag(real(exp(1i*omega_p)))]*QMriT;

e2m = [zeros(2*M-1,1); 1];

Wqri_fullperp_tri = zeros(2*M,2*M-q);
for i = 1:2*M-q,
  Wqri_fullperp_tri(:,i) = Ari^(i-1)*e2m;
end
Wqriperp = QMriT*Wqri_fullperp_tri;

for i = 1:length(model_ranks),

  gamma = gamma_vec(model_idx(i));

  fprintf('\n  Model: %i, optimization rank = %i, gamma = %g',i,model_ranks(i),...
         gamma);

  w_model = w_vec(:,model_idx(i));    % extract the noise
  Gmodel = Gln_p - w_model;           % build the optimization model result

  %   Reconstruct from the Hankel matrix.

  Hri = Wqri*[real(diag(Gmodel)), imag(diag(Gmodel));...
             -imag(diag(Gmodel)), real(diag(Gmodel))] * Wqriperp;

  [Urimodel,Srimodel,Vrimodel] = svd(Hri);
  srimodel = diag(Srimodel);
  nx_est_ri = sum(srimodel > 1e-7);
  fprintf('  rank(H(gmodel)) = %g\n',nx_est_ri);

  gsvdidx = [1:nx+2]';
  fprintf('  svd(H(gmodel)) = \n  [ ');
  fprintf(' %8.4g ',srimodel(gsvdidx));
  fprintf(' ... ]\n');

  tstr = sprintf('Model: %i, rank = %i, gamma = %g',...
         i,model_ranks(i), gamma_vec(model_idx(i)));
  fprintf('    Extended observability space structure:\n');

  CH = Hri(:,1:nu);           %  first block column of the Hankel matrix.
  RH = Hri((q-1)+1*ny:q*ny,:);  % bottom block row of Hankel matrix.

  %  the scaling on the Hankel form means that we may have to set quite a low
  %  zero tolerance to get the order to match that in the optimization.

  ztol = srimodel(model_ranks(i))/10;
  [Aest_ri,Best_riX,Cest_ri,Pest,Qest,CH0,RH0] = hankel2ss(CH,RH,ztol);
  [nx_est_ri,nx_est_ri] = size(Aest_ri);

  % the B matrix probably doesn't match the data - examine it anyway.

  fprintf('      Hankel/Ho estimated A matrix eigs:\n')
  eAest_ri = eig(Aest_ri);
  disp(eAest_ri)
  if any(abs(eAest_ri)>= 1.0),
    fprintf('\n    ****** WARNING: Hankel/Ho estimated A matrix is unstable ******\n');
  end
 [est_nx_ri,est_nx_ri] = size(Aest_ri);
 fprintf('  State dimension (determined via Hankel/Ho method) = %i\n',est_nx_ri);

  %  now do LS estimation.

  fprintf('\n\n  LS decomposition of extended observability space for comparison\n');

  GmodelOX = [real(Wc*diag(Gmodel)), imag(Wc*diag(Gmodel))]*Wrperp;

  [Ugmodel,Sgmodel,Vgmodel] = svd(GmodelOX);
  sgmodel = diag(Sgmodel);
  nx_est_ls = sum(sgmodel > 1e-7);
  fprintf('  rank(O*X*Wperp) = %g\n',nx_est_ls);

  fprintf('  svd(O*X*Wperp) = \n  [ ');
  fprintf(' %8.4g ',sgmodel(gsvdidx));
  fprintf(' ... ]\n');

  obs_top = Ugmodel(1:q-1, 1:nx_est_ls);
  obs_bot = Ugmodel(2:q, 1:nx_est_ls);

  Aest_ls = obs_top\obs_bot;
  eAest_ls = eig(Aest_ls);
  
  Cest_ls = Ugmodel(1,1:nx_est_ls);   % Simplest way to estimate C.

  fprintf('      LS estimated A matrix eigs:\n')
  disp(eAest_ls)
  if any(abs(eAest_ls)>= 1.0),
    fprintf('\n    ****** WARNING: estimated A matrix is unstable ******\n');
  end
 
  if (max(abs(eig(Aest_ri))) >= 1.0 &  max(abs(eig(Aest_ls))) >= 1.0),

    fprintf('\n  ***  Both A matrix estimates unstable ***\n\n');

  else

    %  Now fit the B & D terms to the models.  These could also be fitted directly
    %  to the data which might compensate for some of the under approximating.  
    %  However we want to see the extent to which solutions of the optimization are
    %  close to low order LTI systems.
   
    X = zeros(M,nx_est_ri+1);
    for j = 1:M,
          X(j,:) = [Cest_ri*inv(exp(sqrt(-1)*omega_p(j))*eye(nx_est_ri,nx_est_ri)-Aest_ri), 1];
    end
    rX = [real(X); imag(X)];
    rGln_p = [real(Gmodel); imag(Gmodel)];
    BD = rX\rGln_p;
    Best_ri = BD(1:nx_est_ri);
    Dest_ri = BD(nx_est_ri+1);

    est_riGmodel_p = zeros(M,1);         % noisy frequency response (positive)
    for l = 1:M,
      est_riGmodel_p(l) = ...
            Cest_ri*inv(exp(sqrt(-1)*omega_p(l))*eye(nx_est_ri,nx_est_ri)-Aest_ri)*Best_ri + Dest_ri;
    end

    % estimate a D matrix to match the B matrix returned by Ho algorithm.

    Y = zeros(M,1);
    X = ones(M,1);
    for j = 1:M,
          Y(j) = ...
           Gmodel(j) -Cest_ri*inv(exp(sqrt(-1)*omega_p(j))*eye(nx_est_ri,nx_est_ri)-Aest_ri)*Best_riX;
    end
    rY = [real(Y); imag(Y)];
    rX = [real(X); imag(X)];
    Dest_riX = rY\rX;

    est_riXGmodel_p = zeros(M,1);         % noisy frequency response (positive)
    for l = 1:M,
      est_riXGmodel_p(l) = ...
            Cest_ri*inv(exp(sqrt(-1)*omega_p(l))*eye(nx_est_ri,nx_est_ri)-Aest_ri)*Best_riX...
                 + Dest_riX;
    end

    X = zeros(M,nx_est_ls+1);
    for j = 1:M,
          X(j,:) = [Cest_ls*inv(exp(sqrt(-1)*omega_p(j))*eye(nx_est_ls,nx_est_ls)-Aest_ls), 1];
    end
    rX = [real(X); imag(X)];
    rGln_p = [real(Gmodel); imag(Gmodel)];
    BD = rX\rGln_p;
    Best_ls = BD(1:nx_est_ls);
    Dest_ls = BD(nx_est_ls+1);

    est_lsGmodel_p = zeros(M,1);         % noisy frequency response (positive)
    for l = 1:M,
      est_lsGmodel_p(l) = ...
            Cest_ls*inv(exp(sqrt(-1)*omega_p(l))*eye(nx_est_ls,nx_est_ls)-Aest_ls)*Best_ls + Dest_ls;
    end

    %  Compare the various fits.

    fprintf('norm(est_riGmodel_p - Gmodel,''fro'') = %g\n',...
                norm(est_riGmodel_p - Gmodel,'fro'))

    fprintf('norm(est_riXGmodel_p - Gmodel,''fro'') = %g\n',...
                norm(est_riXGmodel_p - Gmodel,'fro'))

    fprintf('norm(est_lsGmodel_p - Gmodel,''fro'') = %g\n',...
                norm(est_lsGmodel_p - Gmodel,'fro'))

    figure
    loglog(omega_p,abs(squeeze(Gl_p)),'bx-',...
           omega_p,abs(squeeze(Gln_p)),'rx-.',...
           omega_p,abs(squeeze(est_riGmodel_p)),'m--',...
           omega_p,abs(squeeze(est_riXGmodel_p)),'k-',...
           omega_p,abs(squeeze(est_lsGmodel_p)),'c-.',...
           omega_p,abs(squeeze(Gmodel)),'go-.')
    grid
    xlabel('Freq: [rad/sec]')
    ylabel('Magnitude')
    title(['Magnitude fit: ',tstr])
    axisdat = axis;
    axisdat(3) = 0.01;
    axisdat(4) = 2;
    axis(axisdat);
    legend('G_k','G_k noisy','Gmodel (H est.)','Gmodel (H est. incl. B)','Gmodel (LS est.)',...
                  'Gmodel (dat)','Location','Best')
    title(tstr)

    figure
    semilogx(omega_p,r2d*angle(squeeze(Gl_p)),'bx-.',...
             omega_p,r2d*angle(squeeze(Gln_p)),'rx-.',...
             omega_p,r2d*angle(squeeze(est_riGmodel_p)),'m--',...
             omega_p,r2d*angle(squeeze(est_riXGmodel_p)),'k-',...
             omega_p,r2d*angle(squeeze(est_lsGmodel_p)),'c-.',...
             omega_p,r2d*angle(squeeze(Gmodel)),'go-.')
    grid
    legend('G_k','G_k noisy','Gmodel (H est.)','Gmodel (H est. incl. B)','Gmodel (LS est.)',...
                  'Gmodel (dat)','Location','Best')
    xlabel('Freq: [rad/sec]')
    ylabel('Phase [deg]')
    title(['Phase fit: ',tstr])

    figure;
    subplot(2,1,1)
    plot(omega_p',w.*conj(w),'bx-',omega_p',noise.*conj(noise),'cx-')
    grid
    xlim([0,pi]);
    legend('w (opt. noise)','expt. noise','Location','Best')
    xlabel('Frequency [rad/sec]')
    ylabel('Magnitude')
    title(['Spectrum (w and noise): ',tstr])

    subplot(2,1,2)
    plot(omega_p',cumsum(w.*conj(w)),'bx-',...
         omega_p',cumsum(noise.*conj(noise)),'cx-',...
         [0;pi],[gamma/(M-1),gamma],'r-');
    grid
    xlim([0,pi]);
    legend('w (opt. noise)','expt. noise','Location','Best')
    xlabel('Frequency [rad/sec]')
    ylabel('Magnitude')
    title(['Cummulative spectrum (w and noise): ',tstr])

    drawnow
  end
end

%----------------------------------------------------------------------
