%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: esprit_tls.m
%
% AUTHOR: Steve Kogon
% 
% DATE: January 20, 1999
%
% DESCRIPTION: This file estimates the frequencies of sinusoids using 
%              total least squares ESPRIT algorithm (Roy & Kailath 1989).
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%-----------------------------------------------------------
% Copyright 2000, by Dimitris G. Manolakis, Vinay K. Ingle,
% and Stephen M. Kogon.  For use with the book
% "Statistical and Adaptive Signal Processing"
% McGraw-Hill Higher Education.
%-----------------------------------------------------------


function [fest] = esprit_tls(x,P,M)
% x = signal
% P = number of complex exponentials
% M = time-window length
% Nfft = number of FFT frequencies
N = length(x) - M + 1;
X = zeros(N,M);
for n = 1:M
   X(:,n) = x((1:N) + (M-n));
end

% Compute the SVD of the augmented data matrix
[U,S,V] = svd(X);

% Signal subspace (2*N by K matrix of right singular vectors)
Vs = V(:,1:P);

% Split subspace into two subspaces corresponding to the unshifted
% and shifted versions
V1 = Vs(1:(M-1),:);
V2 = Vs(2:M,:);

% Solve for the rotation between these subspaces (TLS version)
[U,S,V] = svd([V1 V2]);
V11 = V(1:P,1:P);
V12 = V(1:P,(P+1):(2*P));
V21 = V((P+1):(2*P),1:P);
V22 = V((P+1):(2*P),(P+1):(2*P));
Psi = -V12*inv(V22);

% Solve for the original rotation that was needed for the original data
Phi = eig(Psi);

% Estimate frequencies
fest = angle(Phi)/(2*pi);