function [Gsys] = fdsubspaceft(U, nx, Gdata, omega)
%-------------------------------------------------------------------
%
%    [Gsys] = fdsubspaceft(U, nx, 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.
%
%    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: [Gsys] = fdsubspaceft(U, nx, Gdata, omega)')
  return
elseif nargin ~= 4,
  error('incorrect number of input arguments: 4 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

[qc,qr] = size(U);
if nx > qr,
  error('state dimension larger than U column dimension')
  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

Us = U(:,1:nx);
A = pinv(U(1:qr-ny,1:nx))*U(ny+1:qr,1:nx);
C = U(1:ny,1:nx);

%  set up the least squares fit (no added value in doing the
%  negative time data)

ew = exp(sqrt(-1)*omega);
CA = zeros(npts*ny,(ny+nx));
Gd = zeros(ny*npts,nu);
for i = 1:npts,
  CA((i-1)*ny+1:i*ny,1:ny) = eye(ny,ny);
  CA((i-1)*ny+1:i*ny,ny+1:ny+nx) = C*inv(ew(i)*eye(nx,nx)-A);
  Gd((i-1)*ny+1:i*ny,:) = Gdata(:,:,i);
end

DB = [real(CA); imag(CA)]\[real(Gd); imag(Gd)];
D = DB(1:ny,:);
B = DB(ny+1:ny+nx,:);

Gsys = ss(A,B,C,D,-1);    % return a discrete-time system
                          % with an unspecified sample-time.

return

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





