Using Goertzel to find phase shift

Question

I have two signals with nearly the same frequency, and i would like to find the phase shift between them, since i am using micro-controller, i would like to do it in the less expensive way.

I know how to calculate Goertzel for a specific frequency bin (for each signal), but how do i calculate the angle for this bin by using Goertzel?

If you know a lower computational method, please advice.

Thanks

Answer 1

If the complex frequency bin outputs of the two Goertzel filters are $a$ and $b$, you can calculate the phases as $\alpha = \text{atan2}\left(\text{Im}(a),\ \text{Re}(a)\right)$ and $\beta = \text{atan2}\left(\text{Im}(b),\ \text{Re}(b)\right)$, where $\text{Re}$ returns the real component and $\text{Im}$ returns the imaginary component of the argument. Most programming languages will have the two-argument $\text{atan2}$ function. The phase difference can be calculated as $\alpha - \beta = \text{atan2}\left(\text{Im}\left(a\ \text{conj}(b)\right),\ \text{Re}\left(a\ \text{conj}(b)\right)\right)$, where $\text{conj}$ denotes taking the complex conjugate, saving you one $\text{atan2}$ evaluation and wrapping of the phase, at the cost of one complex multiplication.

There's one more trick available to calculate $a\ \text{conj}(b)$. Multiply the Goertzel output $a$ by the second input signal and lowpass filter the result. In frequency domain it looks like this:

The lowpass filtering isolates the circled spectral peak that corresponds to $a\ \text{conj}(b)$. This saves you one Goertzel filter at the cost of a lowpass filter. If you use this trick you must also take into account the phase shift due to the lowpass filter.

For fast $\text{atan2}$ see Methods of computing fixed point atan2 on FPGA.

Answer 2

Following is just a lazy copy-paste from my own code in case that you're stuck at the implementation for real/imag extraction of the goertzel filter. Otherwise, go look at Olli's answer.

template<typename Scalar, class Vector>
static std::complex<Scalar> goertzel(const Vector & data, std::size_t size, Scalar omega)
{
    Scalar sine, cosine, coeff, q0(0), q1(0), q2(0);

    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;

    for (Types::fint_t t = 0; t < size; t++)
    {
        q0 = coeff * q1 - q2 + data[t];
        q2 = q1;
        q1 = q0;
    }

    Scalar real = (q1 - q2 * cosine);
    Scalar imag = (q2 * sine);

    return std::complex<Scalar>(real, imag);

}

where...

    angle = std::arg(complex_number); // which is equivalent to atan2(imag, real);
    magnitude = std::abs(complex_number); // again, same as sqrt(imag^2 + real^2);