diff --git a/src/shared/algorithms/FFT.h b/src/shared/algorithms/FFT.h index 73fada1d85307eee83e8e5b61e738dcb84b780cc..24e49593cee634f486b065f67ce92b56ff83375f 100644 --- a/src/shared/algorithms/FFT.h +++ b/src/shared/algorithms/FFT.h @@ -25,7 +25,7 @@ #include <complex.h> #include <math.h> -#include <Eigen/Core> +#include <Eigen/Dense> namespace Boardcore { @@ -60,15 +60,15 @@ public: VectorCF phasors = VectorCF::Zero(); for (int i = 0; i < N_size; i++) { - rev_i = reverse_bits(i); + rev_i = reverse_bits(i); phasors(rev_i) = std::complex<float>(input_signal(i), 0); } // Cooley-Tukey FFT algorithm for (int s = 1; s <= n_bits(N_size); s++) { - m = powl(2, s); - phi = -2 * EIGEN_PI / m; + m = powl(2, s); + phi = -2 * EIGEN_PI / m; omega_m = std::complex<float>(cos(phi), sin(phi)); for (int k = 0; k < N_size; k += m) { @@ -77,8 +77,10 @@ public: { t = omega * phasors(k + j + m / 2); u = phasors(k + j); - phasors(k + j) = u + t; + + phasors(k + j) = u + t; phasors(k + j + m / 2) = u - t; + omega *= omega_m; } } @@ -107,6 +109,40 @@ public: return bins; } + /** + * @brief Perform the inverse FFT on the input signal + * + * @param fft_vector + * @param sample_rate in Hertz + * @return Eigen::Vector<std::complex<float>, N_size> + */ + static const VectorF ifft(VectorCF fft_vector, float sample_rate) + { + float amplitude, alpha, phi; + VectorF ifft_vector = VectorF::Zero(); + VectorF bins = fftfreq(sample_rate); + + for (int i = 0; i < N_size; i++) + { + for (int j = 0; j < N_size; j++) + { + // A = |f| + amplitude = std::abs(fft_vector(j)); + // α = 2πf * t + alpha = 2 * EIGEN_PI * bins(j) * (float)i / sample_rate; + // ϕ = arg(f) + phi = std::arg(fft_vector(j)); + + // i = A * cos(α + ϕ) + ifft_vector(i) += amplitude * cos(alpha + phi); + } + // scale down + ifft_vector(i) /= N_size; + } + + return ifft_vector; + } + private: /** * @brief Get the number of bits to differentiate x elements @@ -133,6 +169,8 @@ private: } }; +typedef FFT<8> FFT8; +typedef FFT<16> FFT16; typedef FFT<32> FFT32; typedef FFT<64> FFT64; typedef FFT<128> FFT128; diff --git a/src/tests/algorithms/FFT/test-fft.cpp b/src/tests/algorithms/FFT/test-fft.cpp index 01773e8e2a38fe51526b1387105fa19baf50ed89..9158d53146fcdb2c751e56bd29764e7e862e27c8 100644 --- a/src/tests/algorithms/FFT/test-fft.cpp +++ b/src/tests/algorithms/FFT/test-fft.cpp @@ -31,21 +31,24 @@ using namespace Boardcore; using namespace Eigen; +using VectorF = Eigen::Vector<float, SAMPLES>; +using VectorCF = Eigen::Vector<std::complex<float>, SAMPLES>; + int main() { bool failed = false; - Eigen::Vector<float, SAMPLES> signal_vector = - Eigen::Vector<float, SAMPLES>::Zero(); + VectorF signal_vector = VectorF::Zero(); for (int i = 0; i < SAMPLES; i++) { signal_vector(i) = SIGNAL[i]; } - Vector<std::complex<float>, SAMPLES> fft_result = - FFT<SAMPLES>::fft(signal_vector); - Vector<float, SAMPLES> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); + VectorCF fft_result = FFT<SAMPLES>::fft(signal_vector); + VectorF fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE); + VectorF ifft_result = FFT<SAMPLES>::ifft(fft_result, SAMPLE_RATE); + // test for fft for (int i = 0; i < fft_result.size(); i++) { if (std::abs(1.0 / SAMPLES * std::abs(fft_result(i)) - INTENSITY[i]) > @@ -57,6 +60,7 @@ int main() } } + // test for fftfreq for (int i = 0; i < fft_freq.size(); i++) { if (fft_freq(i) != FREQUENCY[i]) @@ -67,6 +71,17 @@ int main() } } + // test for ifft + for (int i = 0; i < ifft_result.size(); i++) + { + if (std::abs(ifft_result(i) - SIGNAL[i]) > 0.001) + { + printf("IFFT result differs from the correct one [%d]: %f != %f\n", + i, ifft_result(i), SIGNAL[i]); + failed = true; + } + } + failed ? printf("FAILED\n") : printf("PASSED\n"); return 0;