
    function [omega_ss, residues_ss]=sm(P,Ahat,bhat)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%%    [omega_ss,residues_ss]=sm(P,Ahat,bhat)
%%
%% Signal Estimation via Selected Harmonic Amplification
%% Subspace Method 2: generalization of ESPRIT
%%
%% Employs an input-to-state filter SYS to generate covariance data.
%%
%% INPUTS:
%%            y  is the time series
%%
%%          sys  filter G(z)= C(I - z^-1 A)^-1 b
%%                          = [A,A*b,C,C*b] 
%%               with C square, and invertible, normalized so that the
%%               state/output covariance with white-noise-input is the identity
%%               i.e.,  if E - A*E*A' = b*b',  then  C=inv(sqrtm(E))
%% OUTPUTS:
%%      omega_ss  frequency estimates based on analysis of the signal subspace
%%   residues_ss  corresponding residues
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%            m  number of frequencies tested for
%%
%%       margin  number of extra modes to model non-whiteness
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   %%% Generate a basis for the signal subspace
   m1=length(Ahat)-1; mm=m1;
   [U,svds,Ustar]=svd(P); Ua=U(:,1:m1);
   sigmashalf=diag(svds(1:m1,1:m1)).^.5;

   phi=[bhat Ahat*Ua]\Ua; mu=phi(1,:); phi(1,:)=[];    % equation (33) in [1]
   omega_ss=angle(eig(phi));

   D=diag(exp(j*omega_ss));
   Gs=axxbc(-Ahat,D',bhat*(diag(D)'));                 % equation (26) in [1]

   Rhalf=Gs\(Ua*diag(sigmashalf));
   residues=diag(Rhalf*Rhalf');

   [residues,order]=sort(residues);

   orderedthetas=omega_ss(order);

   omega_ss=orderedthetas(1:m1);      omega_ss=flipud(omega_ss);
   omega_ss=omega_ss+(omega_ss<0)*2*pi;
   residues_ss=residues(1:m1);        residues_ss=flipud(residues_ss);
   residues_ss=abs(residues_ss);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Based on:
%
% [1] T.T. Georgiou, ``Spectral Estimation via Selective Harmonic Amplification,''
%                    in IEEE Trans. on Automatic Control, to appear
%
%%%%%%% Written by Tryphon Georgiou -- last line of sm.m (March 10, 2000) %%%%%%%%%%%%%%%%%
