thdf.m

function [thd, p0, l0] = thdf( P1, f, varargin )
%thdf Computes the total harmonic distortion to the fundamental.
%   Fundamental is the first harmonic peak that's larger than 2% than 
%   the largest harmonic in the supplied frequency spectrum.
%
%   thd = thdf( P1, f )
%
%   P1      Single-sided amplitude spectrum of the signal.
%   f       Frequency span of P1.
%   thd     Total harmonic distortion.
%
%   Options:
%   'NFundamentals' [1]   Amount of fundamentals assumed in computation.
%   'FThres'        [Inf] Upper limit for accounted frequencies. 

p = inputParser;

p.addOptional('NFundamentals', 1, @isnumeric);
p.addOptional('FThres', Inf, @isnumeric);
p.parse(varargin{:});

% Amount of fundamental harmonics
nf = p.Results.NFundamentals;
% Frequency threshold
ft = p.Results.FThres;
% Fundamental amplitude threshold: >2% than the largest harmonic in P1
athres = 0.02*max(P1);
% Find all harmonics above threshold
[p, l] = findpeaks(P1, f);
% Filter out irrelevant harmonics
p0 = p(p > athres & l < ft);
l0 = l(p > athres & l < ft);
% Fundamentals 
p1 = p0(1:nf);
% Compute distortion
thd = sqrt(sum(p0((nf+1):end).^2))/sqrt(sum(p1.^2));

end