%_______________________________________________________________________________________________
%
%    mk_figures_TAC_2012_subspace_id
%
%    Generate figures for the 2012 TAC paper on subspace ID.   The basis for the 
%    calculations is the file annihilator_method3b1.m.   The data from a particular
%    run (with a randomly generated system) is saved and processed here.
%
%    Roy Smith,  15/November/2012.
%
%    version _v2:  RSS,  25 July, 2013.  Modified plots in response to the
%    reviewers' comments.
%
%_______________________________________________________________________________________________

clear
    
load 'annihilator_method3b1_2012_11_11c.mat'


%   Plot the frequency response data

figure(1);
subplot(2,1,1)
loglog(omega_p,abs(squeeze(Gln_p)),'b-','LineWidth',1)
hold on
loglog(omega_p,abs(squeeze(Gln_p)),'o-','LineWidth',1)
hold off
grid
xlabel('Frequency [rad/sec]','FontSize',12)
ylabel('Magnitude','FontSize',12)
axis([0.04,4,0.01,2]);

subplot(2,1,2)
semilogx(omega_p,r2d*unwrap(angle(squeeze(Gln_p))),'bo-','LineWidth',1)
grid
xlabel('Frequency [rad/sec]','FontSize',12)
ylabel('Phase [deg]','FontSize',12)
axis([0.04,4,-200,200])
drawnow

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

model_idx = [12, 26]           % choose the model to compare
Nmdls = length(model_idx);       % number of models to compare.

figure(2)
semilogx(gamma_vec',OXrank_vec,'bx-','LineWidth',1)
axis([0.05,10,0,20]);
xlabel('\gamma bound','FontSize',12)
ylabel('rank','FontSize',12)
text(1,1,'Model 1','FontSize',12)
text(0.1,3,'Model 2','FontSize',12)
hold on
semilogx(gamma_vec(model_idx(1)),OXrank_vec(model_idx(1)),'bo','LineWidth',1.5)
semilogx(gamma_vec(model_idx(2)),OXrank_vec(model_idx(2)),'bo','LineWidth',1.5)
hold off
grid

%   Calculate the real system singular values.

Gtrue = Wc*diag(Gl_p);       % form complex data matrix.
OXWperp_true = [real(Gtrue), imag(Gtrue)]*Wrperp;
strue = svd(OXWperp_true);

%   repeat purely for the noise.

WnoiseWperp = [real(Wc*diag(noise)), imag(Wc*diag(noise))]*Wrperp;
snoise = svd(WnoiseWperp);

%   If additional gamma values are to be analyzed, include here.

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

M_f = 200;
omega_f = logspace(-2,log10(pi),M_f);   %  fine frequency grid

% now calculate a frequency response
G_f = zeros(M_f,1);         % noisy frequency response (positive)
for l = 1:M_f,
  G_f(l) = ...
        C*inv(exp(1i*omega_f(l))*eye(nx,nx)-A)*B + D;
end

for i = 1:Nmdls,

  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

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

  %   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');

  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(nx_est_ri)/10;
  [Aest_ri,Best_ri,Cest_ri,Pest,Qest,CH0,RH0] = hankel2ss(CH,RH,ztol);
  [nx_est_ri,nx_est_ri] = size(Aest_ri);

  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 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.  
  %  The D term is later refitted to the data.
 
  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)];
  rGmodel = [real(Gmodel); imag(Gmodel)];
  BD = rX\rGmodel;
  Best_ri = BD(1:nx_est_ri);
  Dest_ri = BD(nx_est_ri+1);

  %  Now refit the D term.

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

  % now calculate a frequency response
  est_riGmodel_p = zeros(M_f,1);         % noisy frequency response (positive)
  for l = 1:M_f,
    est_riGmodel_f(l) = ...
          Cest_ri*inv(exp(sqrt(-1)*omega_f(l))*eye(nx_est_ri,nx_est_ri)-Aest_ri)*Best_ri + Dest_ri;
  end

  %  Compare the various fits.
  
  stmp = svd(OXWperp);
  idx0 = [1:nx]';
  idx1 = [1:length(stmp)]';
  idx2 = [1:length(svdGr)]';

  figure(3)
  if i == 1,
    semilogy(idx2,svdGr,'bx-','LineWidth',1.0)
%    semilogy(idx2,svdGr,'bx','LineWidth',1.0)
    text(15,1,'Data','FontSize',12,'color','blue');
    axis([0,20,1e-15,10])
    xlabel('Singular value index','FontSize',12)
    ylabel('Singular values','FontSize',12)
    grid
    hold on
    semilogy(idx1,stmp,'rx-','LineWidth',1.0);
    text(1,1e-13,'Model 1','FontSize',12,'Color','red');
    semilogy(idx1,snoise,'gx-','LineWidth',1.0);
    text(12.5,2e-2,'Noise','FontSize',12,'Color','green');
    semilogy(idx0,strue(idx0),'kx-','LineWidth',1.0);
    text(6.5,3e-3,'G_{true}','FontSize',12,'Color','black');
    hold off
  elseif i == 2,
    hold on
    semilogy(idx1,stmp,'mx-','LineWidth',1.0);
    text(5,1e-5,'Model 2','FontSize',12,'color','magenta');
    hold off
  elseif i == 3,
    hold on
    semilogy(idx1,stmp,'cx-','LineWidth',1.0);
    hold off
  end

  figure(4)  
  if i == 1
%    loglog(omega_p,abs(squeeze(Gln_p)),'bo-','LineWidth',1.0);
    loglog(omega_p,abs(squeeze(Gln_p)),'bo','LineWidth',1.0);
    text(1.3,0.16,'Data','FontSize',12,'color','blue');
    grid
    xlabel('Frequency [rad/sec]','FontSize',12)
    ylabel('Magnitude','FontSize',12)
    axis([0.04,4,0.01,2]);
    hold on
    loglog(omega_f,abs(squeeze(G_f)),'k-','LineWidth',1.0)
    text(0.3,0.8,'G_{true}','FontSize',12,'Color','black');
%    annotation('arrow',[0.50,0.56], [0.78, 0.70],'color','black');
    annotation('arrow',[0.52,0.56], [0.78, 0.70],'color','black');
    loglog(omega_f,abs(squeeze(est_riGmodel_f)),'r-','LineWidth',1.0)
    text(0.5,0.16,'Model 1','FontSize',12,'Color','red');
    hold off
  elseif i == 2,
    hold on
    loglog(omega_f,abs(squeeze(est_riGmodel_f)),'m-','LineWidth',1.0)
    text(0.18,0.16,'Model 2','FontSize',12,'Color','magenta');
    annotation('arrow',[0.47,0.58],[0.53,0.66],'Color','magenta')
    hold off
  elseif i == 3,
    hold on
    loglog(omega_f,abs(squeeze(est_riGmodel_f)),'c-','LineWidth',1.0)
    hold off
  end    

  figure(5)
  if i == 1
 %   semilogx(omega_p,r2d*unwrap(angle(squeeze(Gln_p))),'bo-','LineWidth',1.0)
    semilogx(omega_p,r2d*unwrap(angle(squeeze(Gln_p))),'bo','LineWidth',1.0)
    text(1.5,-170,'Data','FontSize',12,'color','blue');
    grid
    xlabel('Frequency [rad/sec]','FontSize',12)
    axis([0.04,4,-200,200])
    ylabel('Phase [deg.]','FontSize',12)
    hold on
    semilogx(omega_f,r2d*angle(squeeze(G_f)),'k-','LineWidth',1.0)
    text(0.8,120,'G_{true}','FontSize',12,'Color','black');
    annotation('arrow',[0.63, 0.57], [0.75, 0.69],'Color','black');
    semilogx(omega_f,r2d*angle(squeeze(est_riGmodel_f)),'r-','LineWidth',1.0)
    text(0.8,55,'Model 1','FontSize',12,'Color','red');
    hold off
  elseif i==2,
    hold on
    semilogx(omega_f,r2d*angle(squeeze(est_riGmodel_f)),'m-','LineWidth',1.0)
    text(0.3,-40,'Model 2','FontSize',12,'Color','magenta');
    annotation('arrow',[0.55,0.66],[0.435,0.525],'Color','magenta');
    hold off
  elseif i==3,
    hold on
    semilogx(omega_f,r2d*angle(squeeze(est_riGmodel_f)),'c-','LineWidth',1.0)
    hold off
  end
end

%  Save figures for paper

figure(2)
print -depsc 'TAC2012_id_fig_rank.eps'

figure(3)
print -depsc 'TAC2012_id_fig_svds_v2.eps'

figure(4) 
print -depsc 'TAC2012_id_fig_fitmag_v2.eps'

figure(5) 
print -depsc 'TAC2012_id_fig_fitphase_v2.eps'

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

%  Additional impulse response figure


