function c = robnest(sig, fondfreq, corder, nd);

%robnest        Robust noise psd estimation for a mixed harmonic + noise model.
%               c = robnest(sig, fondfreq, corder[, nd]);
%               sig             Signal frame
%               fondfreq        Fundamental frequency (radian)
%               corder          Order for the cepstrum of the psd
%               nd              Number of frequency bins discarded left and
%                               right of each harmonic
%
%Frequency based psd estimation using the method of Pawitan and O'Sullivan,
%(Journal of the American Statistical Association, 89(426), 1994) omitting the
%frequency bins located near the harmonic frequencies.
%
%_______________________________________________________________________
%For more information about this mfile please take a look at the paper
%"Estimation of the spectral envelope of voiced sounds using a penalized
%likelihood approach" by Marine Oudot, Olivier Cappé and Eric Moulines.

% Author: Olivier Cappé, June 2000
% ENST Dpt. TSI / LTCI (CNRS URA 820), Paris

% Euler constant (use -digamma(1) otherwise)
ec = 0.577215664901532;

% Number of points discarded left and right of each harmonic
if (nargin < 4)
  nd = 1;
end
% Data taper
T = length(sig);
w = hanning(T);
sw2 = sum(w.^2);

% Compute the windowed periodogram
I = abs(fft(sig(:).*w)).^2/sw2;

% Locate harmonics
nbharm = floor(pi/fondfreq);
pos = 1 + round(fondfreq*(1:nbharm)/(2*pi)*T);
% Discard some points around
POS = pos;
for i = 1:nd
  POS = [pos-i; POS; pos+i];
end;
pos = reshape(POS, 1, (2*nd+1)*length(pos));

% Valid frequency indexes
ind = [0; ones(floor(T/2)-1,1); zeros(T-floor(T/2),1)];
ind(pos) = zeros(1, length(pos));
ind = find(ind);
li = length(ind);
fprintf('robnest: Using %d data points to estimate the psd\n', li);

% Cepstral regression matrix
R = [ones(li,1), 2*cos((2*pi*(ind-1)/T)*(1:corder))];

% Start with penalized log-regression estimate
% Smoothing param
lambda = length(ind)*0.005;
B = diag(2*((0:corder).^2));
% Adding -digamma(1) removes the bias of the log periodogram
c = (R'*R + lambda*B)\(R'*(log(I(ind)+realmin)+ec));
%plot(2*pi/T*(ind-1), log(I(ind)+realmin)*10/log(10), '*');
%hold on;
%plot(2*pi/T*(ind-1), R*c*10/log(10), 'r');

% Refine with Newton steps
NIT = 3;
for i = 1:NIT
  grad = lambda*2*B*c + sum((1-(I(ind)./exp(R*c))*ones(1,corder+1)).*R)';
  Hess = lambda*2*B + R'*((I(ind)./exp(R*c)*ones(1,corder+1)).*R);
  c = c - Hess\grad;
  %plot(2*pi/T*(ind-1), R*c*10/log(10));
end
%plot(2*pi/T*(ind-1), R*c*10/log(10), 'g');
