/* phd.c Pisarenko Harmonic Decomposition */ /* This routine estimates the frequency of a single sinusoid in white noise using the P.H.D. Input: array of floats containing the signal, and its length Returned: frequency estimate in digital units Digital units of frequency are in the range 0 < f < 1/2 to convert to physical frequency in Hz multiply by the sampling rate. The accuracy of the PHD improves with buffer size and SNR. To use in real-time collect the incoming samples in a buffer of sufficient length (dependent on the amount of noise averaging needed). To increase time resolution you can overlap these buffers. For further details see DSP-CSP Section 13.5 */ /* Jonathan (Y) Stein */ #include float TwoPi=6.28318530718; float Pisarenko(float x[], int N) { int i ; float r1, r2, a, g, w ; /* first, the needed autocorrelations */ for (r1=0.0,i=0;i=0.0) g = a + sqrt(a*a + 2) ; else g = a - sqrt(a*a + 2) ; /* convert from angular and return */ return(acos(g/2.0) / TwoPi) ; } #ifdef TEST /* to compile for test cc -DTEST phd.c -o phdtest.exe */ void main( void ) { /* test PHD on artificial signal of various frequencies */ #include #include float *x ; float f, fp ; int t, N=128 ; /* allocate array of length N */ x = (float *) malloc(N * sizeof(float)) ; /* titles for printout */ printf ( "\n\n f f_phd err\n") ; /* loop on frequencies */ for (f=0.05;f<=0.5;f+=0.05) { /* create noisy signal */ for (t=0;t