## ML Estimation of Several Frequencies Using the Nonuniform FFT

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

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

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(:);

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};
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);

MRsy = pr(:,1);
MRsd = pr(:,2:end);

varargout = {st.EnergyRyy-RealTrace(Rsy',MRsy),-2*RealDiag(MRsy,Rdy'-MRsy'*Rsd),...

case 'value0'

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

vf = varargin{3};
Rsy = Correlation(st.icf,'value',vf);

end

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

function C = RealDiag(A,B)

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

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=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);