function [A,B,C,P,Q,CH0,RH0] = hankel2ss(CH,RH,tol);
%____________________________________________________________________________________
%
%    [A,B,C,P,Q,CH0,RH0] = hankel2ss(CH,RH,tol);
%
%    Construct a state-space representation from a Hankel matrix.   The construction
%    is based on the Ho algorithm.
%
%    The representation should satisfy,
%
%       H(i,j) = C*A^{i+j-2}*B.
%
%    Several calling formats are supported for efficiency.   These are:
%
%       [A,B,C,P,Q,CH0,RH0] = hankel2ss(H,tol)
%           The matrix H is assumed to be Hankel.  The second argument
%           is the zero tolerance for determining the rank of H.  It is
%           optional with a default value of 1e-8.
%
%       [A,B,C,P,Q,CH0,RH0] = hankel2ss(CH,RH,tol)
%           Block hankel matrices, H, are specified by CH being the first block
%           column and RH being the bottom block row.  This specification
%           format matches the arguments in blkhankel.m.  Block hankel 
%           matrices corresponding to MIMO systems must be specified via this
%           CH,RH format.  SISO systems can be specified either by CH,RH or by
%           H in the syntax given above.    The third argument
%           is the zero tolerance for determining the rank of H. It is 
%           optional with a default value of 1e-8.
%
%     The returned variables are the state-space matrices, A, B and C, and the
%     matrix factors P and Q satisfying,
%
%           Q * blkhankel(CH0,RH0) * P = [I, 0;
%                                         0, 0],
%
%     where the dimension of I is the state dimension of the representation.
%     Note that CH0 and RH0 specify a block Hankel matrix with one fewer block
%     than CH and RH.  These matrices are useful is assessing the effect of the
%     low rank truncation applied in calculating A, B and C.
%
%     References:  BL Ho and RE Kalman, "Effective construction of linear 
%                  state-variable models from input/output functions,"
%                  Regelungstechnik, v.12, pp.545-548, 1966.
%
%                  R Gupta and F Fairman, "Derivation of the Ho algorithm,"
%                  IEEE Trans. Auto. Control, v.19, n.3, pp.270-271, 1974.
%
%     
%     Roy Smith,  10 November 2012.
%
%____________________________________________________________________________________

ztol = 1e-8;
if nargin == 0,
   disp('Syntax:  [A,B,C,P,Q,CH0,RH0] = hankel2ss(CH,RH,tol)');
   return
elseif (nargin == 1),   % SISO case
  nr = 1;
  nc = 1;
  [p,q] = size(CH);  
  RH = CH(p,:);           % bottom row
  CH = CH(:,1);           % first column

elseif (nargin == 2 | nargin == 3),
  if (nargin == 2 & length(RH) == 1),   % SISO case with a tolerance
    ztol = RH;
    nr = 1;
    nc = 1;
    [p,q] = size(CH);  
    RH = CH(p,:);           % bottom row
    CH = CH(:,1);           % first column

  else
    [nrCH,nc] = size(CH);     % determine block column dimension (nc)
    [nr,ncRH] = size(RH);     % determine block row dimension (nr)
    p = nrCH/nr;             % number of block rows
    q = ncRH/nc;             % number of block columns
  end
  if nargin == 3,
    ztol = tol;
  end
end


%  Check for matrix dimension consistency.

if mod(p,1) ~= 0,
  error('Number of rows in column data is not an integer multiple of block row dimension')
end
if mod(q,1) ~= 0,
  error('Number of columns in row data is not an integer multiple of block column dimension')
end

if q == 1,
  error('Insufficient number of blocks in bottom row specification');
  return
end
if p == 1,
  error('Insufficient number of blocks in first column specification');
  return
end
if p + q <= 4,    % need to have at least 3 blocks for both reduced problems.
  error('Insufficient number of impulse matrix blocks specified');
  return
end

if (length(ztol) > 1 | ztol(1) <= 0),
  error('zero tolerance must be strictly positive')
end
  
%   For simplicity we will do this via a singular value decomposition.  It could be done
%   more elegantly via Householder reflections.

if p >= q+1,      % reduce number of block rows.

                             %  note that CH has priority and so 2 elements go into RH0
  CH0 = CH(1:(p-1)*nr,:);
  RH0 = [CH((p-2)*nr+1:(p-1)*nr,:), CH((p-1)*nr+1:p*nr,:), RH(:,nc+1:(q-1)*nc)];

                             % one element of CH inserted in RH to duplicate data.
  CH1 = CH(nr+1:p*nr,:);     % one time-step shift with the same dimensions.
  RH1 = [CH((p-1)*nr+1:p*nr,:), RH(:,nc+1:q*nc)];

else   % in this case reduce the number of block columns (easier)

  CH0 = CH;
  RH0 = RH(:,1:(q-1)*nc);

  CH1 = [CH(nr+1:p*nr,:); RH(:,nc+1:2*nc)];
  RH1 = RH(:,nc+1:q*nc);

end

[x,ncRH0] = size(RH0);
[nrCH0,x] = size(CH0);

H0 = blkhankel(CH0,RH0);
H1 = blkhankel(CH1,RH1);

[U,S,V] = svd(H0);
s = diag(S);
nx = max(find(diag(S) > ztol));    % rank of H.

if (nx == nrCH0 | nx == ncRH0),
  warning('Hankel matrix is full rank - decomposition may be invalid');
end

is2 = 1./sqrt(s(1:nx));        % create the scaling

Q1h = U(:,1:nx)*diag(is2);     % scale the non-zero rank parts.
P1 = V(:,1:nx)*diag(is2);
     
Q = [Q1h';                     % form Q
     U(:,nx+1:nrCH0)'];

P = [P1, V(:,nx+1:ncRH0)];       % form P

A = Q(1:nx,:)*H1*P1;
B = Q(1:nx,:)*H0;
B = B(:,1:nc);
C = H0*P1;
C = C(1:nr,:);

return

%____________________________________________________________________________________






