HPS algorithm for tone detection

HPS.m

function [ freq ] = HPS(audiofile)
    CORRECTFACTOR=1;
    
    [sample, fs] = audioread(audiofile);
    fftn = fs;
    sample = sample .* hann(length(sample));
    
    fftr = abs(fft(sample,fftn));
    fftr = fftr(1:fftn/2);
    
    d2 = downsample(fftr, 2);
    d3 = downsample(fftr, 3);
    d4 = downsample(fftr, 4);
    d5 = downsample(fftr, 5);
    
    fftrsize = fftn/2;
    d2 = padarray(d2, fftrsize-length(d2), 1, 'post');
    d3 = padarray(d3, fftrsize-length(d3), 1, 'post');
    d4 = padarray(d4, fftrsize-length(d4), 1, 'post');
    d5 = padarray(d5, fftrsize-length(d5), 1, 'post');
    
    result = fftr .* d2 .* d3 .* d4 .* d5;
    
    plot(result);
    
    [~, I] = max(result);
    
    freq = I * (fs / fftn) * CORRECTFACTOR;
end