Code

ML Estimation of Several Frequencies Using the Nonuniform FFT

Jesus Selva July 8, 2011 Coded in Matlab

This snippet computes the ML estimate of several frequencies, and is a significant enhancement of snippet
 
     http://www.dsprelated.com/showcode/164.php

These are its main features:

+Its complexity is independent of the number of samples, once a single FFT has been computed. So, its speed is roughly the same for short or long data vectors. This is due to the barycentric interpolation which I used in other snippets.

+It is quite robust because it detects the frequencies even when the separation among them is small.

To work, the method needs two additional input parameters:

 1) An estimate of the noise variance.

 2) A false alarm probability. This is the probability of detecting
    a frequency in a data vector when there isn't any.

From these input data the method adds frequencies sequentially until the maximum of the periodogram is below a threshold, which is computed from 1) and 2).

There are two ways to run the code:

1) Place it all in a file called Demo.m and execute it. Then you can choose two options:

1.1) Perform a single estimation. Then there are two options:

1.1.1) Perform estimation directly from a random data realization.

1.1.2) Perform estimation manually. Here, a figure appears in which there are two subplots. The first shows the FFT of the data (blue) and the fit produced by the frequencies you have previously introduced (red). The second shows the residual error, i.e, the FFT of the data but substracting the frequencies you have already fitted. The red line is the detection threshold. You have the following options:

+Add a frequency. For this, input 'a' in the Matlab command window and then select one of the local maxima in the second subplot. It is important to select the maximum accurately; it is a good idea to make the figure wider or full screen. Once selected, the ML fit is recomputed automatically. If you were accurate in selecting the maximum, then it should have disappeared in the second subplot. Repeat this operation till the blue curve in the second subplot (residual error) is below the red line (detection threshold). Then, press 'x' in the command window to obtain a struct with the estimates.

+Delete a frequency. If something went wrong and one frequency is fitting the noise, input 'd' in the command window and then press the mouse in the first subplot at a position close to the frequency you wish to delete. It will disappear.

+Exit and get estimates. Press 'x' in the command window.

1.2) Evaluate the root-mean-square error performance. In this option, the RMS error is computed for a random scenario with five frequencies. You can see in the command window the true frequencies, the Cramer-Rao bounds and the RMS errors. Usually the RMS error should be the same as the Cramer-Rao bound since the ML estimator is optimum (efficient), unless the separation between two frequencies is too small.

2) Place all functions in seperate files and call MLEstimate for performing single estimations.

The algorithms in this code have been published in several places. The interpolation method is that in

[Bary] J. Selva, 'Design of barycentric interpolators for uniform and
nonuniform sampling grids,' IEEE Transactions on Signal Processing,,
vol. 58, no. 3, pp. 1618-1627, Mar 2010.

However, it is applied to a DFT sum. This point is explained in

[Spec] http://arxiv.org/pdf/1104.3069v1

As to the ML cost function minimization, there are several references. See, among others,

[Cost] J. Selva, 'An efficient Newton-type method for the computation of ML estimators in a Uniform Linear Array,' IEEE Transactions on Signal Processing, vol. 53, no. 6, pp. 2036-2045, June 2005.

J. Selva, 'Interpolation of bounded band-limited signals and applications,' IEEE Transactions on Signal Processing, vol. 54, no. 11, pp. 4244-4260, Nov 2006.


I hope you enjoy it! Please let me know if you find any bug.


function st = Demo

% Author: J. Selva.
% Date: July 2011.
% Version: 0.

st = [];

n = input('Options: 1 perform single estimation, 2 estimate RMS error: ');

switch n
  case 1
    st = TestSingleTrial;
  case 2
    TestRMSError;
end

function st = TestSingleTrial

%Scenario with 5 frequencies. 

M = 2048;
SNRInSpecdB = 27; %Signal-to-noise ratio after correlation. 

Nf = 5;
df = 1/M; %Minimum separation among frequencies. 

MaxAmp = 1; %Maximum amplitude.
PFA = 10.^-5;
nVar = M*MaxAmp^2/10^(SNRInSpecdB/10);
nThres = NoiseThres(M,nVar,PFA);
MinAmp = 2*nThres/M;

[vf,va] = RandomFreqAndAmpVectors(Nf,df,MinAmp,MaxAmp);

y = DataRealization(vf,va,M,nVar);

p = Preprocessing(y,nVar,PFA);

disp('True frequencies: ')
disp(vf)

disp('True amplitudes: ')
disp(va)

disp(['Post-correlation Signal-to-noise ratio: ',...
      num2str(SNRInSpecdB),' dB'])

s = input('To compute ML estimate, use (S)erial  or (M)anual method? ','s');

if strcmp(lower(s),'s')
  st = MethodSerial1(p);
else
  st = MethodManual1(p);
end

disp('ML frequency estimates: ')
disp(st.Freqs)

disp('ML amplitude estimates: ')
disp(st.Amps)

function TestRMSError

%Estimates the RMS error and computes the Cramer-Rao bounds.

format compact
format short g

M = 2048;
SNRInSpecdB = 27;

Nf = 5;
df = 1/M;

MaxAmp = 1;
PFA = 10.^-5;
nVar = M*MaxAmp^2/10^(SNRInSpecdB/10);
nThres = NoiseThres(M,nVar,PFA);
MinAmp = 2*nThres/M;

[vf,va] = RandomFreqAndAmpVectors(Nf,df,MinAmp,MaxAmp);

crb = CRBBound(vf,va,M,nVar);

Ntr = 0;
S2 = zeros(Nf,1);
for k = 1:1e5

  y = DataRealization(vf,va,M,nVar);

  p = Preprocessing(y,nVar,PFA);

  st = MethodSerial1(p);
  
  if length(st.Freqs) == 5
    Ntr = Ntr + 1;
    S2 = S2 + (vf-st.Freqs).^2;
  end
  
  if mod(k,100) == 0
    disp(['Number or trials: ', num2str(k)])
    disp(['             Frequencies:      ' num2str(vf.')])
    disp(['Deviation CRB bounds (dB):   ' num2str(20*log10(crb.'))])
    disp(['          RMS errors (dB):   ' num2str(10*log10(S2(:).'/Ntr))])
    disp(' ');
  end
end

function [vf,va] = RandomFreqAndAmpVectors(Nf,MinFreqSep,MinAmp,MaxAmp)

%Constructs random vectors of frequencies and amplitudes. The module of the largest
%amplitude is MaxAmp. There are no amplitudes with module below MinAmp, and the minimum
%separation among frequencies is MinFreqSep.

vf = sort(rand(Nf,1));
vf = vf + MinFreqSep*(cumsum(ones(Nf,1))-1);
vf = vf/(1+(Nf-1)*MinFreqSep) - 0.5;

pr = rand(Nf,1);
ind = floor(rand(1)/Nf)+1;
pr(ind) = 1;

va = (MinAmp+(MaxAmp-MinAmp)*pr) .* exp(1i*2*pi*rand(Nf,1));

function y = DataRealization(vf,a,M,nVar)

m1 = FirstIndex(M);

y = exp(1i*2*pi*(m1:m1+M-1).'*vf(:).')*a(:)+randn(M,2)*[1 ; 1i]*sqrt(nVar/2);

function p = Preprocessing(y,nVar,PFA)

%Performs the preprocessing. Fundamentally this consists in computing the FFT inside 
%"Correlation.m".
%The input parameters are
%
% y: Data vector.
% nVar: Noise variance estimate. 
% PFA: Probability of seeing a frequency when there isn't any. Usually a very low value
%      like 1e-5.

p = [];
p.M = length(y);
p.nVar = nVar;
p.PFA = PFA;
p.nThres = NoiseThres(p.M,p.nVar,p.PFA); %Noise threshold.

icf = Correlation(y,p.nThres); %Struct with data for interpolating the correlation.

p.cfd = Cost(real(y(:)'*y(:)),icf); %Struct with data for computing the ML cost function ...
                                    %and its derivatives. 

p.freqThres = 1/(10*p.M); %If two frequencies differ in less than ...
                          %freqThres  in  the  iterative  search, then   one  of  them  is
                          %discarded. This prevents ill-conditioning.

p.freqStopThres = 1/(p.M*1e4); %Threshold for stopping the ML cost function minimization.

function st = MethodSerial1(p)

%Adds frequencies and computes ML estimates sequentially.

vfEst = [];
ModifiedEstimation = logical(1);

while ModifiedEstimation
  
  [vf1,va1] = Correlation(p.cfd.icf,'gridMainLocalMaxima',vfEst);

  ModifiedEstimation = ~isempty(vf1);

  if ModifiedEstimation
    vfEst(end+1) = vf1(1);
    [listv,listLv,listCL,ErrRatio] = LookForMinimum(p.cfd,vfEst,...
	10.^-8,p.freqThres,p.freqStopThres);
    vfEst = sort(listv(:,end));
  end 
end

st = Report(p,vfEst);

function st = MethodManual1(p,varargin)

%Allows the user to set the initial frequency estimates on a plot.

if nargin == 1
  vfEst = [];
else
  vfEst = varargin{1};
end

vfEst = vfEst(:);

figure
subplot(2,1,1)
Correlation(p.cfd.icf,'PlotPeriodogramFit',vfEst);
yl = get(gca,'YLim');
subplot(2,1,2)
Correlation(p.cfd.icf,'PlotPeriodogramFitError',vfEst);

while 1
  pr1 = input('A/a -> Add freq., D/d -> Delete freq, X/x -> exit: ','s');
  
  switch lower(pr1)
    case 'a'
      [f1,pr] = ginput(1);
      vfEst = sort([vfEst; f1]);
      [listv,listLv,listCL,ErrRatio] = LookForMinimum(p.cfd,vfEst,10.^-8,... 
	  p.freqThres,p.freqStopThres);
      vfEst = sort(listv(:,end));

      subplot(2,1,1)
      Correlation(p.cfd.icf,'PlotPeriodogramFit',vfEst);

      subplot(2,1,2)
      Correlation(p.cfd.icf,'PlotPeriodogramFitError',vfEst);

    case 'd'
      [f1,pr] = ginput(1);
      [pr,ind] = min(abs(f1-vfEst));
      vfEst(ind) = [];

      subplot(2,1,1)
      Correlation(p.cfd.icf,'PlotPeriodogramFit',vfEst);

      subplot(2,1,2)
      Correlation(p.cfd.icf,'PlotPeriodogramFitError',vfEst);

    case 'x'
      
      st = Report(p,vfEst);
      return
  end
end

function thres = NoiseThres(M,nVar,PFA)

%Computes the noise threshold. The method will discard any correlation local maximum below
%this threshold.

thres = sqrt((M*nVar/2)*icdf('chi2',(1-PFA)^(1/M),2));

function m1 = FirstIndex(M)

m1 = -ceil(M/2);

function varargout = Correlation(varargin)

%Computes the correlation value and its derivatives by calling "Barycentric". It does
%other things like plotting. 

if ~isstruct(varargin{1})
  st = [];
  temp = {[],[],10.^-15,1.5};
  [temp{1:nargin}] = deal(varargin{:});
  [a,st.nThres,epsilon,OvFMin] = deal(temp{:});
  a = a(:).';
  
  st.M = length(a);
  st.K = 2^nextpow2(st.M*OvFMin);
  st.bary = Barycentric(0,SampleFourierCorrelation1DVer1(a,st.K),...
           -2*FirstIndex(st.M),1/st.K,whatP(1/st.K,-2*FirstIndex(st.M),epsilon));
       

  st.IndActive = find(abs(st.bary.a) > st.nThres);
  st.IndActive = unique([st.IndActive-1,st.IndActive,st.IndActive+1]);
  if st.IndActive(1) == 0
    st.IndActive(1) = [];
  end

  if st.IndActive(end) == st.K+1
    st.IndActive(end) = [];
  end
  
  st.aTActive = st.bary.a(st.IndActive);
  st.vfActive = centreFreq((st.IndActive-1)/st.K);
  varargout = {st};
  return
end 

[st,msg] = deal(varargin{1:2});
  
switch msg
    
  case 'value'
    varargout = {Barycentric(st.bary,varargin{3:end})};
    
  case 'gridValueInRange'
      
    [ind1,ind2] = deal(varargin{3:4});
    varargout = {st.bary.a(mod(ind1:ind2,st.K)+1)};
     
  case 'ampEstimates'
      
    vf = varargin{3};
    if length(vf) == 0
      varargout = {[]};
    end

    varargout = {RegGeoSum(st.M,vf,vf,0,0) \ Barycentric(st.bary,vf)};
      
  case 'gridValue'
    
    lInd = varargin{3};
    if nargin == 4
      vf = varargin{4};
    else
      vf = [];
    end
      
    if isempty(vf)
      varargout = {st.bary.a(mod(lInd,st.K)+1)};
    else
      vf1 = centreFreq(lInd/st.K);
      
      VRaa = RegGeoSum(st.M,vf,vf,0,0);
      VRa1a = RegGeoSum(st.M,vf1,vf,0,0);
      
      pr = VRa1a*(VRaa\Barycentric(st.bary,vf));
      pr = reshape(pr,size(lInd));

      varargout = {st.bary.a(mod(lInd,st.K)+1)-pr};
    end

  case 'gridFit'
    
    lInd = varargin{3};
    if nargin == 4
      vf = varargin{4};
    else
      vf = [];
    end
      
    if isempty(vf)
      varargout = {zeros(size(lInd))};
    else
      vf1 = centreFreq(lInd/st.K);
      
      VRaa = RegGeoSum(st.M,vf,vf,0,0);
      VRa1a = RegGeoSum(st.M,vf1,vf,0,0);
      
      pr = VRa1a*(VRaa\Barycentric(st.bary,vf));
      pr = reshape(pr,size(lInd));

      varargout = {pr};
    end
    
  case 'plotErrorIndB'

    if nargin == 2
      vf = [];
    else 
      vf = varargin{3};
    end
      
    s = 20*log10(abs(Correlation(st,'gridValue',0:st.K-1,vf)));
    gf = centreFreq((0:st.K-1)/st.K);
    [gf,ord] = sort(gf);
    s = s(ord);
      
    plot(gf,s,[-0.5,0.5],20*log10(abs(st.nThres)*[1,1]))
      
    Ms = max(s);
    set(gca,'YLim',[Ms-50,Ms+10])
    xlabel('Frequency (f)')
    ylabel('Error spectrum (dB)')
    grid on
    
    varargout = {};
      
  case 'PlotPeriodogramFit'
    
    vfEst = varargin{3};
    
    if isempty(vfEst)
      
       s0 = 20*log10(abs(Correlation(st,'gridValue',0:st.K-1)));
       gf = centreFreq((0:st.K-1)/st.K);
       [gf,ord] = sort(gf);
       s0 = s0(ord);
      
       plot(gf,s0)
       hold on
       nt = 20*log10(abs(st.nThres));
       plot([-0.5,0.5],nt([1,1]).',...
	   'LineWidth',2,'LineStyle','--','Color',[1 0 0])
       hold off
       text(-0.49,nt+2,'Noise threshold')
       
       xlabel('Frequency (f)')
       ylabel('Amplitude (dB)')
       title('Periodogram data, fit, and est. frequencies (stems)')
       return
    end
    
    vaEst = Correlation(st,'ampEstimates',vfEst);
    
    stem(vfEst,20*log10(st.M*abs(vaEst)),'g')
    
    hold on 
    
    s0 = 20*log10(abs(Correlation(st,'gridValue',0:st.K-1)));
    gf = centreFreq((0:st.K-1)/st.K);
    [gf,ord] = sort(gf);
    s0 = s0(ord);
      
    plot(gf,s0,'b')
    hold on
    nt = 20*log10(abs(st.nThres));
    plot([-0.5,0.5],nt([1,1]).',...
	'LineWidth',2,'LineStyle','--','Color',[1 0 0])
      
    s1 = 20*log10(abs(Correlation(st,'gridFit',0:st.K-1,vfEst)));
    s1 = s1(ord);
      
    plot(gf,s1,'r')

    Ms = max(s0);
    set(gca,'YLim',[Ms-50,Ms+10])
    
    xlabel('Frequency (f)')
    ylabel('Amplitude (dB)')
    title('Periodogram fit')
    
    hold off
    
    varargout = {};
    
  case 'PlotPeriodogramFitError'
    
    if nargin == 2
      vfEst = [];
    else
      vfEst = varargin{3};
    end
    
    vaEst = Correlation(st,'ampEstimates',vfEst);
    
    s0 = 20*log10(abs(Correlation(st,'gridValue',0:st.K-1,vfEst)));
    gf = centreFreq((0:st.K-1)/st.K);
    [gf,ord] = sort(gf);
    s0 = s0(ord);
     
    nt = 20*log10(abs(st.nThres));
    
    plot(gf,s0)
    hold on 
    plot([-0.5,0.5],nt([1,1]).',...
	'LineWidth',2,'LineStyle','--','Color',[1 0 0])
    hold off
    
    Ms = max(s0);
    set(gca,'YLim',[Ms-50,Ms+10])
    
    xlabel('Frequency (f)')
    ylabel('Amplitude (dB)')
    title('Periodogram error')
    
    varargout = {};
    
  case 'gridValueGivenAmps'
      
    [lInd,vf,aEst] = deal(varargin{3:5});
    vf1 = centreFreq(lInd/st.K);
    VRa1a = RegGeoSum(st.M,vf1,vf,0,0);
    
    pr1 = st.bary.a(mod(lInd,st.K)+1);
    pr1 = reshape(pr1,size(lInd));
    pr2 = VRa1a*aEst;
    pr2 = reshape(pr2,size(lInd));
      
    varargout = {pr1-pr2};
      
  case 'gridLocalMaxima'
      
    if nargin == 2

      pr = [-2,-1,0].';
      
      pr = abs(st.bary.a(mod(st.IndActive([1;1;1],:) + ...
             pr(:,ones(1,length(st.IndActive))),st.K)+1));

      pr = pr(1,:)<=pr(2,:) & pr(3,:)<=pr(2,:);
      
      vf = st.vfActive(pr);
      va = st.aTActive(pr);
   
      varargout = {vf,va};
      return
    end
      
    vf = varargin{3};
      
    if length(vf) == 0
      [varargout{1:2}] = Correlation(st,'gridLocalMaxima');
      return
    end
   
    amp = Correlation(st,'ampEstimates',vf);

    tmp = abs(Correlation(st,...
	    'gridValueGivenAmps',st.IndActive-1,vf,amp)) >= st.nThres;
     
    ind = st.IndActive(tmp);
    
    pr = [-2,-1,0].';
    
    ind1 = ind([1,1,1].',:) + pr(:,ones(1,length(ind)));
    
    pr = abs(Correlation(st,'gridValueGivenAmps',ind1,vf,amp));

    pr = pr(1,:)<=pr(2,:) & pr(3,:)<=pr(2,:);
    
    ind = ind(pr)-1;
   
    if isempty(ind)
      varargout = {[],[]};
    else
      varargout = {centreFreq(ind/st.K),Correlation(st,...
	  'gridValueGivenAmps',ind,vf,amp)};
    end

  case 'gridMainLocalMaxima'
      
    [vf,va] = Correlation(st,'gridLocalMaxima',varargin{3:end});
    if length(vf) == 0
      varargout = {[],[]};
      return
    end
	  
    [varargout{1:2}] = selectLocalMaximaVer3(vf,va,st.nThres,st.M);
	
      
  case 'gridMaximum'
      
    pr = Correlation(st,'gridValue',0:st.K-1,varargin{3:end});
    [ma,ind] = max(abs(pr));
      
    varargout = {(ind-1)/st.K,ma};
      
end

function tab = SampleFourierCorrelation1DVer1(a,K)  

M = length(a);
m1 = FirstIndex(M);

a = a(:).';
tab = fft([a(-m1+1:end),zeros(1,K-M),a(1:-m1)]);

function out = centreFreq(f)

out = f + ceil(-0.5-f);

function P = whatP(T,B,epsilon)

P = ceil(acsch(epsilon)/(pi*(1-B*T)));

function d = dist(x,y)

if x<y
  d = min([y-x,x+1-y]);
else
  d = min([x-y,y+1-x]);
end

function v = sincMask(x,y,M)

v = zeros(size(x));
for k = 1:length(x)
  x1 = max([dist(x(k),y)-1/(2*M),0]);
  v(k) = 1/max([M*sin(pi*x1),1]); 
end

function v = slBoundVer2(x,vam,vfe,nThres,M1)

vam = vam(:);
vfe = vfe(:);

sll = sincMask(vfe,x,M1).'*vam;

v = max([sll+nThres,1.1*sll]);

function varargout = selectLocalMaximaVer3(vf,va,nThres,M)

va1 = abs(va(:));
[pr,ord] = sort(va1);
ord = flipud(ord);

vf1 = vf(ord);
va1 = va1(ord);

n1 = length(va1);

if va1(1) < nThres 
  varargout = {[],[]};
  return
end

va2 = va1(1);
va2Abs = va1(1);
vf2 = vf1(1);

for n=2:n1
  if va1(n)<nThres
    continue
  end
  if va1(n) < slBoundVer2(vf1(n),va2Abs,vf2,nThres,M)
    continue
  end
  vf2(end+1) = vf1(n);
  va2(end+1) = va1(n);
  va2Abs(end+1) = abs(va(ord(n)));
end

varargout = {vf2,va2};

function out = Barycentric(varargin)

%Performs  barycentric interpolation using the method in [Bary].

if ~isstruct(varargin{1})
  st = [];
  [st.n0,st.a,st.B,st.T,st.P] = deal(varargin{:});
  
  st.N = length(st.a);
  st.alpha = baryWeights(st.T,st.B,st.P);
  st.alpha = st.alpha([1:st.P,st.P+2:end,st.P+1]);
  
  st.vt = (-st.P:st.P)*st.T; 
  st.vt = st.vt([1:st.P,st.P+2:end,st.P+1]);

  out = st;
  
else
  
  [st,v] = deal(varargin{1:2});
  
  if nargin == 3
    nd = varargin{3};
  else
    nd = 0;
  end
 
  v = v(:);
  Nv = numel(v);

  n = floor(v/st.T+0.5);
  u = v - n*st.T;
  n1 = n - st.n0;
    
  pr = [-st.P:-1,1:st.P,0];
  da = st.a(mod(n1(:,ones(1,2*st.P+1)) + pr(ones(Nv,1),:),st.N)+1);
  
  out = DerBarycentricInterp3Vec(st.alpha,da,st.vt,u,nd);
     
end

function w = baryWeights(T,B,P)

vt = (-P:P)*T;
g = APPulse(vt,1/T-B,T*(P+1));
gam = gamma(vt/T+P+1) .* gamma(-vt/T+P+1) .* g;

N = length(vt);
LD = ones(1,N);

for k = 1:N
  LD(k) = prod(vt(k)-vt(1:k-1))* prod(vt(k)-vt(k+1:N));
end

w = gam ./ LD;
w = w / max(abs(w));

function v = APPulse(t,B,TSL)

v = real(sinc(B*sqrt(t.^2-TSL^2)))/real(sinc(1i*pi*B*TSL));

function out = DerBarycentricInterp3Vec(alpha,zL,t,tauL,nd)

NtauL = size(tauL,1);
LBtau = 200; %Block size for vectorized processing. Change this to adjust the memory
             %usage.

NBlock = 1+floor(NtauL/LBtau);

t = t(:).';
Nt = length(t);

out = zeros(NtauL,nd+1);

for r = 0:NBlock-1
  ind1 = 1+r*LBtau;
  ind2 = min([(r+1)*LBtau,NtauL]);
 
  Ntau = ind2-ind1+1;
  z = zL(ind1:ind2,:);
  tau = tauL(ind1:ind2);
 
  vD = zeros(Ntau,1);
 
  LF = [1,zeros(1,Nt-1)];
  LF = LF(ones(Ntau,1),:);

  for k = 1:Nt-1
    vD = vD .* (tau-t(k))+alpha(k)*LF(:,k);
    LF(:,k+1) = LF(:,k) .* (tau-t(k));
  end

  vD = vD .* (tau-t(Nt)) + alpha(Nt) * LF(:,Nt);

  for kd = 0:nd

    pr = z(:,end); z1 = z-pr(:,ones(1,Nt)); cN = zeros(Ntau,1);

    for k = 1:Nt-1
      cN = cN .* (tau-t(k)) + z1(:,k) .* alpha(k) .* LF(:,k);
    end
    cN = cN ./ vD;
 
    ztau = z(:,end)+(tau-t(end)) .* cN;
    out(ind1:ind2,kd+1) = ztau;
 
    if kd < nd
      pr1 = ztau;
      pr1 = z(:,1:end-1) - pr1(:,ones(1,Nt-1));
     
      pr2 = t(1:end-1);
      pr2 = pr2(ones(Ntau,1),:)-tau(:,ones(1,Nt-1));
     
      z = [pr1 ./ pr2, cN] * (kd+1);
     
    end
  end
 
end

function varargout = Cost(varargin)

%Computes the ML cost function differentials by calling "Correlation" and "RegGeoSum". 

if ~isstruct(varargin{1})
  st = [];
  [st.EnergyRyy,st.icf] = deal(varargin{:});
  varargout = {st};
  return
end

st = varargin{1};

if nargin > 2
  vf = varargin{3};
else
  vf = [];
end

switch varargin{2}
  
  case 'value'
    
    vf = varargin{3};
    Rss = RegGeoSum(st.icf.M,vf,vf,0,0);
    Rsd = RegGeoSum(st.icf.M,vf,vf,0,1);
    Rdd = RegGeoSum(st.icf.M,vf,vf,1,1);
    
    pr = Correlation(st.icf,'value',vf,1);
    Rsy = pr(:,1);
    Rdy = pr(:,2);
    
    pr = Rss\[Rsy,Rsd];
    MRsy = pr(:,1);
    MRsd = pr(:,2:end);
    
    varargout = {st.EnergyRyy-RealTrace(Rsy',MRsy),-2*RealDiag(MRsy,Rdy'-MRsy'*Rsd),...
	2*RealHadamard((Rdd'-Rsd'*MRsd).',MRsy*MRsy')};

  case 'value0'
  
    if length(vf) == 0
      varargout = {st.EnergyRyy};
      return
    end

    vf = varargin{3};
    Rss = RegGeoSum(st.icf.M,vf,vf,0,0);
    Rsy = Correlation(st.icf,'value',vf);
    
    [varargout{1}] = st.EnergyRyy-RealTrace(Rsy',Rss\Rsy);
    
end

function v = RealHadamard(A,B)

v = real(A).*real(B)-imag(A).*imag(B);

function C = RealDiag(A,B)

% Same as real(diag(A*B)) but using less flops.

C = RealHadamard(A.',B);

if size(C,1) == 1
  C = C.';
else
  C = sum(C).';
end

function C = RealTrace(A,B)

% Same as real(trace(A*B)) but using less flops.

C=RealHadamard(A.',B);
C=sum(C(:));

function [listv,listLv,listCL,ErrRatio] = ...
    LookForMinimum(L,v0,nThres,freqThres,freqStopThres,varargin)

%Executes a descent algorithm using a method similar to that in [ML].

if ~isempty(varargin) 
  nItMax = varargin{1};
else
  nItMax = 20;
end

v0 = v0(:);
Nf = numel(v0);

listv = [v0,zeros(Nf,nItMax)];
listLv = [Cost(L,'value0',v0),zeros(1,nItMax)];
listCL = [NaN,zeros(1,nItMax)];

succ = 1;
nIt = 1;

while succ && nIt <= nItMax
  [succ,v,Lv,CL] = NewtonIt(L,listv(:,nIt),freqThres);
  
  if succ 
    nIt = nIt + 1;
    listv(:,nIt) = v;
    listLv(nIt) = Lv;
    listCL(nIt) = CL;
  else
    break
  end
    
  if max(abs(listv(:,nIt)-listv(:,nIt-1))) < freqStopThres
    break 
  end
end

ErrRatio = (Cost(L,'value0')-listLv(end))/listLv(end);
listv(:,nIt+1:end) = [];
listLv(nIt+1:end) = [];
listCL(nIt+1:end) = [];

function [v,vf] = validVector(vf,thres)

vf = sort(vf(:));
if length(vf)==1 
  v = 1;
  return
end

m = vf(2:end)-vf(1:end-1) < thres;

if any(m)
  v = 0;
  vf([logical(0); m]) = [];
else
  v = 1;
end

function [succ,v1,Lv1,nCutsLeft] = NewtonIt(L,v0,thres)
  
Lv1 = [];
nCutsLeft = [];

[vv,v1] = validVector(v0,thres);

if ~vv
  succ = 0;
  return
end

[Lv0,g0,h0] = Cost(L,'value',v0);

d = -h0\g0;

nCutsLeft = 20;

while nCutsLeft > 0
  [vv,v1] = validVector(v0+d,thres);
  if ~vv
    succ = 0;
    v1 = v0;
  end
  
  Lv1 = Cost(L,'value0',v0+d);
  
  if Lv1 < Lv0
    succ = 1;
    v1 = v0 + d;
    break;
  end
  
  nCutsLeft = nCutsLeft - 1;
  d = d/2;
end

if nCutsLeft == 0
  succ = 0;
end

function out = RegGeoSum(varargin)

%Computes the value of a geometric sum or its differentials using the exact formula. It
%takes care of the preventable singularity at zero.

switch nargin
  case 2
    [M,vf] = deal(varargin{:});
    nd = 0;
    msg = 'value';
    
  case 3
    [M,vf,nd] = deal(varargin{:});
    msg = 'value';
  case 5
    [M,vf1,vf2,nd1,nd2] = deal(varargin{:});
    msg = 'corr';
end

switch msg
  
  case 'value'
    out = RegGeoSum1(M,vf,nd);
    
  case 'corr'
    vf1 = vf1(:);
    Nf1 = length(vf1);
    vf2 = vf2(:);
    Nf2 = length(vf2);
    
    out = RegGeoSum1(M,-vf1(:,ones(1,Nf2))+vf2(:,ones(1,Nf1)).',nd1+nd2)*(-1)^nd1;
end

function v = RegGeoSum1(M,vf,nd)

m1 = -ceil(M/2);

ind = abs(vf)>eps^(1/3);

vf1 = vf(ind);

v = zeros(size(vf));

switch nd
  
  case 0

    v(ind) = -(exp(1i*2*pi*m1*vf1)-exp(1i*2*pi*(m1+M)*vf1))./(-1+exp(1i*2*pi*vf1));
    
    v(~ind) = M;
  
  case 1
    
    v(ind) = (-2*1i*pi*(  exp(1i*2*pi*vf1*(1 + m1))*(-1 + m1) - exp(1i*2*pi*vf1*m1)*m1  ...
	-exp(1i*pi*2*vf1*(1 + M + m1))*(-1 + M + m1) + ...
       exp(1i*2*pi*vf1*(M + m1))*(M + m1)))./(-1 + exp(1i*2*pi*vf1)).^2;
   
    v(~ind) = pi*1i*M*(-1+M+2*m1);
    
  case 2
    
    v(ind) = (4*(exp(1i*2*pi*vf1*(2 + m1))*(-1 + m1)^2 + exp(1i*2*pi*vf1*m1)*m1^2 - ...
       exp(1i*2*pi*vf1*(2 + M + m1))*(-1 + M + m1)^2 - ...
       exp(1i*2*pi*vf1*(M + m1))*(M + m1)^2 + ...
       exp(1i*2*pi*vf1*(1 + m1))*(1 - 2*(-1 + m1)*m1) + ... 
       exp(1i*2*pi*vf1*(1 + M + m1))* ...
        (-1 + 2*M^2 + 2*(-1 + m1)*m1 + M*(-2 + 4*m1)))*pi^2)./ ...
       (-1 + exp(1i*2*pi*vf1)).^3;
   
    v(~ind) = -(2*pi^2/3)*M*(1+6*m1^2+6*m1*(-1+M)-3*M+2*M^2);
    
end

function st = Report(p,vfEst)

st = [];
st.Freqs = vfEst;
st.Amps = Correlation(p.cfd.icf,'ampEstimates',vfEst);

function FreqsDevBounds = CRBBound(vf,va,M,nVar)

%Computes the Cramer-Rao bound of the frequencies.

va = va(:);
vf = vf(:);

Raa = RegGeoSum(M,vf,vf,0,0);
Rad = RegGeoSum(M,vf,vf,0,1);
Rdd = RegGeoSum(M,vf,vf,1,1);

FreqsDevBounds = sqrt((nVar/2)*diag(inv(real((conj(va)*va.') .* (Rdd-Rad'*(Raa\Rad))))));

function st = MLEstimate(y,nVar,PFA)

p = Preprocessing(y,nVar,PFA);

st = MethodSerial1(p);

st.CRBBoundsOnFreqDevs = CRBBound(st.Freqs,st.Amps,length(y),nVar);