Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions doc/analysis/GoertzelAlgorithm.md
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,8 @@ The DC bin ($k = 0$) and the Nyquist bin ($k = N/2$) are purely real; their imag

The magnitude-squared form $s_1^2 + s_2^2 - c\,s_1 s_2$ avoids the square root when only power detection (threshold comparison) is needed, saving a transcendental call on platforms without hardware `sqrt`.

The recurrence and the final step need only $\cos\omega$ and $\sin\omega$ of the bin, $\omega = 2\pi k/N$, evaluated once per configuration. A caller that already evaluates them by other means — a sine table refined by a short series, or an exact value for a known angle — can supply the unit bin phasor $e^{j\omega}$ directly. On targets whose sine and cosine come from a software math library, that keeps the library's general argument-reduction code, several kilobytes on a Cortex-M4, out of the image. The phasor's accuracy sets the resonator frequency: an error $\varepsilon$ in $\cos\omega$ detunes it by about $\varepsilon / \sin\omega$ radians per sample, which over a block of $N$ samples rotates the result by about $N\varepsilon / (2\sin\omega)$.

When several tones must be tracked simultaneously, one independent Goertzel instance per tone runs in $O(BN)$ total with $O(B)$ state — still far cheaper than an FFT for small $B$.

A sliding Goertzel variant updates one sample at a time by subtracting the oldest sample, giving a continuous output at the cost of maintaining an input ring buffer; this trades $O(1)$ memory for $O(N)$ memory and is outside the scope of the no-heap constraint here.
Expand Down
21 changes: 18 additions & 3 deletions numerical/analysis/GoertzelAlgorithm.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ namespace analysis
public:
GoertzelAlgorithm(std::size_t k, std::size_t blockLength);
GoertzelAlgorithm(T targetHz, T sampleHz, std::size_t blockLength);
GoertzelAlgorithm(math::Complex<T> binPhasor, std::size_t blockLength);

[[nodiscard]] static std::optional<GoertzelAlgorithm> TryCreate(std::size_t k, std::size_t blockLength);

Expand All @@ -31,6 +32,8 @@ namespace analysis
static T Coefficient(std::size_t k, std::size_t blockLength);

private:
static math::Complex<T> BinPhasor(std::size_t k, std::size_t blockLength);

T coeff;
T cosine;
T sine;
Expand All @@ -51,9 +54,14 @@ namespace analysis

template<typename T>
GoertzelAlgorithm<T>::GoertzelAlgorithm(std::size_t k, std::size_t blockLength)
: coeff{ T{ 2 } * math::Cos(T{ 2 } * std::numbers::pi_v<T> * static_cast<T>(k) / static_cast<T>(blockLength)) }
, cosine{ math::Cos(T{ 2 } * std::numbers::pi_v<T> * static_cast<T>(k) / static_cast<T>(blockLength)) }
, sine{ math::Sin(T{ 2 } * std::numbers::pi_v<T> * static_cast<T>(k) / static_cast<T>(blockLength)) }
: GoertzelAlgorithm(BinPhasor(k, blockLength), blockLength)
{}

template<typename T>
GoertzelAlgorithm<T>::GoertzelAlgorithm(math::Complex<T> binPhasor, std::size_t blockLength)
: coeff{ T{ 2 } * binPhasor.Real() }
, cosine{ binPhasor.Real() }
, sine{ binPhasor.Imaginary() }
, blockSize{ blockLength }
{
really_assert(blockLength > 0);
Expand Down Expand Up @@ -106,6 +114,13 @@ namespace analysis
sampleCount = 0;
}

template<typename T>
math::Complex<T> GoertzelAlgorithm<T>::BinPhasor(std::size_t k, std::size_t blockLength)
{
const T angle{ T{ 2 } * std::numbers::pi_v<T> * static_cast<T>(k) / static_cast<T>(blockLength) };
return math::Complex<T>{ math::Cos(angle), math::Sin(angle) };
}

template<typename T>
T GoertzelAlgorithm<T>::Coefficient(std::size_t k, std::size_t blockLength)
{
Expand Down
26 changes: 26 additions & 0 deletions numerical/analysis/test/TestGoertzelAlgorithm.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,32 @@ TEST_F(TestGoertzelAlgorithm, result_matches_direct_dft_bin)
EXPECT_NEAR(result.Imaginary(), refImag, 1e-3f);
}

TEST_F(TestGoertzelAlgorithm, supplied_bin_phasor_matches_direct_dft_bin)
{
constexpr std::size_t k{ 3 };
const float binAngle{ 2.0f * std::numbers::pi_v<float> * static_cast<float>(k) / static_cast<float>(N) };
analysis::GoertzelAlgorithm<float> goertzel{ math::Complex<float>{ std::cos(binAngle), std::sin(binAngle) }, N };

std::array<float, N> signal{};
for (std::size_t n = 0; n < N; ++n)
signal[n] = std::sin(binAngle * static_cast<float>(n)) + 0.5f;

for (float s : signal)
goertzel.Push(s);

float refReal{ 0.0f };
float refImag{ 0.0f };
for (std::size_t n = 0; n < N; ++n)
{
refReal += signal[n] * std::cos(binAngle * static_cast<float>(n));
refImag -= signal[n] * std::sin(binAngle * static_cast<float>(n));
}

math::Complex<float> result{ goertzel.Result() };
EXPECT_NEAR(result.Real(), refReal, 1e-3f);
EXPECT_NEAR(result.Imaginary(), refImag, 1e-3f);
}

TEST_F(TestGoertzelAlgorithm, magnitude_squared_equals_result_norm_squared)
{
constexpr std::size_t k{ 2 };
Expand Down
Loading