%% Estimation of Sinusoid Amplitudes and Phase
%
% Copyright Barry Van Veen 2015
%
% Code consists of four sections
%   1) Set Up Workspace
%   2) Generate example data
%   3) Estimate amplitude and phase
%   4) Analyze noise variance

%% Set up Workspace

close all
clear

set(0,'defaultaxesfontsize',22);
set(0,'defaulttextfontsize',22);
linwidth = 2;

%% Generate Example Data
%
% In this example we generate data consisting of 10 sinusoids. The 
% frequencies in Hz are chosen as prime numbers between 100 and 200 Hz.
% The sampling frequency is 1000 Hz, and the number of samples N=100. The
% amplitudes and phases are chosen for convenience in displaying the final
% result.

fs = 1000; % sampling frequency in Hz
N = 100; % number of samples

a = [ 3, 2, 1, 4, 1, 3, 2, 1, 4, 1 ]; % amplitudes
phi = [ 0, 30, 45, 60, 90, 0, 30, 45, 60, 90]*pi/180; % phase in rads

freq = [101, 103, 107, 109, 113, 127, 137 149, 157, 167]; % frequency Hz

omega = freq*2*pi/fs; % discrete-time frequency in rads

n = 0:N-1;

x = zeros(N,1); % column vector of synthetic data

for k = 1:length(a);
    x = x + a(k)*cos(omega(k)*n - phi(k))';
end

%% Estimate Amplitudes and Phases
%
% This section only requires the frequencies of the sinusoids in a vector
% omega and the data in a column vector x. Furthermore, it assumes there is
% no frequency of zero or pi, as that is a special case.

K = length(omega); % Number of sinusoids
N = length(x); % Number of data samples

n = 0:N-1;

H = zeros(N,2*K);

for k = 1:K
    c = cos(omega(k)*n)';  % column vector of cosine at omega(k)
    s = sin(omega(k)*n)';  % column vector of sine at omega(k)
    
    H(:,2*k-1:2*k) = [c , s]; % collect cosines/sines into matrix
end

f = inv(H'*H)*H'*x;

% Now calculate amplitudes and phases from cosine and sine amplitudes

for k = 1:K
    amp(k) = sqrt(f(2*k-1)^2+f(2*k)^2);
    phase(k) = atan2(f(2*k),f(2*k-1))*180/pi; % 4-quandrant arc tan
end

% Now display the results
figure
subplot(211)
g = stem(freq,amp);
set(g,'LineWidth',linwidth);
ylabel('Amplitude')
subplot(212)
g = stem(freq,phase);
set(g,'LineWidth',linwidth);
ylabel('Phase (Degrees)')
xlabel('Frequency (Hz)')
ylim([-5,100])

%% Noise Analysis
%
% If the data contains white noise with variance sigma2, then the covariance
% of the estimates of f are given by C=sigma2*inv(H'*H). This section
% displays the variance of each element of f obtained from the diagonal
% elements of C for several different values of N. We assume sigma2 = 1 for
% convenience.
%
% This section depends only on the discrete-time frequencies of interest in
% units of radians.

Nvals = [100, 250, 500];
K = length(omega);

for l = 1:length(Nvals);

    N = Nvals(l);
    n = 0:N-1;

    H = zeros(N,2*K);

    for k = 1:K
        c = cos(omega(k)*n)';  % column vector of cosine at omega(k)
        s = sin(omega(k)*n)';  % column vector of sine at omega(k)

        H(:,2*k-1:2*k) = [c , s]; % collect cosines/sines into matrix
    end

    C = inv(H'*H);  % normalized error covariance matrix
    var = diag(C);  % variance of estimates of each element of f

    cos_index = 1:2:2*K-1;  % index of cosine terms in f
    sin_index = 2:2:2*K;   % index of sine terms in f
    
    var_cos(l,:) = 10*log10(var(cos_index)); % variance for cosine amplitudes
    var_sin(l,:) = 10*log10(var(sin_index)); % variance for sine amplitudes
    
end

% Display results (assume 3 different N)

figure
g = plot(freq,var_cos(1,:),'bo',freq,var_cos(2,:),'r*',freq,var_cos(3,:),'kd');
set(g,'LineWidth',linwidth);
legend('N = 100','N = 250', 'N = 500')
ylabel('Var cos amp (dB)')
xlabel('Frequency (Hz)')