diff --git a/doc/analysis/GoertzelAlgorithm.md b/doc/analysis/GoertzelAlgorithm.md index 0aed745c..a90d5e13 100644 --- a/doc/analysis/GoertzelAlgorithm.md +++ b/doc/analysis/GoertzelAlgorithm.md @@ -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. diff --git a/numerical/analysis/GoertzelAlgorithm.hpp b/numerical/analysis/GoertzelAlgorithm.hpp index cea30756..1b1dbf5f 100644 --- a/numerical/analysis/GoertzelAlgorithm.hpp +++ b/numerical/analysis/GoertzelAlgorithm.hpp @@ -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 binPhasor, std::size_t blockLength); [[nodiscard]] static std::optional TryCreate(std::size_t k, std::size_t blockLength); @@ -31,6 +32,8 @@ namespace analysis static T Coefficient(std::size_t k, std::size_t blockLength); private: + static math::Complex BinPhasor(std::size_t k, std::size_t blockLength); + T coeff; T cosine; T sine; @@ -51,9 +54,14 @@ namespace analysis template GoertzelAlgorithm::GoertzelAlgorithm(std::size_t k, std::size_t blockLength) - : coeff{ T{ 2 } * math::Cos(T{ 2 } * std::numbers::pi_v * static_cast(k) / static_cast(blockLength)) } - , cosine{ math::Cos(T{ 2 } * std::numbers::pi_v * static_cast(k) / static_cast(blockLength)) } - , sine{ math::Sin(T{ 2 } * std::numbers::pi_v * static_cast(k) / static_cast(blockLength)) } + : GoertzelAlgorithm(BinPhasor(k, blockLength), blockLength) + {} + + template + GoertzelAlgorithm::GoertzelAlgorithm(math::Complex binPhasor, std::size_t blockLength) + : coeff{ T{ 2 } * binPhasor.Real() } + , cosine{ binPhasor.Real() } + , sine{ binPhasor.Imaginary() } , blockSize{ blockLength } { really_assert(blockLength > 0); @@ -106,6 +114,13 @@ namespace analysis sampleCount = 0; } + template + math::Complex GoertzelAlgorithm::BinPhasor(std::size_t k, std::size_t blockLength) + { + const T angle{ T{ 2 } * std::numbers::pi_v * static_cast(k) / static_cast(blockLength) }; + return math::Complex{ math::Cos(angle), math::Sin(angle) }; + } + template T GoertzelAlgorithm::Coefficient(std::size_t k, std::size_t blockLength) { diff --git a/numerical/analysis/test/TestGoertzelAlgorithm.cpp b/numerical/analysis/test/TestGoertzelAlgorithm.cpp index a63c7d4c..3f56aa50 100644 --- a/numerical/analysis/test/TestGoertzelAlgorithm.cpp +++ b/numerical/analysis/test/TestGoertzelAlgorithm.cpp @@ -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 * static_cast(k) / static_cast(N) }; + analysis::GoertzelAlgorithm goertzel{ math::Complex{ std::cos(binAngle), std::sin(binAngle) }, N }; + + std::array signal{}; + for (std::size_t n = 0; n < N; ++n) + signal[n] = std::sin(binAngle * static_cast(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(n)); + refImag -= signal[n] * std::sin(binAngle * static_cast(n)); + } + + math::Complex 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 };