%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  A solution to CS&E Your Homework Assignment Project 4 %
%  The DOA Problem: Coming at You                        %
%  Dianne P. O'Leary 07/03                               %
%                                                        %
%  Filename: solution.m                                  %
%                                                        %
%  This program loads some sample data and estimates     %
%  the directions of arrival (DOAs) of some signals      %
%  using two variants of the Esprit algorithm: SVD       %
%  and eigendecomposition.                               %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  Load the data and initialize some constants.          %
%  Divide the DOA data into two arrays, X1dat and X2dat. %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

load doadata          % X and theta_tru
X1dat = X(1:5,:);
X2dat = X(6:10,:);

nsamp = 700;          % number of DOA times to consider
m = 5;                % number of pairs of sensors

n = 10;               % size of rectangular window for data
forget = .9;          % forgetting factor for exponential
                      %    windowing
dtrue = 2;            % true number of signals
i = sqrt(-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  Problem 3.  Use the SVD algorithm and rectangular     %
%  windowing to find the DOAs, assuming that d is        %
%  known.                                                %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tol = 5*sqrt(n);
[thetasvd,err_rect,rerr_rect,wrongdrect] = ...
                  use_svdesprit(X1dat,X2dat,2,2,n,tol,theta_tru,nsamp);
figure(1)
hold off
plot(n:nsamp,theta_tru(n:nsamp,1),'b','LineWidth',1.5)
hold on
plot(n:nsamp,theta_tru(n:nsamp,2),'b','LineWidth',1.5)
plot(n:nsamp,thetasvd(n:nsamp,1),'r','LineWidth',1.5)
plot(n:nsamp,thetasvd(n:nsamp,2),'r','LineWidth',1.5)
title('Results using rectangular windowing')
xlabel('time')
ylabel('DOA')
axis([0 700 0 30])
print -depsc prob3soln.eps

disp('Problem 3; number of signals known.')
disp('Results from rectangular windowing:')
disp(sprintf('  %f average error',err_rect))
disp(sprintf('  %f average relative error',rerr_rect))
disp('')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  Problem 5.  Use the eigenEsprit algorithm and         %
%  exponential windowing to find the DOAs, assuming      %
%  d is known.                                           %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tol = 5*forget*forget/sqrt(1-forget*forget);
[thetaexp,err_exp,rerr_exp,wrongdexp] = ...
           use_eigesprit(X1dat,X2dat,2,2,forget,tol,theta_tru,nsamp);
figure(2)
hold off
plot(2:nsamp,theta_tru(2:nsamp,1),'b')
hold on
plot(2:nsamp,theta_tru(2:nsamp,2),'b')
plot(2:nsamp,thetaexp(2:nsamp,1),'r')
plot(2:nsamp,thetaexp(2:nsamp,2),'r')
title('Results using exponential windowing')
axis([0 700 0 30])

disp('Problem 5; number of signals known.')
disp('Results from exponential windowing:')
disp(sprintf('  %f average error',err_exp ))
disp(sprintf('  %f average relative error',rerr_exp ))


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                        %
%  Problem 7.  Use the eigenEsprit algorithm and         %
%  exponential windowing to find the DOAs, assuming      %
%  d is unknown.                                         %
%  Experiment with various window sizes, tolerances,     %
%  and forgetting factors.                               %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

disp('Problem 7; number of signals unknown.')
disp('')
disp('Results from rectangular windowing:')
disp('')

disp('    tol_k    n    wrong_d  ave_error  ave_rel_error ')
for k=2:2:6,
for n=4:2:20,
   tol = k*sqrt(n);
   [thetasvd,err_rect,rerr_rect,wrongdrect] = ...
                  use_svdesprit(X1dat,X2dat,2,0,n,tol,theta_tru,nsamp);
   disp(sprintf('   %4d    %4d    %3d    %f    %f', ...
                   k,n,wrongdrect,err_rect,rerr_rect))
end
end

disp('')
disp('Results from exponential windowing:')
disp('')

disp('   tol_k forget wrong_d   ave_error  ave_rel_error ')
for k=2:2:6, 
for forget=.6:.05:.95
   tol = k*forget*forget/sqrt(1-forget*forget);
   [thetaexp,err_exp,rerr_exp,wrongdexp] = ...
           use_eigesprit(X1dat,X2dat,2,0,forget,tol,theta_tru,nsamp);
   disp(sprintf('  %4d %f   %4d    %f    %f', ...
                   k,forget,wrongdexp,err_exp,rerr_exp))
end
end