This is from your test code. If I increase the 0.1 to higher values in the test code, the algorithm starts to produce incorrect estimates: freq = 0.1 + 0.3*rand()