function [U, sval] = fdsubspaceid(Gdata,omega)
%-------------------------------------------------------------------
%
%    [U, sval] = fdsubspaceid(Gdata,omega)
%
%    Frequency domain subspace identification.  The frequency
%    domain data is stacked in Gdata, with the frequencies specified
%    (in rad/sec) in omega.  We require that 0 <= omega <= pi.
%
%    Gdata may represent data from a MIMO system and the stacking
%    (by row or column) is commensurate with omega.  It may also
%    be a 3 dimensional matrix with frequency as the third dimension.
%    
%    U is an extended observability matrix for the system,
%    and sval are the singular values of the Hankel matrix.
%
%    The function fdsubspaceid(Gdata, omega) generates an extended
%    observability matrix, U, and the Hankel singular values, sval.
%
%    The function fdsubspaceft(U, nx, Gdata, omega) fits the
%    actual state space realization for a specified state
%    dimension.
%
%    The algorithm is that given by McKelvey et al. (1996 Trans. Auto Control)
%    for uniformly spaced data.   The code implements a simple and 
%    non-optimized version of the algorithm - it is intended only for small 
%    academic problems.
%
%    Roy Smith,  16/February/2001.  (modified 6/Dec/2011)
%
%   (c) Roy Smith, 2011.  This code is only for the use of students currently
%   enrolled in 227-0689-00L (System Identification) at the ETH, Zurich.
%
%-----------------------------------------------------------------

if nargin == 0,
  disp('Syntax: [U, sval] = fdsubspaceid(Gdata,omega)')
  return
elseif nargin ~= 2,
  error('incorrect number of input arguments: 2 required')
  return
end

if any (omega > pi) | any (omega < 0),
  error('omega out of range:  0 <= omega <= pi')
  return
end
[n,m] = size(omega);
if min(n,m) > 1,
  error('omega must be a vector')
  return
end
npts = max(n,m);
omega(:) = omega;       % make it a column vector

if (max(diff(omega)) - min(diff(omega)) > sqrt(eps)),
  error('omega must be regularly spaced')
  return
end
if min(omega) ~= 0 ! max(omega) ~= pi,
  error('frequencies must range from 0 to pi')
  return
end

%   The data is put in a 3-D array of dimension: ny x nu x npts.

if ndims(Gdata) == 2,       % data is stacked 
  if n == 1,             % assume stacked side by side (like omega)
    [ny,np] = size(Gdata);
    if mod(np,npts) ~= 0,
      error('Frequency domain data size inconsistency')
      return
    else
      nu = np/npts;
    end
    Gtmp = zeros(ny,nu,npts);
    Gtmp(:) = Gdata;
  else                  % assume stacked vertically (like omega)
    [np,nu] = size(Gdata);
    if mod(np,npts) ~= 0,
      error('Frequency domain data size inconsistency')
      return
    else
      ny = np/npts;
    end
    Gtmp = zeros(ny,nu,npts);
    Gtmp(:) = Gdata.';                 % transpose as it fills by row
    Gtmp = permute(Gtmp,[2,1,3]);  % and transpose the result
  end
  Gdata = Gtmp;       
elseif ndims(Gdata) == 3,
  [ny,nu,np] = size(Gdata);
  if np ~= npts,
    error('Frequency vector length does does not match data')
    return
  end
else
  error('dimension of frequency domain data must be either 2 or 3')
  return
end

M = npts-1;          % M corresponds to that in Tomas' paper.

%   Now conjugate the data.  We assume that it runs from
%   0 to pi.

G1 = cat(3,Gdata,flipdim(conj(Gdata(:,:,[2:end-1])),3));

h = real(ifft(G1,[],3));

%   construct the block Hankel matrix.

q = M;       % set up a square matrix (most likely to exceed)
r = M;      % the dimension of the system.

H = zeros(q*ny,r*nu);  
for i = 1:q,
  for j = 1:r,
    H((i-1)*ny+1:i*ny,(j-1)*nu+1: j*nu) = h(:,:,i+j);
  end  
end

%  H contains h(2) to h(2*M)

[U,S,V] = svd(H);
sval = diag(S);

return

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





