Run05_RunForDifferentSamplingFreq.m

clc
clear
fontSize = 10;
format long

t_sample = 0.13;
f0 = 50;
w0 = 2*pi*f0;

N = 20;
tf = 0.5;
nFilter = 1.5*N;
f1 = 20;
f2 = 90;

h3Coeff = 0;%0.020;
h5Coeff = 0;%0.010;
sigma =0;

fs = N*f0;


a = fs/4/pi/sin(2*pi/N);
b = 2*cos(2*pi/N);


dt = 1/fs;

%% filter
fnyq = fs/2;


w1 = f1/fnyq;
w2 = f2/fnyq;

filterCoeffs = fir1(nFilter, [w1 w2]);

%%



Am = 1*sqrt(2);



t = 0:dt:tf;
[row, col] = size(t);
Nsamples = col;

f = f0 *ones(size(t))+ sin(2*pi*1*t) +0.5*sin(2*pi*6*t);
%f = 52 *ones(size(t));
%f = f0 *ones(size(t))+25*t-25*t.^2;
f3 = 3*f;
f5 = 5*f;

f_max = max(f);
f_min = min(f);

w = 2*pi*f;

w3 = 2*pi*f3;
w5 = 2*pi*f5;




phi0 = 0;

theta1(1) = phi0;
theta3(1) = phi0;
theta5(1) = phi0;

thetb1(1) = phi0-2*pi/3;
thetb3(1) = phi0-2*pi/3;
thetb5(1) = phi0-2*pi/3;

thetc1(1) = phi0+2*pi/3;
thetc3(1) = phi0+2*pi/3;
thetc5(1) = phi0+2*pi/3;


for k=2:length(t) 
    theta1(k) = theta1(k-1)+w(k)*dt;
    theta3(k) = theta3(k-1)+w3(k)*dt;
    theta5(k) = theta5(k-1)+w5(k)*dt;
    
    thetb1(k) = thetb1(k-1)+w(k)*dt;
    thetb3(k) = thetb3(k-1)+w3(k)*dt;
    thetb5(k) = thetb5(k-1)+w5(k)*dt;
    
    thetc1(k) = thetc1(k-1)+w(k)*dt;
    thetc3(k) = thetc3(k-1)+w3(k)*dt;
    thetc5(k) = thetc5(k-1)+w5(k)*dt;
end



x = Am*cos(theta1);% + sigma*randn(size(t))+ h3Coeff*Am*cos(theta3)+ h5Coeff*Am*cos(theta5);
y = Am*cos(thetb1);% + sigma*randn(size(t))+ h3Coeff*Am*cos(thetb3)+ h5Coeff*Am*cos(thetb5);
z = Am*cos(thetc1);% + sigma*randn(size(t))+ h3Coeff*Am*cos(thetc3)+ h5Coeff*Am*cos(thetc5);

% Begin: signal preperation for Akke algorithm
V_Akke(1,:) = x;
V_Akke(2,:) = y;
V_Akke(3,:) = z;

V_alpha_beta_Akke = sqrt(2/3)*[1, -1/2, -1/2; 0, sqrt(3)/2, -sqrt(3)/2]*V_Akke;

Vk_Akke = V_alpha_beta_Akke(1,:)+j*V_alpha_beta_Akke(2,:);

Zk_Akke = exp(-j*(w0*t));

Yk_Akke = Vk_Akke .* Zk_Akke;
% End: signal preperation for Akke algorithm


%xh: x with harmonics
%yh: y with harmonics
%zh: z with harmonics
xh = Am*cos(theta1) + sigma*randn(size(t))+ h3Coeff*Am*cos(theta3)+ h5Coeff*Am*cos(theta5);
yh = Am*cos(thetb1) + sigma*randn(size(t))+ h3Coeff*Am*cos(thetb3)+ h5Coeff*Am*cos(thetb5);
zh = Am*cos(thetc1) + sigma*randn(size(t))+ h3Coeff*Am*cos(thetc3)+ h5Coeff*Am*cos(thetc5);

% Begin: signal preperation for Akke algorithm
Vh_Akke(1,:) = xh;
Vh_Akke(2,:) = yh;
Vh_Akke(3,:) = zh;

Vh_alpha_beta_Akke = sqrt(2/3)*[1, -1/2, -1/2; 0, sqrt(3)/2, -sqrt(3)/2]*Vh_Akke;

Vkh_Akke = Vh_alpha_beta_Akke(1,:)+j*Vh_alpha_beta_Akke(2,:);

Zkh_Akke = exp(-j*(w0*t));

Ykh_Akke = Vkh_Akke .* Zkh_Akke;
% End: signal preperation for Akke algorithm


% xhf: xh filtered
% yhf: yh filtered
% zhf: zh filtered
xhf = filter(filterCoeffs,1,xh);
yhf = filter(filterCoeffs,1,yh);
zhf = filter(filterCoeffs,1,zh);

% Begin: signal preperation for Akke algorithm
Vhf_Akke(1,:) = xhf;
Vhf_Akke(2,:) = yhf;
Vhf_Akke(3,:) = zhf;

Vhf_alpha_beta_Akke = sqrt(2/3)*[1, -1/2, -1/2; 0, sqrt(3)/2, -sqrt(3)/2]*Vhf_Akke;

Vkhf_Akke = Vhf_alpha_beta_Akke(1,:)+j*Vhf_alpha_beta_Akke(2,:);

Zkhf_Akke = exp(-j*(w0*t));

Ykhf_Akke = Vkhf_Akke .* Zkhf_Akke;
% End: signal preperation for Akke algorithm

%% our approach for window length of 9

M = 3;
windowLength = 2*M+3; % = 9
K = M; % K = (windowLength-3)/2

firstIndex = K+2;

for k=firstIndex:Nsamples-K-1
    Xk = x(k-K:k+K)';
    Xk_minus = x(k-K-1:k+K-1)';
    Xk_plus = x(k-K+1:k+K+1)';
    f_hatWin9(k) = fs/2/pi*acos(Xk'*(Xk_minus+Xk_plus)/2/(Xk'*Xk));
    f_hatWin9_arccosinefree(k) = f0 + a*(Xk'*(b*Xk-Xk_minus-Xk_plus))/(Xk'*Xk);

    Yk = y(k-K:k+K)';
    Yk_minus = y(k-K-1:k+K-1)';
    Yk_plus = y(k-K+1:k+K+1)';

    Zk = z(k-K:k+K)';
    Zk_minus = z(k-K-1:k+K-1)';
    Zk_plus = z(k-K+1:k+K+1)';


    numArg = Xk'*(Xk_minus+Xk_plus);
    numArg = numArg + Yk'*(Yk_minus+Yk_plus);
    numArg = numArg + Zk'*(Zk_minus+Zk_plus);

    denArg = 2*(Xk'*Xk);
    denArg = denArg + 2*(Yk'*Yk);
    denArg = denArg + 2*(Zk'*Zk);

    arg = numArg/denArg;
    f_hatWin9_3phase(k) = fs/2/pi*acos(arg);
    
    % Antonio Lopez Algorithm
    Xk = [x(k-3); x(k); x(k+3)];
    Xk_minus = [x(k-4); x(k-1); x(k+2)];
    Xk_plus = [x(k-2); x(k+1); x(k+4)];
    f_hat_lopez(k) = fs/2/pi*acos(Xk'*(Xk_minus+Xk_plus)/2/(Xk'*Xk));
    
    % Antonio Lopez Algorithm-- signal distorted
    Xkh = [xh(k-3); xh(k); xh(k+3)];
    Xkh_minus = [xh(k-4); xh(k-1); xh(k+2)];
    Xkh_plus = [xh(k-2); xh(k+1); xh(k+4)];
    
    arg = Xkh'*(Xkh_minus+Xkh_plus)/2/(Xkh'*Xkh);

    if(abs(arg)>1)
        f_hat_lopez_withHarmonics(k) = f_hat_lopez_withHarmonics(k-1);
    else
        f_hat_lopez_withHarmonics(k) = fs/2/pi*acos(arg);
    end

    
    % Antonio Lopez Algorithm-- signal distorted, but pre-filtered
    Xkhf = [xhf(k-3); xhf(k); xhf(k+3)];
    Xkhf_minus = [xhf(k-4); xhf(k-1); xhf(k+2)];
    Xkhf_plus = [xhf(k-2); xhf(k+1); xhf(k+4)];
    f_hat_lopez_withHarmonics_filtered(k) = fs/2/pi*acos(Xkhf'*(Xkhf_minus+Xkhf_plus)/2/(Xkhf'*Xkhf));
    
end

%%

[r, c] = size(t);
Nsamples = c;

Kmax =3;
% we used Kmax instead of K for calculating firstIndex and lastIndex so
firstIndex = Kmax+2+nFilter;% the first element of Xk_minus must be available: it must be the first element of the signal
                            % k-K-1 = 1  ==>  k=K+2
lastIndex = Nsamples-Kmax-1;% the last element of Xk_plus must be available: it must be the first element of the signal
                            % k+K+1 = Nsamples  ==> k = Nsamples-K-1

                            

%% Akke frequency estimation
for k=firstIndex:lastIndex

    Uk_Akke = Yk_Akke(k)*conj(Yk_Akke(k-1));
    f_hat_Akke(k) = f0 + fs/2/pi*atan(imag(Uk_Akke)/real(Uk_Akke));

    Ukh_Akke = Ykh_Akke(k)*conj(Ykh_Akke(k-1));
    f_hat_Akke_withHarmonics(k) = f0 + fs/2/pi*atan(imag(Ukh_Akke)/real(Ukh_Akke));

    Ukhf_Akke = Ykhf_Akke(k)*conj(Ykhf_Akke(k-1));
    f_hat_Akke_withHarmonics_filtered(k) = f0 + fs/2/pi*atan(imag(Ukhf_Akke)/real(Ukhf_Akke));
end
se_Akke = (f(firstIndex:lastIndex)-f_hat_Akke(firstIndex:lastIndex)).^2;
maxSE_Akke = max(se_Akke);
MSE_Akke = mean(se_Akke);


se_Akke_withHarmonics = (f(firstIndex:lastIndex)-f_hat_Akke_withHarmonics(firstIndex:lastIndex)).^2;
maxSE_Akke_withHarmonics = max(se_Akke_withHarmonics);
MSE_Akke_withHarmonics = mean(se_Akke_withHarmonics);

se_Akke_withHarmonics_filtered = (f(firstIndex:lastIndex-nFilter/2)-f_hat_Akke_withHarmonics_filtered(firstIndex+nFilter/2:lastIndex)).^2;
maxSE_Akke_withHarmonics_filtered = max(se_Akke_withHarmonics_filtered);
MSE_Akke_withHarmonics_filtered = mean(se_Akke_withHarmonics_filtered);

% the f_hat_Akke is in fact ahould be assigned to a half dT back
%
%

%% Lopez MSE, single-phase case, window length = 9
se_Lopez = (f(firstIndex:lastIndex)-f_hat_lopez(firstIndex:lastIndex)).^2;
maxSE_Lopez = max(se_Lopez);
MSE_Lopez = mean(se_Lopez);

se_Lopez_withHarmonics = (f(firstIndex:lastIndex)-f_hat_lopez_withHarmonics(firstIndex:lastIndex)).^2;
maxSE_Lopez_withHarmonics = max(se_Lopez_withHarmonics);
MSE_Lopez_withHarmonics = mean(se_Lopez_withHarmonics);

se_Lopez_withHarmonics_filtered = (f(firstIndex:lastIndex-nFilter/2)-f_hat_lopez_withHarmonics_filtered(firstIndex+nFilter/2:lastIndex)).^2;
maxSE_Lopez_withHarmonics_filtered = max(se_Lopez_withHarmonics_filtered);
MSE_Lopez_withHarmonics_filtered = mean(se_Lopez_withHarmonics_filtered);

%% our approach

f_hat_1phase = f0*ones(Kmax+1,Kmax+1+nFilter);
f_hat_3phase = f0*ones(Kmax+1,Kmax+1+nFilter);

f_hat_1phase_withHarmonics = f0*ones(Kmax+1,Kmax+1+nFilter);
f_hat_3phase_withHarmonics = f0*ones(Kmax+1,Kmax+1+nFilter);

f_hat_1phase_withHarmonics_filtered = f0*ones(Kmax+1,Kmax+1+nFilter);
f_hat_3phase_withHarmonics_filtered = f0*ones(Kmax+1,Kmax+1+nFilter);



for K = 0:Kmax
    % K = 0,           Window Length = 3
    % K = 1,           Window Length = 5
    % K = 2,           Window Length = 7
    % K = 3,           Window Length = 9
    % K = 4,           Window Length = 11
    lengthWindow(K+1)  = 2*K+3;
    for k=firstIndex:lastIndex
        Xk = x(k-K:k+K)';
        Xk_minus = x(k-K-1:k+K-1)';
        Xk_plus = x(k-K+1:k+K+1)';
        arg = Xk'*(Xk_minus+Xk_plus)/2/(Xk'*Xk);
        if(abs(arg)>1)
            f_hat_1phase(K+1,k) = f_hat_1phase(K+1,k-1);
        else
            f_hat_1phase(K+1,k) = fs/2/pi*acos(arg);
        end
        f_hat_1phase_arccosinefree(K+1,k) = f0 + a*(Xk'*(b*Xk-Xk_minus-Xk_plus))/(Xk'*Xk);
        
        Xkh = xh(k-K:k+K)';
        Xkh_minus = xh(k-K-1:k+K-1)';
        Xkh_plus = xh(k-K+1:k+K+1)';
        arg = Xkh'*(Xkh_minus+Xkh_plus)/2/(Xkh'*Xkh);
        if(abs(arg)>1)
            f_hat_1phase_withHarmonics(K+1,k) = f_hat_1phase_withHarmonics(K+1,k-1);
        else
            f_hat_1phase_withHarmonics(K+1,k) = fs/2/pi*acos(arg);
        end
        f_hat_1phase_arccosinefree_withHarmonics(K+1,k) = f0 + a*(Xkh'*(b*Xkh-Xkh_minus-Xkh_plus))/(Xkh'*Xkh);
        
        Xkhf = xhf(k-K:k+K)';
        Xkhf_minus = xhf(k-K-1:k+K-1)';
        Xkhf_plus = xhf(k-K+1:k+K+1)';
        arg = Xkhf'*(Xkhf_minus+Xkhf_plus)/2/(Xkhf'*Xkhf);
        if(abs(arg)>1)
            f_hat_1phase_withHarmonics_filtered(K+1,k) = f_hat_1phase_withHarmonics_filtered(K+1,k-1);
        else
            f_hat_1phase_withHarmonics_filtered(K+1,k) = fs/2/pi*acos(arg);
        end
        f_hat_1phase_arccosinefree_withHarmonics_filtered(K+1,k) = f0 + a*(Xkhf'*(b*Xkhf-Xkhf_minus-Xkhf_plus))/(Xkhf'*Xkhf);


        Yk = y(k-K:k+K)';
        Yk_minus = y(k-K-1:k+K-1)';
        Yk_plus = y(k-K+1:k+K+1)';
        
        Zk = z(k-K:k+K)';
        Zk_minus = z(k-K-1:k+K-1)';
        Zk_plus = z(k-K+1:k+K+1)';

        
        numArg = Xk'*(Xk_minus+Xk_plus);
        numArg = numArg + Yk'*(Yk_minus+Yk_plus);
        numArg = numArg + Zk'*(Zk_minus+Zk_plus);
        
        denArg = 2*(Xk'*Xk);
        denArg = denArg + 2*(Yk'*Yk);
        denArg = denArg + 2*(Zk'*Zk);
        
        arg = numArg/denArg;
        if(abs(arg)>1)
            f_hat_3phase(K+1,k) = f_hat_3phase(K+1,k-1);
        else
            f_hat_3phase(K+1,k) = fs/2/pi*acos(arg);
        end
        
        % three-phase arccosine-free
        num = a*(Xk'*(b*Xk-Xk_minus-Xk_plus));
        num = num + a*(Yk'*(b*Yk-Yk_minus-Yk_plus));
        num = num + a*(Zk'*(b*Zk-Zk_minus-Zk_plus));
        
        den = (Xk'*Xk);
        den = den + (Yk'*Yk);
        den = den + (Zk'*Zk);
        
        f_hat_3phase_arccosinefree(K+1,k) = f0 + num/den;
        
        % three-phase with harmonics
        Ykh = yh(k-K:k+K)';
        Ykh_minus = yh(k-K-1:k+K-1)';
        Ykh_plus = yh(k-K+1:k+K+1)';
        
        Zkh = zh(k-K:k+K)';
        Zkh_minus = zh(k-K-1:k+K-1)';
        Zkh_plus = zh(k-K+1:k+K+1)';

        
        numArg = Xkh'*(Xkh_minus+Xkh_plus);
        numArg = numArg + Ykh'*(Ykh_minus+Ykh_plus);
        numArg = numArg + Zkh'*(Zkh_minus+Zkh_plus);
        
        denArg = 2*(Xkh'*Xkh);
        denArg = denArg + 2*(Ykh'*Ykh);
        denArg = denArg + 2*(Zkh'*Zkh);
        
        arg = numArg/denArg;
        if(abs(arg)>1)
            f_hat_3phase_withHarmonics(K+1,k) = f_hat_3phase_withHarmonics(K+1,k-1);
        else
            f_hat_3phase_withHarmonics(K+1,k) = fs/2/pi*acos(arg);
        end
        
        % three-phase with harmonics arccosine-free
        num = a*(Xkh'*(b*Xkh-Xkh_minus-Xkh_plus));
        num = num + a*(Ykh'*(b*Ykh-Ykh_minus-Ykh_plus));
        num = num + a*(Zkh'*(b*Zkh-Zkh_minus-Zkh_plus));
        
        den = (Xkh'*Xkh);
        den = den + (Ykh'*Ykh);
        den = den + (Zkh'*Zkh);
        
        f_hat_3phase_arccosinefree_withHarmonics(K+1,k) = f0 + num/den;
        
        
        % three-phase with harmonics,  Filtered
        Ykhf = yhf(k-K:k+K)';
        Ykhf_minus = yhf(k-K-1:k+K-1)';
        Ykhf_plus = yhf(k-K+1:k+K+1)';
        
        Zkhf = zhf(k-K:k+K)';
        Zkhf_minus = zhf(k-K-1:k+K-1)';
        Zkhf_plus = zhf(k-K+1:k+K+1)';

        
        numArg = Xkhf'*(Xkhf_minus+Xkhf_plus);
        numArg = numArg + Ykhf'*(Ykhf_minus+Ykhf_plus);
        numArg = numArg + Zkhf'*(Zkhf_minus+Zkhf_plus);
        
        denArg = 2*(Xkhf'*Xkhf);
        denArg = denArg + 2*(Ykhf'*Ykhf);
        denArg = denArg + 2*(Zkhf'*Zkhf);
        
        arg = numArg/denArg;
        if(abs(arg)>1)
            f_hat_3phase_withHarmonics_filtered(K+1,k) = f_hat_3phase_withHarmonics_filtered(K+1,k-1);
        else
            f_hat_3phase_withHarmonics_filtered(K+1,k) = fs/2/pi*acos(arg);
        end
        
        % three-phase with harmonics arccosine-free, filtered
        num = a*(Xkhf'*(b*Xkhf-Xkhf_minus-Xkhf_plus));
        num = num + a*(Ykhf'*(b*Ykhf-Ykhf_minus-Ykhf_plus));
        num = num + a*(Zkhf'*(b*Zkhf-Zkhf_minus-Zkhf_plus));
        
        den = (Xkhf'*Xkhf);
        den = den + (Ykhf'*Ykhf);
        den = den + (Zkhf'*Zkhf);
        
        f_hat_3phase_arccosinefree_withHarmonics_filtered(K+1,k) = f0 + num/den;
               
    end
    

%     figure()
%     plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
%     hold on
%     plot(t(firstIndex:lastIndex), f_hat_1phase(K+1,firstIndex:lastIndex),'--','Color','blue','LineWidth',1)
%     plot(t(firstIndex:lastIndex), f_hat_1phase_arccosinefree(K+1,firstIndex:lastIndex),'--','Color','green','LineWidth',1)
%     plot(t(firstIndex:lastIndex), f_hat_3phase(K+1,firstIndex:lastIndex),'--','Color','red','LineWidth',1)
%     plot(t(firstIndex:lastIndex), f_hat_3phase_arccosinefree(K+1,firstIndex:lastIndex),'--','Color','cyan','LineWidth',1)
%     xlabel('time (sec)','FontSize', fontSize, 'FontWeight', 'bold')
%     ylabel('f(t) and f_e_s_t(t))','FontSize', fontSize, 'FontWeight', 'bold')
%     title(['data window length = ' int2str(2*K+3)],'FontSize', fontSize, 'FontWeight', 'bold')
%     legend('exact frequency','estimated frequency')
%     axis([0, tf, f_min-3, f_max+3])

    
%% MSE for distorted signals (with harmonics and/or noise)
    se_1phase_withHarmonics = (f(firstIndex:lastIndex)-f_hat_1phase_withHarmonics(K+1,firstIndex:lastIndex)).^2;
    maxSE_1phase_withHarmonics = max(se_1phase_withHarmonics);
    MSE_1phase_withHarmonics(K+1) = mean(se_1phase_withHarmonics);
    MSE_1phase3phase_withHarmonics(K+1,1) = mean(se_1phase_withHarmonics);

    
    se_1phase_arccosinefree_withHarmonics = (f(firstIndex:lastIndex)-f_hat_1phase_arccosinefree_withHarmonics(K+1,firstIndex:lastIndex)).^2;
    maxSE_1phase_arccosinefree_withHarmonics = max(se_1phase_arccosinefree_withHarmonics);
    MSE_1phase_arccosinefree_withHarmonics(K+1) = mean(se_1phase_arccosinefree_withHarmonics);
    MSE_1phase3phase_withHarmonics(K+1,2) = mean(se_1phase_arccosinefree_withHarmonics);
    
    
    se_3phase_withHarmonics = (f(firstIndex:lastIndex)-f_hat_3phase_withHarmonics(K+1,firstIndex:lastIndex)).^2;
    maxSE_3phase_withHarmonics = max(se_3phase_withHarmonics);
    MSE_3phase_withHarmonics(K+1) = mean(se_3phase_withHarmonics);
    MSE_1phase3phase_withHarmonics(K+1,3) = mean(se_3phase_withHarmonics);
    
    
    se_3phase_arccosinefree_withHarmonics = (f(firstIndex:lastIndex)-f_hat_3phase_arccosinefree_withHarmonics(K+1,firstIndex:lastIndex)).^2;
    maxSE_3phase_arccosinefree_withHarmonics = max(se_3phase_arccosinefree_withHarmonics);
    MSE_3phase_arccosinefree_withHarmonics(K+1) = mean(se_3phase_arccosinefree_withHarmonics);
    MSE_1phase3phase_withHarmonics(K+1,4) = mean(se_3phase_arccosinefree_withHarmonics);       

    
    if (3==K) % i.e. window length of 9
        MSE_1phase3phase_withHarmonics(K+1,6) = MSE_Lopez_withHarmonics;            
        MSE_1phase3phase_withHarmonics(K+1,8) = MSE_Akke_withHarmonics;             
    end
    
%% MSE for filtered signals
    se_1phase_withHarmonics_filtered = (f(firstIndex:lastIndex-nFilter/2)-f_hat_1phase_withHarmonics_filtered(K+1,firstIndex+nFilter/2:lastIndex)).^2;
    maxSE_1phase_withHarmonics_filtered = max(se_1phase_withHarmonics_filtered);
    MSE_1phase_withHarmonics_filtered(K+1) = mean(se_1phase_withHarmonics_filtered);
    MSE_1phase3phase_withHarmonics_filtered(K+1,1) = mean(se_1phase_withHarmonics_filtered);
    
    se_1phase_arccosinefree_withHarmonics_filtered = (f(firstIndex:lastIndex-nFilter/2)-f_hat_1phase_arccosinefree_withHarmonics_filtered(K+1,firstIndex+nFilter/2:lastIndex)).^2;
    maxSE_1phase_arccosinefree_withHarmonics_filtered = max(se_1phase_arccosinefree_withHarmonics_filtered);
    MSE_1phase_arccosinefree_withHarmonics_filtered(K+1) = mean(se_1phase_arccosinefree_withHarmonics_filtered);
    MSE_1phase3phase_withHarmonics_filtered(K+1,2) = mean(se_1phase_arccosinefree_withHarmonics_filtered);
    
    
    se_3phase_withHarmonics_filtered = (f(firstIndex:lastIndex-nFilter/2)-f_hat_3phase_withHarmonics_filtered(K+1,firstIndex+nFilter/2:lastIndex)).^2;
    maxSE_3phase_withHarmonics_filtered = max(se_3phase_withHarmonics_filtered);
    MSE_3phase_withHarmonics_filtered(K+1) = mean(se_3phase_withHarmonics_filtered);
    MSE_1phase3phase_withHarmonics_filtered(K+1,3) = mean(se_3phase_withHarmonics_filtered);
    
    se_3phase_arccosinefree_withHarmonics_filtered = (f(firstIndex:lastIndex-nFilter/2)-f_hat_3phase_arccosinefree_withHarmonics_filtered(K+1,firstIndex+nFilter/2:lastIndex)).^2;
    maxSE_3phase_arccosinefree_withHarmonics_filtered = max(se_3phase_arccosinefree_withHarmonics_filtered);
    MSE_3phase_arccosinefree_withHarmonics_filtered(K+1) = mean(se_3phase_arccosinefree_withHarmonics_filtered);
    MSE_1phase3phase_withHarmonics_filtered(K+1,4) = mean(se_3phase_arccosinefree_withHarmonics_filtered);
    
%     
%     if (0==K)
%         MSE_1phase3phase_withHarmonics_filtered(K+1,5) = MSE_Akke_withHarmonics_filtered;       
%     else
%         MSE_1phase3phase_withHarmonics_filtered(K+1,5) = 0;       
%     end
%     
%     if (3==K) % i.e. window length of 9
%         MSE_1phase3phase_withHarmonics_filtered(K+1,6) = MSE_Lopez_withHarmonics_filtered;       
%     else
%         MSE_1phase3phase_withHarmonics_filtered(K+1,6) = 0;       
%     end

    
    
%      figure()
%      plot(t(firstIndex:lastIndex),mse,'Color','black','LineWidth',1)
%      xlabel('time (sec)','FontSize', fontSize, 'FontWeight', 'bold')
%      ylabel('MSE(f-f_e_s_t)','FontSize', fontSize, 'FontWeight', 'bold')
%      title(['data window length = ' int2str(2*K+3)],'FontSize', fontSize, 'FontWeight', 'bold')
%      axis([0, tf, 0, maxSE])
end



%%
% for 1-phase case, the wibdow of length 3 is completely inefficent 
% therefore we ignore showing their MSE
MSE_1phase3phase_withHarmonics(1,1)=0;
MSE_1phase3phase_withHarmonics(1,2)=0;
% figure(1)
% bar(lengthWindow, MSE_1phase3phase_withHarmonics,'group')
% xlabel('length of data window','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('mean(f-f_e_s_t)^2','FontSize', fontSize, 'FontWeight', 'bold')
% legend('1-phase', '1-phase arccosine-free', '3-phase', '3-phase arccosine-free', 'Akke', 'Lopez')
% title(['Comparison of four proposed approaches, 3^r^d harmonic = ' int2str(h3Coeff*100),'%, 5^t^h harmonic = ' int2str(h5Coeff*100),'%'],'FontSize', fontSize, 'FontWeight', 'bold')
% %title(['Comparison of four proposed approaches, SNR = ' int2str(20*log10(1/sigma))],'FontSize', fontSize, 'FontWeight', 'bold')
% 
% % for 1-phase case, the wibdow of length 3 is completely inefficent 
% % therefore we ignore showing their MSE
% MSE_1phase3phase_withHarmonics_filtered(1,1)=0;
% MSE_1phase3phase_withHarmonics_filtered(1,2)=0;
% figure(2)
% bar(lengthWindow, MSE_1phase3phase_withHarmonics_filtered,'group')
% xlabel('length of data window','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('mean(f-f_e_s_t)^2','FontSize', fontSize, 'FontWeight', 'bold')
% legend('1-phase', '1-phase arccosine-free', '3-phase', '3-phase arccosine-free', 'Akke', 'Lopez')
% title(['Comparison of four proposed approaches, 3^r^d harmonic = ' int2str(h3Coeff*100),'%, 5^t^h harmonic = ' int2str(h5Coeff*100),'%, signal pre-filtered'],'FontSize', fontSize, 'FontWeight', 'bold')
% %title(['Comparison of four proposed approaches, SNR = ' int2str(20*log10(1/sigma)) ', signal pre-filtered'],'FontSize', fontSize, 'FontWeight', 'bold')
% 
% 
% figure(3)
% plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
% hold on
% %plot(t(firstIndex:lastIndex), f_hat_1phase(4, firstIndex:lastIndex), 'red')
% %plot(t(firstIndex:lastIndex), f_hat_1phase_withHarmonics_filtered(4,firstIndex:lastIndex), 'green')
% plot(t(firstIndex:lastIndex), f_hat_3phase_withHarmonics(1,firstIndex:lastIndex), '--', 'Color','blue','LineWidth',1)
% plot(t(firstIndex:lastIndex), f_hat_Akke_withHarmonics(firstIndex:lastIndex), '--', 'Color','red','LineWidth',1)
% xlabel('time (sec)','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('frequency (Hz)','FontSize', fontSize, 'FontWeight', 'bold')
% title('Comparison of our approach and Akke (Distorted Signal)','FontSize', fontSize, 'FontWeight', 'bold')
% legend('exact frequency', 'estimated frequency - our approach', 'estimated frequency - Akke')
% 
% figure(4)
% plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
% hold on
% %plot(t(firstIndex:lastIndex), f_hat_1phase(4, firstIndex:lastIndex), 'red')
% %plot(t(firstIndex:lastIndex), f_hat_1phase_withHarmonics_filtered(4,firstIndex:lastIndex), 'green')
% plot(t(firstIndex:lastIndex-nFilter/2), f_hat_3phase_withHarmonics_filtered(1,firstIndex+nFilter/2:lastIndex), '--', 'Color','blue','LineWidth',1)
% plot(t(firstIndex:lastIndex-nFilter/2), f_hat_Akke_withHarmonics_filtered(firstIndex+nFilter/2:lastIndex), '--', 'Color','red','LineWidth',1)
% xlabel('time (sec)','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('frequency (Hz)','FontSize', fontSize, 'FontWeight', 'bold')
% title('Comparison of our approach and Akke (Pre-filtered Signal)','FontSize', fontSize, 'FontWeight', 'bold')
% legend('exact frequency', 'estimated frequency - our approach', 'estimated frequency - Akke')
% 
% figure(5)
% subplot(211)
% bar(lengthWindow, MSE_1phase3phase_withHarmonics,'group')
% xlabel('length of data window','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('mean(f-f_e_s_t)^2','FontSize', fontSize, 'FontWeight', 'bold')
% legend('1-phase', '1-phase arccosine-free', '3-phase', '3-phase arccosine-free', 'Akke', 'Lopez')
% title(['Comparison of four proposed approaches, 3^r^d harmonic = ' int2str(h3Coeff*100),'%, 5^t^h harmonic = ' int2str(h5Coeff*100),'%'],'FontSize', fontSize, 'FontWeight', 'bold')
% 
% subplot(212)
% bar(lengthWindow, MSE_1phase3phase_withHarmonics_filtered,'group')
% xlabel('length of data window','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('mean(f-f_e_s_t)^2','FontSize', fontSize, 'FontWeight', 'bold')
% legend('1-phase', '1-phase arccosine-free', '3-phase', '3-phase arccosine-free', 'Akke', 'Lopez')
% title(['Comparison of four proposed approaches, 3^r^d harmonic = ' int2str(h3Coeff*100),'%, 5^t^h harmonic = ' int2str(h5Coeff*100),'%, signal pre-filtered'],'FontSize', fontSize, 'FontWeight', 'bold')
% 
% figure(6)
% plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
% hold on
% %plot(t(firstIndex:lastIndex), f_hat_1phase(4, firstIndex:lastIndex), 'red')
% %plot(t(firstIndex:lastIndex), f_hat_1phase_withHarmonics_filtered(4,firstIndex:lastIndex), 'green')
% plot(t(firstIndex:lastIndex), f_hat_1phase_withHarmonics(4,firstIndex:lastIndex), '--', 'Color','blue','LineWidth',1)
% plot(t(firstIndex:lastIndex), f_hat_lopez_withHarmonics(firstIndex:lastIndex), '--', 'Color','red','LineWidth',1)
% xlabel('time (sec)','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('frequency (Hz)','FontSize', fontSize, 'FontWeight', 'bold')
% title('Comparison of our approach and Lopez (Distorted Signal)','FontSize', fontSize, 'FontWeight', 'bold')
% legend('exact frequency', 'estimated frequency - our approach', 'estimated frequency - Lopez')
% 
% figure(7)
% plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
% hold on
% %plot(t(firstIndex:lastIndex), f_hat_1phase(4, firstIndex:lastIndex), 'red')
% %plot(t(firstIndex:lastIndex), f_hat_1phase_withHarmonics_filtered(4,firstIndex:lastIndex), 'green')
% plot(t(firstIndex:lastIndex-nFilter/2), f_hat_1phase_withHarmonics_filtered(4,firstIndex+nFilter/2:lastIndex), '--', 'Color','blue','LineWidth',1)
% plot(t(firstIndex:lastIndex-nFilter/2), f_hat_lopez_withHarmonics_filtered(firstIndex+nFilter/2:lastIndex), '--', 'Color','red','LineWidth',1)
% xlabel('time (sec)','FontSize', fontSize, 'FontWeight', 'bold')
% ylabel('frequency (Hz)','FontSize', fontSize, 'FontWeight', 'bold')
% title('Comparison of our approach and Lopez (Pre-filtered Signal)','FontSize', fontSize, 'FontWeight', 'bold')
% legend('exact frequency', 'estimated frequency - our approach', 'estimated frequency - Lopez')
% 
% 
% 


%%
% N= 3;
% fc = 20;
% fn = fs/2;
% wn = fc/fn;
% [B, A] = butter(N,wn)
% 
% 
% our = f_hat_3phase_withHarmonics(2,:);
% akke = f_hat_Akke_withHarmonics;
% ourf = filter(B,A,our);
% akkef = filter(B,A,akke);
% figure(1)
% plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
% hold on
% plot(t(firstIndex:lastIndex), our(firstIndex:lastIndex),'Color','blue','LineWidth',1)
% plot(t(firstIndex:lastIndex), akke(firstIndex:lastIndex),'Color','red','LineWidth',1)
% 
% 
% figure(2)
% plot(t(firstIndex:lastIndex), f(firstIndex:lastIndex),'Color','black','LineWidth',1)
% hold on
% plot(t(firstIndex:lastIndex), ourf(firstIndex:lastIndex),'Color','blue','LineWidth',1)
% plot(t(firstIndex:lastIndex), akkef(firstIndex:lastIndex),'Color','red','LineWidth',1)

%%
if(sigma==0)

    switch N
        case 10
            save Run05_N10_NoiseFree
        case 20
            save Run05_N20_NoiseFree
        case 30
            save Run05_N30_NoiseFree
        case 40
            save Run05_N40_NoiseFree
        case 50
            save Run05_N50_NoiseFree
        case 60
            save Run05_N60_NoiseFree
        case 70
            save Run05_N70_NoiseFree
        case 80
            save Run05_N80_NoiseFree
        case 90
            save Run05_N90_NoiseFree
        case 100
            save Run05_N100_NoiseFree
        case 110
            save Run05_N110_NoiseFree
        case 120
            save Run05_N120_NoiseFree
        case 130
            save Run05_N130_NoiseFree
        case 140
            save Run05_N140_NoiseFree
        case 150
            save Run05_N150_NoiseFree

    end

else
    switch N
        case 10
            save Run05_N10
        case 20
            save Run05_N20
        case 30
            save Run05_N30
        case 40
            save Run05_N40
        case 50
            save Run05_N50
        case 60
            save Run05_N60
        case 70
            save Run05_N70
        case 80
            save Run05_N80
        case 90
            save Run05_N90
        case 100
            save Run05_N100
        case 110
            save Run05_N110
        case 120
            save Run05_N120
        case 130
            save Run05_N130
        case 140
            save Run05_N140
        case 150
            save Run05_N150
    end
end