Code

ML frequency estimation using the FFT

Jesus Selva April 18, 2011 Coded in Matlab

This snippet computes the maximum likelihood (ML) estimate of the frequency, amplitude, and phase of an undamped exponential, i.e, it locates the periodogram's peak accurately. The snippet contains the code for the non-uniform FFT in

http://www.dsprelated.com/showcode/159.php

The peak is located using a Newton method, and usually 2 or 3 iterations are sufficient. The total computational cost is just that of the FFT, O(N log N). The algorithm is explained in

http://arxiv.org/pdf/1104.3069v1

Simply place the code that follows in a file named "Demo.m" and execute it. Note that you may change any of the parameters from the command line. For example,

Demo({'M',512,'gdB',22}) runs the code with 512 samples and SNR equal to 22 dB.

function Demo(L)

%Author: J. Selva. 
%Date: April 2011.

%This demo constructs trials of an undamped exponential plus noise, and then computes the
%maximum likelihood estimate of its frequency, amplitude, and phase. Only one FFT of size
%2^ceil(log2(st.M)+1) is computed, plus a few operations. The complexity is that given by
%the FFT. The algoritm is explained in 
%
%         http://arxiv.org/abs/1104.3069
%

format long g
format compact

M = 1024; %Number of samples.
gdB = 20; %SNR at matched filter output.
ARef = 1.3; %Amplitude.
fRef = 0.372; %Frequency in [-0.5,0.5[.
PhiRef = 0.43; %Phase (rad).

if nargin > 0
  for k = 1:2:length(L)
    eval([L{k} '=L{k+1};']);
  end
end

%Define a random vector with 1024 elements. 

SNRdB =  gdB - 10*log10(M); %Signal-to-noise ratio (dB).

%Complex sinusoidal amplitude, frequency, and phase.

NVar = ARef^2*10.^(-SNRdB/10); %Noise variance. 

m1 = -ceil(M/2);

while 1
  
  disp(' ')
  disp(['M = ', num2str(M), ', gdB = ', num2str(gdB), ' dB, ARef = ', num2str(ARef), ...
	', fRef = ', num2str(fRef), ', PhiRef = ', num2str(PhiRef), ' rad'])
  disp(' ')
  
  %Set up a trial.
  a = ARef*exp(1i*2*pi*fRef*(m1:m1+M-1).'+1i*PhiRef)+(randn(M,1)+1i*randn(M,1))*sqrt(NVar/2);

  %Call the nufft function. The output is a struct with some variables for later use.

  st = nufft(a);

  %Compute the DFT peak. 
  [f1,A1,Phi1,L1,Deg,nIt] = MLEstimate(st);

  %The output is the following, 
  %
  %   f1, A1, and Phi1 are the ML estimates of frequency, amplitude and phase, respectively. 
  %
  %   L1 is the ML cost function value at f1 and its first two  derivatives. L1(1) should be
  %     large   since it is   the square value of   the peak amplitude.    L1(2) is the cost
  %     function   derivative,  and  it  should  be   small  since  we  are  at   a critical
  %     point. Finally  L1(2) should be a  negative number with large  amplitude since f1 is
  %     the abcissa of a maximum (second derivative).
  %
  %   Deg: If Deg==1 then the Newton iteration was cut too many times. 
  %   nIt: Number of Newton iterations. 

  disp('Exact and estimated frequency, amplitude, and phase:')
  disp([fRef,ARef,PhiRef;f1,A1,Phi1])
  disp(['Number of iterations: ' num2str(nIt)])
  %
  %Plot the spectrum close to the ML estimate
  %
  f = fRef+(-10:.05:10)/st.K;
  sp = nufft(st,f);
  plot(f,20*log10(abs(sp)/M))

  hold on
  stem(f1,20*log10(A1),'r')
  hold off

  grid on
  set(gca,'YLim',[-40,10])
  xlabel('f')
  ylabel('Amplitude (dB)')
  text(fRef+2/st.K,5,{['Freq. ' num2str(f1)],['Ampl. ' num2str(A1)],...
      ['Phase. (rad) ' num2str(Phi1)]})
  %

  R = input('Press Enter for a new trial, q to quit: ','s');

  if any(strcmp(R,{'q','Q'}))
    break
  end

end

%------------------------------------------------------
%------------------------------------------------------

function [f1,A1,Phi1,L1,Deg,nIt] = MLEstimate(st)

%This function finds out the peak of a DFT. Its input is the struct produced by the nufft function. 

%Find the spectral peak in the frequency grid.
[pr11,ind] = max(abs(st.aF).^2);

%Compute its frequency.
f0 = (ind-1)/st.K;
f0 = f0+ceil(-0.5-f0);

%L0 contains the differentials of the ML cost function of orders 0,1, 2 ...
%at f0. 
L0 = LDiff(nufft(st,f0,2));

nItMax = 20;
err = 10.^-12;
nIt = 0;

%Call Iter (Newton iteration) until there is no change or the solution is degenerate.
while 1
  [Deg,pr11,f1,L1] = Iter(st,f0,L0);
  
  if Deg || abs(f1-f0) <= err || nIt >= nItMax
    break
  end
  
  f0 = f1;
  L0 = L1;
  nIt = nIt+1;
end

pr = nufft(st,f1);
A1 = abs(pr)/st.M;
Phi1 = angle(pr); 

function L = LDiff(c)

%Computes the differentials of the ML cost function from the nufft differentials.

L = [real(c(1)*conj(c(1))); 2*real(conj(c(1))*c(2)); ...
  2*real(conj(c(2))*c(2)+conj(c(1))*c(3))];

function [Deg,nCut,f1,L1] = Iter(st,f0,L0)

%Performs the Newton iteration, cutting the Newton direction if necessary.

f1 = f0-L0(2)/L0(3); %Newton iteration.
f1 = f1+ceil(-0.5-f1); %Make it fall in [-0.5,0.5].
L1 = LDiff(nufft(st,f1,2)); %Compute the differentials at f1.

nCutMax = 20; %The difference f1-f0 will be reduced in the sequel if there is no ...
              %improvement in the cost function
nCut = 0;
d = 1;

%In this loop the difference f1-f0 is halved a maximum of nCutMax times until there is ...
%an increase in the cost function

while L1(1) < L0(1) && nCut < nCutMax
  d = d/2;
  f1 = f0-d*L0(2)/L0(3);
  f1 = f1+ceil(-0.5-f1);
  L1 = LDiff(nufft(st,f1,2)); 
  nCut = nCut+1;
end

Deg = nCut >= nCutMax;

%----------------------------------------------------------------
%----------------------------------------------------------------

%Non-uniform FFT function. This code is discussed in 
%   http://www.dsprelated.com/showcode/159.php

function varargout = nufft(varargin)

%Author: J. Selva
%Date: April 2011.
%
%nufft computes the FFT at arbitrary frequencies using barycentric interpolation.
%
%For the interpolation method, see
%
%J. Selva, 'Design of barycentric interpolator for uniform and nonuniform sampling
%  grids', IEEE Trans. on Signal Processing, vol. 58, n. 3, pp. 1618-1627, March 2010.
%
%Usage:
%
% -First call nufft with the vector of samples as argument,
%
%       st = nufft(a); %a is the vector of samples.
%
%    The output is an struct. The field st.aF is the DFT of vector a, sampled with spacing
%    1/st.K. 
%
%    The DFT is defined using
%
%        A_k = \sum_{n=m_1}^{m_1+M-1} a_n e^{-j 2 \pi n f}
%
%    where m_1=-ceil(M/2) and M the number of samples. 
%
% -Then call nufft with sintax 
%
%        out = nufft(st,f,nd,method)
%
%   where
%
%     f: Vector of frequencies where the DFT is evaluated. Its elements must follow
%        abs(f(k))<=0.5
%
%     nd: Derivative order. nufft computes derivatives up to order nd. At the output 
%         out(p,q) is the (q-1)-order derivative of the DFT at frequency f(p). The first
%         column of out contains the zero-order derivatives, i.e, the values of the DFT at
%         frequencies in vector f. 
%     method: If omitted, it is 'baryVec'. Four methods have been implemented:
%
%         +'baryLoop': The output is interpolated using the barycentric method and a loop
%           implementation.
%
%         +'baryVec': The output is interpolated using the barycentric method and a
%          vectorized implementation.
%
%         +'directLoop': The output is computed using the DFT sum directly and a loop
%           implementation.
%  
%         +'directVec': The output is computed using the DFT sum directly and a vectorized
%           implementation.

 
if ~isstruct(varargin{1})
  st = [];
  st.a = varargin{1};
  st.a = st.a(:);
  
  st.M = length(st.a);
  st.m1 = -ceil(st.M/2);
  st.K =  2^ceil(log2(st.M)+1);
  st.aF = fft([st.a(-st.m1+1:st.M); zeros(st.K-st.M,1); st.a(1:-st.m1)]);
  
  errb = 10.^-13; %This is the interpolation accuracy. You can set any larger value, and ...
      %this would reduce st.P. The number of interpolation samples is 2*st.P+1.
  
  st.T = 1/st.K;
  st.B = -2*st.m1;
  st.P = ceil(acsch(errb)/(pi*(1-st.B*st.T)));
  st.vt = MidElementToEnd((-st.P:st.P)*st.T);
  
  st.alpha =  MidElementToEnd(baryWeights(st.T,st.B,st.P));
  
  varargout = {st};
  
  return
end

[st,f] = deal(varargin{1:2});
  
nd = 0;
   
if nargin > 2
  nd = varargin{3};
end
  
method = 'baryVec';
  
if nargin > 3
  method = varargin{4};
end
  
Nf = length(f);
out = zeros(Nf,nd+1);

switch method
    
  case 'baryLoop' %Interpolated method using loops

    for k = 1:length(f)

      x = f(k);
      
      n = floor(x/st.T+0.5);
      u = x - n * st.T;

      da = MidElementToEnd(st.aF(1+mod(n-st.P:n+st.P,st.K)).');

      out(k,:) = DerBarycentricInterp3(st.alpha,da,st.vt,u,nd);

    end

  case 'baryVec' %Vectorized interpolated method
    
    f = f(:);
    Nf = length(f);
    
    n = floor(f/st.T+0.5);
    u = f - n * st.T;
    
    pr = [-st.P:-1 , 1:st.P , 0];
    
    ms = st.aF(1+mod(n(:,ones(1,2*st.P+1)) + pr(ones(Nf,1),:),st.K));
    
    if length(f) == 1
      ms = ms.';
    end
    
    out = DerBarycentricInterp3Vec(st.alpha,ms,st.vt,u,nd);
    
  case 'directLoop' % Direct method using loops
      
    for k = 1:length(f)
      out(k,1) = 0;
	
      for r = st.m1:st.m1+st.M-1
	out(k,1) = out(k,1)+exp(-1i*2*pi*r*f(k))*st.a(r-st.m1+1);
      end
	
      for kd = 1:nd
	out(k,kd+1) = 0;
	
	for r = st.m1:st.m1+st.M-1
	  out(k,kd+1) = out(k,kd+1) + ...
	      ((-1i*2*pi*r).^kd .* exp(-1i*2*pi*r*f(k)))*st.a(r-st.m1+1);
	end
      
      end
    end  
    
  case 'directVec' %Vectorized direct method
	
    for k = 1:length(f)
      out(k,1) = exp(-1i*2*pi*(st.m1:st.m1+st.M-1)*f(k))*st.a;
      
      for kd = 1:nd
	out(k,kd+1) = ...
	    ((-1i*2*pi*(st.m1:st.m1+st.M-1)).^kd .* ...
	    exp(-1i*2*pi*(st.m1:st.m1+st.M-1)*f(k)))*st.a;
      end
      
    end
   
end
 
varargout = {out};

function v = MidElementToEnd(v)

ind = ceil(length(v)/2);
v = [v(1:ind-1),v(ind+1:end),v(ind)];

function v = APPulse(t,B,TSL)

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

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 out = DerBarycentricInterp3(alpha,s,t,tau,nd)

vD = 0;
Nt = length(t);
LF = [1,zeros(1,Nt-1)];
out = zeros(1,nd+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);

z = s;

for kd = 0:nd

  z1 = z-z(end); cN = 0;

  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(kd+1) = ztau;
  
  if kd < nd
    z = [ (z(1:end-1)-ztau)./(t(1:end-1)-tau) , cN ] * (kd+1);
  end
  
end

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

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