From c495f8223511a55824a84d41c219121a50725fd4 Mon Sep 17 00:00:00 2001 From: f4exb Date: Thu, 17 May 2018 00:09:56 +0200 Subject: [PATCH] Imported Iowa Hills Software IIR and FIR calculator --- CMakeLists.txt | 1 + kitiirfir/CMakeLists.txt | 38 ++ kitiirfir/FFTCode.cpp | 770 +++++++++++++++++++++++++++++++ kitiirfir/FFTCode.h | 60 +++ kitiirfir/FIRFilterCode.cpp | 401 ++++++++++++++++ kitiirfir/FIRFilterCode.h | 36 ++ kitiirfir/FreqSamplingCode.cpp | 228 +++++++++ kitiirfir/FreqSamplingCode.h | 17 + kitiirfir/IIRFilterCode.cpp | 526 +++++++++++++++++++++ kitiirfir/IIRFilterCode.h | 38 ++ kitiirfir/LowPassPrototypes.cpp | 469 +++++++++++++++++++ kitiirfir/LowPassPrototypes.h | 45 ++ kitiirfir/LowPassRoots.cpp | 683 +++++++++++++++++++++++++++ kitiirfir/LowPassRoots.h | 27 ++ kitiirfir/NewParksMcClellan.cpp | 627 +++++++++++++++++++++++++ kitiirfir/NewParksMcClellan.h | 31 ++ kitiirfir/PFiftyOneRevE.cpp | 649 ++++++++++++++++++++++++++ kitiirfir/PFiftyOneRevE.h | 43 ++ kitiirfir/QuadRootsRevH.cpp | 344 ++++++++++++++ kitiirfir/QuadRootsRevH.h | 27 ++ kitiirfir/readme.md | 8 + sdrbase/dsp/phaselockcomplex.cpp | 18 +- sdrbase/dsp/phaselockcomplex.h | 2 - 23 files changed, 5069 insertions(+), 19 deletions(-) create mode 100644 kitiirfir/CMakeLists.txt create mode 100644 kitiirfir/FFTCode.cpp create mode 100644 kitiirfir/FFTCode.h create mode 100644 kitiirfir/FIRFilterCode.cpp create mode 100644 kitiirfir/FIRFilterCode.h create mode 100644 kitiirfir/FreqSamplingCode.cpp create mode 100644 kitiirfir/FreqSamplingCode.h create mode 100644 kitiirfir/IIRFilterCode.cpp create mode 100644 kitiirfir/IIRFilterCode.h create mode 100644 kitiirfir/LowPassPrototypes.cpp create mode 100644 kitiirfir/LowPassPrototypes.h create mode 100644 kitiirfir/LowPassRoots.cpp create mode 100644 kitiirfir/LowPassRoots.h create mode 100644 kitiirfir/NewParksMcClellan.cpp create mode 100644 kitiirfir/NewParksMcClellan.h create mode 100644 kitiirfir/PFiftyOneRevE.cpp create mode 100644 kitiirfir/PFiftyOneRevE.h create mode 100644 kitiirfir/QuadRootsRevH.cpp create mode 100644 kitiirfir/QuadRootsRevH.h create mode 100644 kitiirfir/readme.md diff --git a/CMakeLists.txt b/CMakeLists.txt index f3b26d126..9f5929be5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -219,6 +219,7 @@ endif() ############################################################################## # base libraries +add_subdirectory(kitiirfir) add_subdirectory(sdrbase) add_subdirectory(sdrgui) add_subdirectory(sdrsrv) diff --git a/kitiirfir/CMakeLists.txt b/kitiirfir/CMakeLists.txt new file mode 100644 index 000000000..0cb88df33 --- /dev/null +++ b/kitiirfir/CMakeLists.txt @@ -0,0 +1,38 @@ +project(kitiirfir) + +set(kitiirfir_SOURCES + FFTCode.cpp + FIRFilterCode.cpp + FreqSamplingCode.cpp + IIRFilterCode.cpp + LowPassPrototypes.cpp + LowPassRoots.cpp + NewParksMcClellan.cpp + PFiftyOneRevE.cpp + QuadRootsRevH.cpp +) + +set(kitiirfir_HEADERS + FFTCode.h + FIRFilterCode.h + FreqSamplingCode.h + IIRFilterCode.h + LowPassPrototypes.h + LowPassRoots.h + NewParksMcClellan.h + PFiftyOneRevE.h + QuadRootsRevH.h +) + +include_directories( + . + ${CMAKE_CURRENT_BINARY_DIR} +) + +add_definitions(-DQT_SHARED) + +add_library(kitiirfir SHARED + ${kitiirfir_SOURCES} +) + +install(TARGETS kitiirfir DESTINATION lib) diff --git a/kitiirfir/FFTCode.cpp b/kitiirfir/FFTCode.cpp new file mode 100644 index 000000000..914a95c08 --- /dev/null +++ b/kitiirfir/FFTCode.cpp @@ -0,0 +1,770 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + June 6, 2016 + + ShowMessage is a C++ Builder function, and it usage has been commented out. + If you are using C++ Builder, include vcl.h for ShowMessage. + Otherwise replace ShowMessage with something appropriate for your compiler. + */ + +#include "FFTCode.h" +#include + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- +// This calculates the required FFT size for a given number of points. +int RequiredFFTSize(int NumPts) { + int N = MINIMUM_FFT_SIZE; + while (N < NumPts && N < MAXIMUM_FFT_SIZE) { + N *= 2; + } + return N; +} + +//--------------------------------------------------------------------------- + +// This verifies that the FFT Size N = 2^M. M is returned +// N must be >= 8 for the Twiddle calculations +int IsValidFFTSize(int N) { + if (N < MINIMUM_FFT_SIZE || N > MAXIMUM_FFT_SIZE || (N & (N - 1)) != 0) + return (0); // N & (N - 1) ensures a power of 2 + return ((int) (log((double) N) / M_LN2 + 0.5)); // return M where N = 2^M +} + +//--------------------------------------------------------------------------- + +// Fast Fourier Transform +// This code puts DC in bin 0 and scales the output of a forward transform by 1/N. +// InputR and InputI are the real and imaginary input arrays of length N. +// The output values are returned in the Input arrays. +// TTransFormType is either FORWARD or INVERSE (defined in the header file) +// 256 pts in 50 us +void FFT(double *InputR, double *InputI, int N, TTransFormType Type) { + int j, LogTwoOfN, *RevBits; + double *BufferR, *BufferI, *TwiddleR, *TwiddleI; + double OneOverN; + + // Verify the FFT size and type. + LogTwoOfN = IsValidFFTSize(N); + if (LogTwoOfN == 0 || (Type != FORWARD && Type != INVERSE)) { + // ShowMessage("Invalid FFT type or size."); + return; + } + + // Memory allocation for all the arrays. + BufferR = new double[N]; + BufferI = new double[N]; + TwiddleR = new double[N / 2]; + TwiddleI = new double[N / 2]; + RevBits = new int[N]; + + if (BufferR == NULL || BufferI == NULL || TwiddleR == NULL + || TwiddleI == NULL || RevBits == NULL) { + // ShowMessage("FFT Memory Allocation Error"); + return; + } + + ReArrangeInput(InputR, InputI, BufferR, BufferI, RevBits, N); + FillTwiddleArray(TwiddleR, TwiddleI, N, Type); + Transform(InputR, InputI, BufferR, BufferI, TwiddleR, TwiddleI, N); + + // The ReArrangeInput function swapped Input[] and Buffer[]. Then Transform() + // swapped them again, LogTwoOfN times. Ultimately, these swaps must be done + // an even number of times, or the pointer to Buffer gets returned. + // So we must do one more swap here, for N = 16, 64, 256, 1024, ... + OneOverN = 1.0; + if (Type == FORWARD) + OneOverN = 1.0 / (double) N; + + if (LogTwoOfN % 2 == 1) { + for (j = 0; j < N; j++) + InputR[j] = InputR[j] * OneOverN; + for (j = 0; j < N; j++) + InputI[j] = InputI[j] * OneOverN; + } else // if(LogTwoOfN % 2 == 0) then the results are still in Buffer. + { + for (j = 0; j < N; j++) + InputR[j] = BufferR[j] * OneOverN; + for (j = 0; j < N; j++) + InputI[j] = BufferI[j] * OneOverN; + } + + delete[] BufferR; + delete[] BufferI; + delete[] TwiddleR; + delete[] TwiddleI; + delete[] RevBits; +} +//--------------------------------------------------------------------------- + +// This puts the input arrays in bit reversed order. +// The while loop generates an array of bit reversed numbers. e.g. +// e.g. N=8: RevBits = 0,4,2,6,1,5,3,7 N=16: RevBits = 0,8,4,12,2,10,6,14,1,9,5,13,3,11,7,15 +void ReArrangeInput(double *InputR, double *InputI, double *BufferR, + double *BufferI, int *RevBits, int N) { + int j, k, J, K; + + J = N / 2; + K = 1; + RevBits[0] = 0; + while (J >= 1) { + for (k = 0; k < K; k++) { + RevBits[k + K] = RevBits[k] + J; + } + K *= 2; + J /= 2; + } + + // Move the rearranged input values to Buffer. + // Take note of the pointer swaps at the top of the transform algorithm. + for (j = 0; j < N; j++) { + BufferR[j] = InputR[RevBits[j]]; + BufferI[j] = InputI[RevBits[j]]; + } + +} + +//--------------------------------------------------------------------------- + +/* + The Pentium takes a surprising amount of time to calculate the sine and cosine. + You may want to make the twiddle arrays static if doing repeated FFTs of the same size. + This uses 4 fold symmetry to calculate the twiddle factors. As a result, this function + requires a minimum FFT size of 8. + */ +void FillTwiddleArray(double *TwiddleR, double *TwiddleI, int N, + TTransFormType Type) { + int j; + double Theta, TwoPiOverN; + + TwoPiOverN = M_2PI / (double) N; + + if (Type == FORWARD) { + TwiddleR[0] = 1.0; + TwiddleI[0] = 0.0; + TwiddleR[N / 4] = 0.0; + TwiddleI[N / 4] = -1.0; + TwiddleR[N / 8] = M_SQRT_2; + TwiddleI[N / 8] = -M_SQRT_2; + TwiddleR[3 * N / 8] = -M_SQRT_2; + TwiddleI[3 * N / 8] = -M_SQRT_2; + for (j = 1; j < N / 8; j++) { + Theta = (double) j * -TwoPiOverN; + TwiddleR[j] = cos(Theta); + TwiddleI[j] = sin(Theta); + TwiddleR[N / 4 - j] = -TwiddleI[j]; + TwiddleI[N / 4 - j] = -TwiddleR[j]; + TwiddleR[N / 4 + j] = TwiddleI[j]; + TwiddleI[N / 4 + j] = -TwiddleR[j]; + TwiddleR[N / 2 - j] = -TwiddleR[j]; + TwiddleI[N / 2 - j] = TwiddleI[j]; + } + } + + else { + TwiddleR[0] = 1.0; + TwiddleI[0] = 0.0; + TwiddleR[N / 4] = 0.0; + TwiddleI[N / 4] = 1.0; + TwiddleR[N / 8] = M_SQRT_2; + TwiddleI[N / 8] = M_SQRT_2; + TwiddleR[3 * N / 8] = -M_SQRT_2; + TwiddleI[3 * N / 8] = M_SQRT_2; + for (j = 1; j < N / 8; j++) { + Theta = (double) j * TwoPiOverN; + TwiddleR[j] = cos(Theta); + TwiddleI[j] = sin(Theta); + TwiddleR[N / 4 - j] = TwiddleI[j]; + TwiddleI[N / 4 - j] = TwiddleR[j]; + TwiddleR[N / 4 + j] = -TwiddleI[j]; + TwiddleI[N / 4 + j] = TwiddleR[j]; + TwiddleR[N / 2 - j] = -TwiddleR[j]; + TwiddleI[N / 2 - j] = TwiddleI[j]; + } + } + +} + +//--------------------------------------------------------------------------- + +// The Fast Fourier Transform. +void Transform(double *InputR, double *InputI, double *BufferR, double *BufferI, + double *TwiddleR, double *TwiddleI, int N) { + int j, k, J, K, I, T; + double *TempPointer; + double TempR, TempI; + + J = N / 2; // J increments down to 1 + K = 1; // K increments up to N/2 + while (J > 0) // Loops Log2(N) times. + { + // Swap pointers, instead doing this: for(j=0; j 0.0) + Mag = sqrt(Mag); + else + Mag = 1.0E-12; + + return (Mag); +} + +//--------------------------------------------------------------------------- + +/* + These are the window definitions. These windows can be used for either + FIR filter design or with an FFT for spectral analysis. + For definitions, see this article: http://en.wikipedia.org/wiki/Window_function + + This function has 6 inputs + Data is the array, of length N, containing the data to to be windowed. + This data is either an FIR filter sinc pulse, or the data to be analyzed by an fft. + + WindowType is an enum defined in the header file. + e.g. wtKAISER, wtSINC, wtHANNING, wtHAMMING, wtBLACKMAN, ... + + Alpha sets the width of the flat top. + Windows such as the Tukey and Trapezoid are defined to have a variably wide flat top. + As can be seen by its definition, the Tukey is just a Hanning window with a flat top. + Alpha can be used to give any of these windows a partial flat top, except the Flattop and Kaiser. + Alpha = 0 gives the original window. (i.e. no flat top) + To generate a Tukey window, use a Hanning with 0 < Alpha < 1 + To generate a Bartlett window (triangular), use a Trapezoid window with Alpha = 0. + Alpha = 1 generates a rectangular window in all cases. (except the Flattop and Kaiser) + + + Beta is used with the Kaiser, Sinc, and Sine windows only. + These three windows are used primarily for FIR filter design. Then + Beta controls the filter's transition bandwidth and the sidelobe levels. + All other windows ignore Beta. + + UnityGain controls whether the gain of these windows is set to unity. + Only the Flattop window has unity gain by design. The Hanning window, for example, has a gain + of 1/2. UnityGain = true sets the gain to 1, which preserves the signal's energy level + when these windows are used for spectral analysis. + + Don't use this with FIR filter design however. Since most of the enegy in an FIR sinc pulse + is in the middle of the window, the window needs a peak amplitude of one, not unity gain. + Setting UnityGain = true will simply cause the resulting FIR filter to have excess gain. + + If using these windows for FIR filters, start with the Kaiser, Sinc, or Sine windows and + adjust Beta for the desired transition BW and sidelobe levels (set Alpha = 0). + While the FlatTop is an excellent window for spectral analysis, don't use it for FIR filter design. + It has a peak amplitude of ~ 4.7 which causes the resulting FIR filter to have about this much gain. + It works poorly for FIR filters even if you adjust its peak amplitude. + The Trapezoid also works poorly for FIR filter design. + + If using these windows with an fft for spectral analysis, start with the Hanning, Gauss, or Flattop. + When choosing a window for spectral analysis, you must trade off between resolution and amplitude + accuracy. The Hanning has the best resolution while the Flatop has the best amplitude accuracy. + The Gauss is midway between these two for both accuracy and resolution. These three were + the only windows available in the HP 89410A Vector Signal Analyzer. Which is to say, these three + are the probably the best windows for general purpose signal analysis. + */ + +void WindowData(double *Data, int N, TWindowType WindowType, double Alpha, + double Beta, bool UnityGain) { + if (WindowType == wtNONE) + return; + + int j, M, TopWidth; + double dM, *WinCoeff; + + if (WindowType == wtKAISER || WindowType == wtFLATTOP) + Alpha = 0.0; + + if (Alpha < 0.0) + Alpha = 0.0; + if (Alpha > 1.0) + Alpha = 1.0; + + if (Beta < 0.0) + Beta = 0.0; + if (Beta > 10.0) + Beta = 10.0; + + WinCoeff = new double[N + 2]; + if (WinCoeff == NULL) { + // ShowMessage("Failed to allocate memory in WindowData() "); + return; + } + + TopWidth = (int) (Alpha * (double) N); + if (TopWidth % 2 != 0) + TopWidth++; + if (TopWidth > N) + TopWidth = N; + M = N - TopWidth; + dM = M + 1; + + // Calculate the window for N/2 points, then fold the window over (at the bottom). + // TopWidth points will be set to 1. + if (WindowType == wtKAISER) { + double Arg; + for (j = 0; j < M; j++) { + Arg = Beta * sqrt(1.0 - pow(((double) (2 * j + 2) - dM) / dM, 2.0)); + WinCoeff[j] = Bessel(Arg) / Bessel(Beta); + } + } + + else if (WindowType == wtSINC) // Lanczos + { + for (j = 0; j < M; j++) + WinCoeff[j] = Sinc((double) (2 * j + 1 - M) / dM * M_PI); + for (j = 0; j < M; j++) + WinCoeff[j] = pow(WinCoeff[j], Beta); + } + + else if (WindowType == wtSINE) // Hanning if Beta = 2 + { + for (j = 0; j < M / 2; j++) + WinCoeff[j] = sin((double) (j + 1) * M_PI / dM); + for (j = 0; j < M / 2; j++) + WinCoeff[j] = pow(WinCoeff[j], Beta); + } + + else if (WindowType == wtHANNING) { + for (j = 0; j < M / 2; j++) + WinCoeff[j] = 0.5 - 0.5 * cos((double) (j + 1) * M_2PI / dM); + } + + else if (WindowType == wtHAMMING) { + for (j = 0; j < M / 2; j++) + WinCoeff[j] = 0.54 - 0.46 * cos((double) (j + 1) * M_2PI / dM); + } + + else if (WindowType == wtBLACKMAN) { + for (j = 0; j < M / 2; j++) { + WinCoeff[j] = 0.42 - 0.50 * cos((double) (j + 1) * M_2PI / dM) + + 0.08 * cos((double) (j + 1) * M_2PI * 2.0 / dM); + } + } + + // Defined at: http://www.bth.se/fou/forskinfo.nsf/0/130c0940c5e7ffcdc1256f7f0065ac60/$file/ICOTA_2004_ttr_icl_mdh.pdf + else if (WindowType == wtFLATTOP) { + for (j = 0; j <= M / 2; j++) { + WinCoeff[j] = 1.0 + - 1.93293488969227 * cos((double) (j + 1) * M_2PI / dM) + + 1.28349769674027 + * cos((double) (j + 1) * M_2PI * 2.0 / dM) + - 0.38130801681619 + * cos((double) (j + 1) * M_2PI * 3.0 / dM) + + 0.02929730258511 + * cos((double) (j + 1) * M_2PI * 4.0 / dM); + } + } + + else if (WindowType == wtBLACKMAN_HARRIS) { + for (j = 0; j < M / 2; j++) { + WinCoeff[j] = 0.35875 - 0.48829 * cos((double) (j + 1) * M_2PI / dM) + + 0.14128 * cos((double) (j + 1) * M_2PI * 2.0 / dM) + - 0.01168 * cos((double) (j + 1) * M_2PI * 3.0 / dM); + } + } + + else if (WindowType == wtBLACKMAN_NUTTALL) { + for (j = 0; j < M / 2; j++) { + WinCoeff[j] = 0.3535819 + - 0.4891775 * cos((double) (j + 1) * M_2PI / dM) + + 0.1365995 * cos((double) (j + 1) * M_2PI * 2.0 / dM) + - 0.0106411 * cos((double) (j + 1) * M_2PI * 3.0 / dM); + } + } + + else if (WindowType == wtNUTTALL) { + for (j = 0; j < M / 2; j++) { + WinCoeff[j] = 0.355768 + - 0.487396 * cos((double) (j + 1) * M_2PI / dM) + + 0.144232 * cos((double) (j + 1) * M_2PI * 2.0 / dM) + - 0.012604 * cos((double) (j + 1) * M_2PI * 3.0 / dM); + } + } + + else if (WindowType == wtKAISER_BESSEL) { + for (j = 0; j <= M / 2; j++) { + WinCoeff[j] = 0.402 - 0.498 * cos(M_2PI * (double) (j + 1) / dM) + + 0.098 * cos(2.0 * M_2PI * (double) (j + 1) / dM) + + 0.001 * cos(3.0 * M_2PI * (double) (j + 1) / dM); + } + } + + else if (WindowType == wtTRAPEZOID) // Rectangle for Alpha = 1 Triangle for Alpha = 0 + { + int K = M / 2; + if (M % 2) + K++; + for (j = 0; j < K; j++) + WinCoeff[j] = (double) (j + 1) / (double) K; + } + + // This definition is from http://en.wikipedia.org/wiki/Window_function (Gauss Generalized normal window) + // We set their p = 2, and use Alpha in the numerator, instead of Sigma in the denominator, as most others do. + // Alpha = 2.718 puts the Gauss window response midway between the Hanning and the Flattop (basically what we want). + // It also gives the same BW as the Gauss window used in the HP 89410A Vector Signal Analyzer. + else if (WindowType == wtGAUSS) { + for (j = 0; j < M / 2; j++) { + WinCoeff[j] = ((double) (j + 1) - dM / 2.0) / (dM / 2.0) * 2.7183; + WinCoeff[j] *= WinCoeff[j]; + WinCoeff[j] = exp(-WinCoeff[j]); + } + } + + else // Error. + { + // ShowMessage("Incorrect window type in WindowFFTData"); + delete[] WinCoeff; + return; + } + + // Fold the coefficients over. + for (j = 0; j < M / 2; j++) + WinCoeff[N - j - 1] = WinCoeff[j]; + + // This is the flat top if Alpha > 0. Cannot be applied to a Kaiser or Flat Top. + if (WindowType != wtKAISER && WindowType != wtFLATTOP) { + for (j = M / 2; j < N - M / 2; j++) + WinCoeff[j] = 1.0; + } + + // UnityGain = true will set the gain of these windows to 1. Don't use this with FIR filter design. + if (UnityGain) { + double Sum = 0.0; + for (j = 0; j < N; j++) + Sum += WinCoeff[j]; + Sum /= (double) N; + if (Sum != 0.0) + for (j = 0; j < N; j++) + WinCoeff[j] /= Sum; + } + + // Apply the window to the data. + for (j = 0; j < N; j++) + Data[j] *= WinCoeff[j]; + + delete[] WinCoeff; + +} + +//--------------------------------------------------------------------------- + +// This gets used with the Kaiser window. +double Bessel(double x) { + double Sum = 0.0, XtoIpower; + int i, j, Factorial; + for (i = 1; i < 10; i++) { + XtoIpower = pow(x / 2.0, (double) i); + Factorial = 1; + for (j = 1; j <= i; j++) + Factorial *= j; + Sum += pow(XtoIpower / (double) Factorial, 2.0); + } + return (1.0 + Sum); +} + +//----------------------------------------------------------------------------- + +// This gets used with the Sinc window. +double Sinc(double x) { + if (x > -1.0E-5 && x < 1.0E-5) + return (1.0); + return (sin(x) / x); +} + +//--------------------------------------------------------------------------- + +} // namespace diff --git a/kitiirfir/FFTCode.h b/kitiirfir/FFTCode.h new file mode 100644 index 000000000..59bdb4388 --- /dev/null +++ b/kitiirfir/FFTCode.h @@ -0,0 +1,60 @@ +//--------------------------------------------------------------------------- + +#ifndef FFTCodeH +#define FFTCodeH + +#define M_2PI 6.28318530717958647692 // 2*Pi +#define M_SQRT_2 0.707106781186547524401 // sqrt(2)/2 +#define MAXIMUM_FFT_SIZE 1048576 +#define MINIMUM_FFT_SIZE 8 + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- +// Must retain the order on the 1st line for legacy FIR code. +enum TWindowType { + wtFIRSTWINDOW, + wtNONE, + wtKAISER, + wtSINC, + wtHANNING, + wtHAMMING, + wtBLACKMAN, + wtFLATTOP, + wtBLACKMAN_HARRIS, + wtBLACKMAN_NUTTALL, + wtNUTTALL, + wtKAISER_BESSEL, + wtTRAPEZOID, + wtGAUSS, + wtSINE, + wtTEST +}; + +enum TTransFormType { + FORWARD, INVERSE +}; + +int RequiredFFTSize(int NumPts); +int IsValidFFTSize(int x); +void FFT(double *InputR, double *InputI, int N, TTransFormType Type); +void ReArrangeInput(double *InputR, double *InputI, double *BufferR, + double *BufferI, int *RevBits, int N); +void FillTwiddleArray(double *TwiddleR, double *TwiddleI, int N, + TTransFormType Type); +void Transform(double *InputR, double *InputI, double *BufferR, double *BufferI, + double *TwiddleR, double *TwiddleI, int N); +void DFT(double *InputR, double *InputI, int N, int Type); +void RealSigDFT(double *Samples, double *OutputR, double *OutputI, int N); +double SingleFreqDFT(double *Samples, int N, double Omega); +double Goertzel(double *Samples, int N, double Omega); +void WindowData(double *Data, int N, TWindowType WindowType, double Alpha, + double Beta, bool UnityGain); +double Bessel(double x); +double Sinc(double x); + +} // namespace + +#endif + diff --git a/kitiirfir/FIRFilterCode.cpp b/kitiirfir/FIRFilterCode.cpp new file mode 100644 index 000000000..f3cb9186f --- /dev/null +++ b/kitiirfir/FIRFilterCode.cpp @@ -0,0 +1,401 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + May 1, 2016 + + ShowMessage is a C++ Builder function, and it usage has been commented out. + If you are using C++ Builder, include vcl.h for ShowMessage. + Otherwise replace ShowMessage with something appropriate for yor compiler. + + RectWinFIR() generates the impulse response for a rectangular windowed low pass, high pass, + band pass, or notch filter. Then a window, such as the Kaiser, is applied to the FIR coefficients. + See the FilterKitMain.cpp file for an example on how to use this code. + + double FirCoeff[MAXNUMTAPS]; + int NumTaps; NumTaps can be even or odd and < MAXNUMTAPS + TPassTypeName PassType; PassType is defined in the header file. firLPF, firHPF, firBPF, firNOTCH, firALLPASS + double OmegaC 0.0 < OmegaC < 1.0 The corner freq, or center freq if BPF or NOTCH + double BW 0.0 < BW < 1.0 The band width if BPF or NOTCH + */ + +#include "FIRFilterCode.h" +#include + +namespace kitiirfir +{ + +// Rectangular Windowed FIR. The equations used here are developed in numerous textbooks. +void RectWinFIR(double *FirCoeff, int NumTaps, TFIRPassTypes PassType, + double OmegaC, double BW) { + int j; + double Arg, OmegaLow, OmegaHigh; + + switch (PassType) { + case firLPF: // Low Pass + for (j = 0; j < NumTaps; j++) { + Arg = (double) j - (double) (NumTaps - 1) / 2.0; + FirCoeff[j] = OmegaC * Sinc(OmegaC * Arg * M_PI); + } + break; + + case firHPF: // High Pass + if (NumTaps % 2 == 1) // Odd tap counts + { + for (j = 0; j < NumTaps; j++) { + Arg = (double) j - (double) (NumTaps - 1) / 2.0; + FirCoeff[j] = Sinc(Arg * M_PI) + - OmegaC * Sinc(OmegaC * Arg * M_PI); + } + } + + else // Even tap counts + { + for (j = 0; j < NumTaps; j++) { + Arg = (double) j - (double) (NumTaps - 1) / 2.0; + if (Arg == 0.0) + FirCoeff[j] = 0.0; + else + FirCoeff[j] = cos(OmegaC * Arg * M_PI) / M_PI / Arg + + cos(Arg * M_PI); + } + } + break; + + case firBPF: // Band Pass + OmegaLow = OmegaC - BW / 2.0; + OmegaHigh = OmegaC + BW / 2.0; + for (j = 0; j < NumTaps; j++) { + Arg = (double) j - (double) (NumTaps - 1) / 2.0; + if (Arg == 0.0) + FirCoeff[j] = 0.0; + else + FirCoeff[j] = (cos(OmegaLow * Arg * M_PI) + - cos(OmegaHigh * Arg * M_PI)) / M_PI / Arg; + } + break; + + case firNOTCH: // Notch, if NumTaps is even, the response at Pi is attenuated. + OmegaLow = OmegaC - BW / 2.0; + OmegaHigh = OmegaC + BW / 2.0; + for (j = 0; j < NumTaps; j++) { + Arg = (double) j - (double) (NumTaps - 1) / 2.0; + FirCoeff[j] = Sinc(Arg * M_PI) + - OmegaHigh * Sinc(OmegaHigh * Arg * M_PI) + - OmegaLow * Sinc(OmegaLow * Arg * M_PI); + } + break; + + case firALLPASS: // All Pass, this is trivial, but it shows how an fir all pass (delay) can be done. + for (j = 0; j < NumTaps; j++) + FirCoeff[j] = 0.0; + FirCoeff[(NumTaps - 1) / 2] = 1.0; + break; + case firNOT_FIR: + default: + break; + } + // Now use the FIRFilterWindow() function to reduce the sinc(x) effects. +} + +//--------------------------------------------------------------------------- + +// Used to reduce the sinc(x) effects on a set of FIR coefficients. This will, unfortunately, +// widen the filter's transition band, but the stop band attenuation will improve dramatically. +void FIRFilterWindow(double *FIRCoeff, int N, TWindowType WindowType, + double Beta) { + if (WindowType == wtNONE) + return; + + int j; + double dN, *WinCoeff; + + if (Beta < 0.0) + Beta = 0.0; + if (Beta > 10.0) + Beta = 10.0; + + WinCoeff = new double[N + 2]; + if (WinCoeff == NULL) { + // ShowMessage("Failed to allocate memory in WindowData() "); + return; + } + + // Calculate the window for N/2 points, then fold the window over (at the bottom). + dN = N + 1; // a double + if (WindowType == wtKAISER) { + double Arg; + for (j = 0; j < N; j++) { + Arg = Beta * sqrt(1.0 - pow(((double) (2 * j + 2) - dN) / dN, 2.0)); + WinCoeff[j] = Bessel(Arg) / Bessel(Beta); + } + } + + else if (WindowType == wtSINC) // Lanczos + { + for (j = 0; j < N; j++) + WinCoeff[j] = Sinc((double) (2 * j + 1 - N) / dN * M_PI); + for (j = 0; j < N; j++) + WinCoeff[j] = pow(WinCoeff[j], Beta); + } + + else if (WindowType == wtSINE) // Hanning if Beta = 2 + { + for (j = 0; j < N / 2; j++) + WinCoeff[j] = sin((double) (j + 1) * M_PI / dN); + for (j = 0; j < N / 2; j++) + WinCoeff[j] = pow(WinCoeff[j], Beta); + } + + else // Error. + { + // ShowMessage("Incorrect window type in WindowFFTData"); + delete[] WinCoeff; + return; + } + + // Fold the coefficients over. + for (j = 0; j < N / 2; j++) + WinCoeff[N - j - 1] = WinCoeff[j]; + + // Apply the window to the FIR coefficients. + for (j = 0; j < N; j++) + FIRCoeff[j] *= WinCoeff[j]; + + delete[] WinCoeff; + +} + +//--------------------------------------------------------------------------- + +// This implements an FIR filter. The register shifts are done by rotating the indexes. +void FilterWithFIR(double *FirCoeff, int NumTaps, double *Signal, + double *FilteredSignal, int NumSigPts) { + int j, k, n, Top = 0; + double y, Reg[MAX_NUMTAPS]; + + for (j = 0; j < NumTaps; j++) + Reg[j] = 0.0; + + for (j = 0; j < NumSigPts; j++) { + Reg[Top] = Signal[j]; + y = 0.0; + n = 0; + + // The FirCoeff index increases while the Reg index decreases. + for (k = Top; k >= 0; k--) { + y += FirCoeff[n++] * Reg[k]; + } + for (k = NumTaps - 1; k > Top; k--) { + y += FirCoeff[n++] * Reg[k]; + } + FilteredSignal[j] = y; + + Top++; + if (Top >= NumTaps) + Top = 0; + } + +} + +//--------------------------------------------------------------------------- + +// This code is equivalent to the code above. It uses register shifts, which makes it +// less efficient, but it is easier to follow (i.e. compare to a FIR flow chart). +void FilterWithFIR2(double *FirCoeff, int NumTaps, double *Signal, + double *FilteredSignal, int NumSigPts) { + int j, k; + double y, Reg[MAX_NUMTAPS]; + + for (j = 0; j < NumTaps; j++) + Reg[j] = 0.0; // Init the delay registers. + + for (j = 0; j < NumSigPts; j++) { + // Shift the register values down and set Reg[0]. + for (k = NumTaps; k > 1; k--) + Reg[k - 1] = Reg[k - 2]; + Reg[0] = Signal[j]; + + y = 0.0; + for (k = 0; k < NumTaps; k++) + y += FirCoeff[k] * Reg[k]; + FilteredSignal[j] = y; + } + +} + +//--------------------------------------------------------------------------- + +// This function is used to correct the corner frequency values on FIR filters. +// We normally specify the 3 dB frequencies when specifing a filter. The Parks McClellan routine +// uses OmegaC and BW to set the 0 dB band edges, so its final OmegaC and BW values are not close +// to -3 dB. The Rectangular Windowed filters are better for high tap counts, but for low tap counts, +// their 3 dB frequencies are also well off the mark. + +// To use this function, first calculate a set of FIR coefficients, then pass them here, along with +// OmegaC and BW. This calculates a corrected OmegaC for low and high pass filters. It calcultes a +// corrected BW for band pass and notch filters. Use these corrected values to recalculate the FIR filter. + +// The Goertzel algorithm is used to calculate the filter's magnitude response at the single +// frequency defined in the loop. We start in the pass band and work out to the -20dB freq. + +void FIRFreqError(double *Coeff, int NumTaps, int PassType, double *OmegaC, + double *BW) { + int j, J3dB, CenterJ; + double Omega, CorrectedOmega, CorrectedBW, Omega1, Omega2, Mag; + + // In these loops, we break at -20 dB to ensure that large ripple is ignored. + if (PassType == firLPF) { + J3dB = 10; + for (j = 0; j < NUM_FREQ_ERR_PTS; j++) { + Omega = (double) j / dNUM_FREQ_ERR_PTS; + Mag = Goertzel(Coeff, NumTaps, Omega); + if (Mag > 0.707) + J3dB = j; // J3dB will be the last j where the response was > -3 dB + if (Mag < 0.1) + break; // Stop when the response is down to -20 dB. + } + Omega = (double) J3dB / dNUM_FREQ_ERR_PTS; + } + + else if (PassType == firHPF) { + J3dB = NUM_FREQ_ERR_PTS - 10; + for (j = NUM_FREQ_ERR_PTS - 1; j >= 0; j--) { + Omega = (double) j / dNUM_FREQ_ERR_PTS; + Mag = Goertzel(Coeff, NumTaps, Omega); + if (Mag > 0.707) + J3dB = j; // J3dB will be the last j where the response was > -3 dB + if (Mag < 0.1) + break; // Stop when the response is down to -20 dB. + } + Omega = (double) J3dB / dNUM_FREQ_ERR_PTS; + } + + else if (PassType == firBPF) { + CenterJ = (int) (dNUM_FREQ_ERR_PTS * *OmegaC); + J3dB = CenterJ; + for (j = CenterJ; j >= 0; j--) { + Omega = (double) j / dNUM_FREQ_ERR_PTS; + Mag = Goertzel(Coeff, NumTaps, Omega); + if (Mag > 0.707) + J3dB = j; + if (Mag < 0.1) + break; + } + Omega1 = (double) J3dB / dNUM_FREQ_ERR_PTS; + + J3dB = CenterJ; + for (j = CenterJ; j < NUM_FREQ_ERR_PTS; j++) { + Omega = (double) j / dNUM_FREQ_ERR_PTS; + Mag = Goertzel(Coeff, NumTaps, Omega); + if (Mag > 0.707) + J3dB = j; + if (Mag < 0.1) + break; + } + Omega2 = (double) J3dB / dNUM_FREQ_ERR_PTS; + } + + // The code above starts in the pass band. This starts in the stop band. + else // PassType == firNOTCH + { + CenterJ = (int) (dNUM_FREQ_ERR_PTS * *OmegaC); + J3dB = CenterJ; + for (j = CenterJ; j >= 0; j--) { + Omega = (double) j / dNUM_FREQ_ERR_PTS; + Mag = Goertzel(Coeff, NumTaps, Omega); + if (Mag <= 0.707) + J3dB = j; + if (Mag > 0.99) + break; + } + Omega1 = (double) J3dB / dNUM_FREQ_ERR_PTS; + + J3dB = CenterJ; + for (j = CenterJ; j < NUM_FREQ_ERR_PTS; j++) { + Omega = (double) j / dNUM_FREQ_ERR_PTS; + Mag = Goertzel(Coeff, NumTaps, Omega); + if (Mag <= 0.707) + J3dB = j; + if (Mag > 0.99) + break; + } + Omega2 = (double) J3dB / dNUM_FREQ_ERR_PTS; + } + + // This calculates the corrected OmegaC and BW and error checks the values. + if (PassType == firLPF || PassType == firHPF) { + CorrectedOmega = *OmegaC * 2.0 - Omega; // This is usually OK. + if (CorrectedOmega < 0.001) + CorrectedOmega = 0.001; + if (CorrectedOmega > 0.99) + CorrectedOmega = 0.99; + *OmegaC = CorrectedOmega; + } + + else // PassType == firBPF || PassType == firNOTCH + { + CorrectedBW = *BW * 2.0 - (Omega2 - Omega1); // This routinely goes neg with Notch. + if (CorrectedBW < 0.01) + CorrectedBW = 0.01; + if (CorrectedBW > *BW * 2.0) + CorrectedBW = *BW * 2.0; + if (CorrectedBW > 0.98) + CorrectedBW = 0.98; + *BW = CorrectedBW; + } + +} + +//----------------------------------------------------------------------------- + +// This shows how to adjust the delay of an FIR by a fractional amount. +// We take the FFT of the FIR coefficients to get to the frequency domain, +// then apply the Laplace delay operator, and then do an inverse FFT. + +// Use this function last. i.e. After the window was applied to the coefficients. +// The Delay value is in terms of a fraction of a sample (not in terms of sampling freq). +// Delay may be pos or neg. Typically a filter's delay can be adjusted by +/- NumTaps/20 +// without affecting its performance significantly. A typical Delay value would be 0.75 +void AdjustDelay(double *FirCoeff, int NumTaps, double Delay) { + int j, FFTSize; + double *FFTInputR, *FFTInputI, Arg, Temp; + FFTSize = RequiredFFTSize(NumTaps + (int) fabs(Delay) + 1); // Zero pad by at least Delay + 1 to prevent the impulse response from wrapping around. + + FFTInputR = new double[FFTSize]; // Real part + FFTInputI = new double[FFTSize]; // Imag part + if (FFTInputR == NULL || FFTInputI == NULL) { + //ShowMessage("Unable to allocate memory in AdjustDelay"); + return; + } + for (j = 0; j < FFTSize; j++) + FFTInputR[j] = FFTInputI[j] = 0.0; // A mandatory init. + for (j = 0; j < NumTaps; j++) + FFTInputR[j] = FirCoeff[j]; // Fill the real part with the FIR coeff. + + FFT(FFTInputR, FFTInputI, FFTSize, FORWARD); // Do an FFT + for (j = 0; j <= FFTSize / 2; j++) // Apply the Laplace Delay operator e^(-j*omega*Delay). + { + Arg = -Delay * (double) j / (double) FFTSize * M_2PI; // This is -Delay * (the FFT bin frequency). + Temp = cos(Arg) * FFTInputR[j] - sin(Arg) * FFTInputI[j]; + FFTInputI[j] = cos(Arg) * FFTInputI[j] + sin(Arg) * FFTInputR[j]; + FFTInputR[j] = Temp; + } + for (j = 1; j < FFTSize / 2; j++) // Fill the neg freq bins with the conjugate values. + { + FFTInputR[FFTSize - j] = FFTInputR[j]; + FFTInputI[FFTSize - j] = -FFTInputI[j]; + } + + FFT(FFTInputR, FFTInputI, FFTSize, INVERSE); // Inverse FFT + for (j = 0; j < NumTaps; j++) { + FirCoeff[j] = FFTInputR[j]; + } + + delete[] FFTInputR; + delete[] FFTInputI; +} +//----------------------------------------------------------------------------- + +} //namedspace diff --git a/kitiirfir/FIRFilterCode.h b/kitiirfir/FIRFilterCode.h new file mode 100644 index 000000000..ee60fee02 --- /dev/null +++ b/kitiirfir/FIRFilterCode.h @@ -0,0 +1,36 @@ +//--------------------------------------------------------------------------- + +#ifndef FIRFilterCodeH +#define FIRFilterCodeH +#include "FFTCode.h" // For the definition of TWindowType +//--------------------------------------------------------------------------- + +#define MAX_NUMTAPS 256 +#define M_2PI 6.28318530717958647692 +#define NUM_FREQ_ERR_PTS 1000 // these are only used in the FIRFreqError function. +#define dNUM_FREQ_ERR_PTS 1000.0 + +namespace kitiirfir +{ + +enum TFIRPassTypes { + firLPF, firHPF, firBPF, firNOTCH, firALLPASS, firNOT_FIR +}; + +void FilterWithFIR(double *FirCoeff, int NumTaps, double *Signal, + double *FilteredSignal, int NumSigPts); +void FilterWithFIR2(double *FirCoeff, int NumTaps, double *Signal, + double *FilteredSignal, int NumSigPts); +void RectWinFIR(double *FirCoeff, int NumTaps, TFIRPassTypes PassType, + double OmegaC, double BW); +void WindowData(double *Data, int N, TWindowType WindowType, double Alpha, + double Beta, bool UnityGain); +void FIRFreqError(double *Coeff, int NumTaps, int PassType, double *OmegaC, + double *BW); +void FIRFilterWindow(double *FIRCoeff, int N, TWindowType WindowType, + double Beta); +void AdjustDelay(double *FirCoeff, int NumTaps, double Delay); + +} // namespace + +#endif diff --git a/kitiirfir/FreqSamplingCode.cpp b/kitiirfir/FreqSamplingCode.cpp new file mode 100644 index 000000000..64565ec8c --- /dev/null +++ b/kitiirfir/FreqSamplingCode.cpp @@ -0,0 +1,228 @@ +#include +#include +#include "FreqSamplingCode.h" +#include "FIRFilterCode.h" // for the definition of TFIRPassTypes +#include "FFTCode.h" +#include "LowPassPrototypes.h" + +namespace kitiirfir +{ + +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + May 1, 2016 + + This code generates an FIR filter with the frequency over-sampling method. By this we + mean that we always sample the frequency domain at least 1024 times (a large power of + 2 suitable for an FFT) even though we typically want a relatively small number of FIR + coefficients (< 50). + + Most authors use N frequency samples to generate N taps. While valid, this tends to loose a + significant amount of frequency domain information if the tap count is small. + + Using a large number of samples will generate a filter equivalent to those generated with + the equations for a classic Rectangular Windowed FIR filter. Those equations were derived + by using an infinite number of frequency samples, which is to say, performing an integral. + + To see how well this works, use this code to sample a simple rectanglular low pass response + and compare the results to the coefficients generated by Windowed FIR code used the + BasicFIR() function in FIRFilterCode.cpp + + See the example code in FilterKitMain for an example of using SampledFreqFIR(). + This function is called after the HofSReal array is filled with the desired magnitude response. + The only trick to this method is getting the phase set correctly. There are two aspects to + setting the phase. Using the correct slope, which is the same for all filters, but does + depend on even or odd tap counts. And we must ensure that the phase value is correct at + Omega = 0.0 and Pi. + + If the filter has a low pass response (magnitude != 0.0 at DC), then the phase at Omega=0 must + be zero. If the filter has a high pass response (magnitude != 0.0 at Pi), then the phase + must be zero of Omega = Pi. + + A band pass filter, which has neither a low or high pass response, can have any phase value at + Omega = 0 and Pi. But a Notch filter, which has both a low and high pass response, must have + zero phase at both zero and Pi. The code below should make this more clear. + + NumTaps Number of FIR taps + FirCoeff The output array for the FIR coefficients. + HofSReal The array containing the desired magnitude response. + HofSImag The imag part of the response (zero when this function is called). + OmegaC The center frequency. (Only needed for notch filters.) + PassType firLPF, firHPF, firBPF, firNOTCH Needed to set the phase properly. (defined in FIRFilterCode.h) + */ + +void SampledFreqFIR(int NumTaps, double *FirCoeff, double *HofSReal, + double *HofSImag, double OmegaC, TFIRPassTypes PassType) { + int j, CenterJ, NumSamples, StartJ; + double dNumSamples, RadPerSample, Arg; + NumSamples = NUM_POS_FREQ_SAMPLES; + dNumSamples = (double) NumSamples; + + // Limit test NumTaps + if (NumTaps > MAX_NUMTAPS) + NumTaps = MAX_NUMTAPS; + if (NumTaps > 2 * NUM_POS_FREQ_SAMPLES) + NumTaps = 2 * NUM_POS_FREQ_SAMPLES; + + // Set the slope of the phase. + RadPerSample = -M_PI_2 * (2.0 * dNumSamples - 1.0) / dNumSamples; // Even tap count. + if (NumTaps % 2 == 1) + RadPerSample = -M_PI; // Odd tap count. + + // Set the phase according to the type of response. + switch (PassType) { + case firLPF: // Low pass and band pass phase = 0 at DC + case firBPF: + for (j = 0; j < NumSamples; j++) { + Arg = RadPerSample * (double) j; // For band pass filters ONLY, an arbitrary amount of phase can be added to Arg. e.g. Add Pi/2 to generate a Hilbert filter, or +/- Pi/4 to 2 different filters to generate a pair of 45 degree Hilberts. + HofSImag[j] = HofSReal[j] * sin(Arg); + HofSReal[j] = HofSReal[j] * cos(Arg); + } + break; + + case firHPF: // High pass phase = 0 at Pi + for (j = NumSamples; j >= 0; j--) { + Arg = RadPerSample * (double) (j - NumSamples); + HofSImag[j] = HofSReal[j] * sin(Arg); + HofSReal[j] = HofSReal[j] * cos(Arg); + } + break; + + case firNOTCH: // Notch phase = 0 at DC and Pi + CenterJ = (int) (OmegaC * dNumSamples); + for (j = 0; j <= CenterJ; j++) { + Arg = RadPerSample * (double) j; + HofSImag[j] = HofSReal[j] * sin(Arg); + HofSReal[j] = HofSReal[j] * cos(Arg); + } + for (j = NumSamples; j >= CenterJ; j--) { + Arg = RadPerSample * (double) (j - NumSamples); + HofSImag[j] = HofSReal[j] * sin(Arg); + HofSReal[j] = HofSReal[j] * cos(Arg); + } + break; + case firALLPASS: + case firNOT_FIR: + default: + break; + } + + // Fill the negative frequency bins of HofS with the conjugate of HofS for the FFT. + for (j = 1; j < NumSamples; j++) + HofSReal[2 * NumSamples - j] = HofSReal[j]; + for (j = 1; j < NumSamples; j++) + HofSImag[2 * NumSamples - j] = -HofSImag[j]; + + // The Fourier Transform requires the center freq bins to be 0 for LPF and BPF, 1 for HPF and Notch + if (PassType == firLPF || PassType == firBPF) { + HofSReal[NumSamples] = 0.0; + HofSImag[NumSamples] = 0.0; + } else { + HofSReal[NumSamples] = 1.0; + HofSImag[NumSamples] = 0.0; + } + + // Do an inverse FFT on HofS to generate the impulse response. On return, HofSImag will be zero. + FFT(HofSReal, HofSImag, 2 * NumSamples, INVERSE); + + // We just generated an impulse response that is 2*NumSamples long. Since we used linear phase + // the response will be symmetric about the center. In general, we only need a small number + // of these taps, so we use the taps from the center of HofSReal, starting at StartJ. + // We also need to scale the FFT's output by the size of the FFT. + StartJ = NumSamples - NumTaps / 2; + for (j = 0; j < NumTaps; j++) + FirCoeff[j] = HofSReal[StartJ + j] / (2.0 * dNumSamples); + +} +//--------------------------------------------------------------------------- + +// This function shows how to sample an analog transfer function H(s) to generate an FIR filter. +// We didn't put much effort into this example because, in general, an analog prototype generates +// a rather poor FIR filter in the sense that it requires such a large number of taps to realize +// the response. For a discussion on this, see this article: +// http://iowahills.com/B2PolynomialFIRFilters.html +// In this example, we generate an FIR low pass from an Inverse Chebyshev low pass prototype. +// We sample the low pass prototype H(s) at frequencies determined by the bilinear transform. +void SampledFreqAnalog(int NumTaps, double *FirCoeff, double *HofSReal, + double *HofSImag, double OmegaC) { + int j, k, NumSamples; + double dNumSamples, Omega, Omega0; + std::complex s, H; + TFIRPassTypes PassType = firLPF; + NumSamples = NUM_POS_FREQ_SAMPLES; + dNumSamples = (double) NumSamples; + + // Limit test NumTaps + if (NumTaps > MAX_NUMTAPS) + NumTaps = MAX_NUMTAPS; + if (NumTaps > 2 * NUM_POS_FREQ_SAMPLES) + NumTaps = 2 * NUM_POS_FREQ_SAMPLES; + + // Define the low pass filter prototype + TLowPassParams LPFProto; // defined in LowPassPrototypes.h + TSPlaneCoeff SCoeff; // defined in LowPassPrototypes.h + LPFProto.ProtoType = INVERSE_CHEBY; // BUTTERWORTH, CHEBYSHEV, GAUSSIAN, BESSEL, ADJUSTABLE, INVERSE_CHEBY, PAPOULIS, ELLIPTIC (defined in LowPassPrototypes.h) + LPFProto.NumPoles = 8; // 1 <= NumPoles <= 12, 15, 20 Depending on the filter. + LPFProto.Ripple = 0.25; // 0.0 <= Ripple <= 1.0 dB Chebyshev and Elliptic (less for high order Chebyshev). + LPFProto.StopBanddB = 60.0; // 20 <= StopBand <= 120 dB Inv Cheby and Elliptic + LPFProto.Gamma = 0.0; // -1.0 <= Gamma <= 1.0 Adjustable Gauss Controls the transition BW. + + // Get the prototype filter's 2nd order s plane coefficients. + SCoeff = CalcLowPassProtoCoeff(LPFProto); + + // Evaluate the prototype's H(s) + Omega0 = 1.0 / tan(OmegaC * M_PI_2); // This sets the corner frequency. + for (j = 0; j < NumSamples; j++) { + Omega = Omega0 * tan(M_PI_2 * (double) j / dNumSamples); // Frequencies per the bilinear transform. + s = std::complex(0.0, Omega); + H = std::complex(1.0, 0.0); + for (k = 0; k < SCoeff.NumSections; k++) { + H *= SCoeff.N2[k] * s * s + SCoeff.N1[k] * s + SCoeff.N0[k]; // The numerator + H /= SCoeff.D2[k] * s * s + SCoeff.D1[k] * s + SCoeff.D0[k]; // The denominator + H *= SCoeff.D0[k] / SCoeff.N0[k]; // The gain constants. + } + HofSReal[j] = H.real(); // We need to do this for the FFT, which uses real arrays. + HofSImag[j] = H.imag(); + } + + // Fill the negative frequency bins of HofS with the conjugate of HofS for the FFT. + for (j = 1; j < NumSamples; j++) + HofSReal[2 * NumSamples - j] = HofSReal[j]; + for (j = 1; j < NumSamples; j++) + HofSImag[2 * NumSamples - j] = -HofSImag[j]; + + // The Fourier Transform requires the center freq bins to be 0 for LPF and BPF, 1 for HPF and Notch + if (PassType == firLPF || PassType == firBPF) { + HofSReal[NumSamples] = 0.0; + HofSImag[NumSamples] = 0.0; + } else { + HofSReal[NumSamples] = 1.0; + HofSImag[NumSamples] = 0.0; + } + + // Do an inverse FFT on HofS to generate the impulse response. On return, HofSImag will be zero. + FFT(HofSReal, HofSImag, 2 * NumSamples, INVERSE); + + // We just generated an impulse response that is 2*NumSamples long. We can use as many of these + // as we like, but if you look at HofSReal you will see that the tail of the response goes to + // zero rather quickly. The desired part of impulse response is at the beginning of HofSReal + // instead of the center because H(s) didn't have linear phase (more like minimum phase). + + // Take the taps from the start of HofSReal and scale by the size of the FFT. + for (j = 0; j < NumTaps; j++) + FirCoeff[j] = HofSReal[j] / (2.0 * dNumSamples); + + // Most FIR responses benefit from the application of a window to reduce the effects of + // truncating the impulse response. But a typical window, such as the Kaiser, can't be used + // because this impulse response isn't symmetric about the center tap. + // A Half Cosine window works quite nicely with these types of responses. + for (j = 0; j < NumTaps; j++) + FirCoeff[j] = FirCoeff[j] * cos(M_PI_2 * j / (double) NumTaps); +} + +//--------------------------------------------------------------------------- + +} // namespace diff --git a/kitiirfir/FreqSamplingCode.h b/kitiirfir/FreqSamplingCode.h new file mode 100644 index 000000000..28c1784b7 --- /dev/null +++ b/kitiirfir/FreqSamplingCode.h @@ -0,0 +1,17 @@ +//--------------------------------------------------------------------------- + +#ifndef FreqSamplingCodeH +#define FreqSamplingCodeH +#include "FIRFilterCode.h" // for the definition of TFIRPassTypes + +#define NUM_POS_FREQ_SAMPLES 1024 // needs to be 1024 for the kit test app + +namespace kitiirfir +{ + +void SampledFreqFIR(int NumTaps, double *FirCoeff, double *HofSReal, double *HofSImag, double OmegaC, TFIRPassTypes PassType); +void SampledFreqAnalog(int NumTaps, double *FirCoeff, double *HofSReal, double *HofSImag, double OmegaC); + +} // namespace + +#endif diff --git a/kitiirfir/IIRFilterCode.cpp b/kitiirfir/IIRFilterCode.cpp new file mode 100644 index 000000000..f700eaca0 --- /dev/null +++ b/kitiirfir/IIRFilterCode.cpp @@ -0,0 +1,526 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + May 1, 2016 + + ShowMessage is a C++ Builder function, and it usage has been commented out. + If you are using C++ Builder, include vcl.h for ShowMessage. + Otherwise replace ShowMessage with something appropriate for your compiler. + + See the FilterKitMain.cpp file for an example on how to use this code. + */ + +#include "IIRFilterCode.h" +#include "PFiftyOneRevE.h" +#include "LowPassPrototypes.h" +#include +#include + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- +/* + This calculates the coefficients for IIR filters from a set of 2nd order s plane coefficients + which are obtained by calling CalcLowPassProtoCoeff() in LowPassPrototypes.cpp. + The s plane filters are frequency scaled so their 3 dB frequency is at s = omega = 1 rad/sec. + The poles and zeros are also ordered in a manner appropriate for IIR filters. + For a derivation of the formulas used here, see the IIREquationDerivations.pdf + This shows how the various poly coefficients are defined. + H(s) = ( Ds^2 + Es + F ) / ( As^2 + Bs + C ) + H(z) = ( b2z^2 + b1z + b0 ) / ( a2z^2 + a1z + a0 ) + */ +TIIRCoeff CalcIIRFilterCoeff(TIIRFilterParams IIRFilt) { + int j, k; + double Scalar, SectionGain, Coeff[5]; + double A, B, C, D, E, F, T, Q, Arg; + double a2[ARRAY_DIM], a1[ARRAY_DIM], a0[ARRAY_DIM]; + double b2[ARRAY_DIM], b1[ARRAY_DIM], b0[ARRAY_DIM]; + std::complex Roots[5]; + + TIIRCoeff IIR; // Gets returned by this function. + TLowPassParams LowPassFilt; // Passed to the CalcLowPassProtoCoeff() function. + TSPlaneCoeff SPlaneCoeff; // Filled by the CalcLowPassProtoCoeff() function. + + // We can set the TLowPassParams variables directly from the TIIRFilterParams variables. + LowPassFilt.ProtoType = IIRFilt.ProtoType; + LowPassFilt.NumPoles = IIRFilt.NumPoles; + LowPassFilt.Ripple = IIRFilt.Ripple; + LowPassFilt.Gamma = IIRFilt.Gamma; + LowPassFilt.StopBanddB = IIRFilt.StopBanddB; + + // Get the low pass prototype 2nd order s plane coefficients. + SPlaneCoeff = CalcLowPassProtoCoeff(LowPassFilt); + + // Init the IIR structure. + for (j = 0; j < ARRAY_DIM; j++) { + IIR.a0[j] = 0.0; + IIR.b0[j] = 0.0; + IIR.a1[j] = 0.0; + IIR.b1[j] = 0.0; + IIR.a2[j] = 0.0; + IIR.b2[j] = 0.0; + IIR.a3[j] = 0.0; + IIR.b3[j] = 0.0; + IIR.a4[j] = 0.0; + IIR.b4[j] = 0.0; + } + + // Set the number of IIR filter sections we will be generating. + IIR.NumSections = (IIRFilt.NumPoles + 1) / 2; + if (IIRFilt.IIRPassType == iirBPF || IIRFilt.IIRPassType == iirNOTCH) + IIR.NumSections = IIRFilt.NumPoles; + + // For All Pass filters, the numerator is set to the denominator values as shown here. + // If the prototype was an Inv Cheby or Elliptic, the S plane numerator is discarded. + // Use the Gauss as the prototype for the best all pass results (most linear phase). + // The all pass H(s) = ( As^2 - Bs + C ) / ( As^2 + Bs + C ) + if (IIRFilt.IIRPassType == iirALLPASS) { + for (j = 0; j < SPlaneCoeff.NumSections; j++) { + SPlaneCoeff.N2[j] = SPlaneCoeff.D2[j]; + SPlaneCoeff.N1[j] = -SPlaneCoeff.D1[j]; + SPlaneCoeff.N0[j] = SPlaneCoeff.D0[j]; + } + } + + // T sets the IIR filter's corner frequency, or center freqency. + // The Bilinear transform is defined as: s = 2/T * tan(Omega/2) = 2/T * (1 - z)/(1 + z) + T = 2.0 * tan(IIRFilt.OmegaC * M_PI_2); + Q = 1.0 + IIRFilt.OmegaC; // Q is used for band pass and notch filters. + if (Q > 1.95) + Q = 1.95; + Q = 0.8 * tan(Q * M_PI_4); // This is a correction factor for Q. + Q = IIRFilt.OmegaC / IIRFilt.BW / Q; // This is the corrected Q. + + // Calc the IIR coefficients. + // SPlaneCoeff.NumSections is the number of 1st and 2nd order s plane factors. + k = 0; + for (j = 0; j < SPlaneCoeff.NumSections; j++) { + A = SPlaneCoeff.D2[j]; // We use A - F to make the code easier to read. + B = SPlaneCoeff.D1[j]; + C = SPlaneCoeff.D0[j]; + D = SPlaneCoeff.N2[j]; + E = SPlaneCoeff.N1[j]; // N1 is always zero, except for the all pass. Consequently, the equations below can be simplified a bit by removing E. + F = SPlaneCoeff.N0[j]; + + // b's are the numerator a's are the denominator + if (IIRFilt.IIRPassType == iirLPF || IIRFilt.IIRPassType == iirALLPASS) // Low Pass and All Pass + { + if (A == 0.0 && D == 0.0) // 1 pole case + { + Arg = (2.0 * B + C * T); + IIR.a2[j] = 0.0; + IIR.a1[j] = (-2.0 * B + C * T) / Arg; + IIR.a0[j] = 1.0; + + IIR.b2[j] = 0.0; + IIR.b1[j] = (-2.0 * E + F * T) / Arg * C / F; + IIR.b0[j] = (2.0 * E + F * T) / Arg * C / F; + } else // 2 poles + { + Arg = (4.0 * A + 2.0 * B * T + C * T * T); + IIR.a2[j] = (4.0 * A - 2.0 * B * T + C * T * T) / Arg; + IIR.a1[j] = (2.0 * C * T * T - 8.0 * A) / Arg; + IIR.a0[j] = 1.0; + + // With all pole filters, our LPF numerator is (z+1)^2, so all our Z Plane zeros are at -1 + IIR.b2[j] = (4.0 * D - 2.0 * E * T + F * T * T) / Arg * C / F; + IIR.b1[j] = (2.0 * F * T * T - 8.0 * D) / Arg * C / F; + IIR.b0[j] = (4 * D + F * T * T + 2.0 * E * T) / Arg * C / F; + } + } + + if (IIRFilt.IIRPassType == iirHPF) // High Pass + { + if (A == 0.0 && D == 0.0) // 1 pole + { + Arg = 2.0 * C + B * T; + IIR.a2[j] = 0.0; + IIR.a1[j] = (B * T - 2.0 * C) / Arg; + IIR.a0[j] = 1.0; + + IIR.b2[j] = 0.0; + IIR.b1[j] = (E * T - 2.0 * F) / Arg * C / F; + IIR.b0[j] = (E * T + 2.0 * F) / Arg * C / F; + } else // 2 poles + { + Arg = A * T * T + 4.0 * C + 2.0 * B * T; + IIR.a2[j] = (A * T * T + 4.0 * C - 2.0 * B * T) / Arg; + IIR.a1[j] = (2.0 * A * T * T - 8.0 * C) / Arg; + IIR.a0[j] = 1.0; + + // With all pole filters, our HPF numerator is (z-1)^2, so all our Z Plane zeros are at 1 + IIR.b2[j] = (D * T * T - 2.0 * E * T + 4.0 * F) / Arg * C / F; + IIR.b1[j] = (2.0 * D * T * T - 8.0 * F) / Arg * C / F; + IIR.b0[j] = (D * T * T + 4.0 * F + 2.0 * E * T) / Arg * C / F; + } + } + + if (IIRFilt.IIRPassType == iirBPF) // Band Pass + { + if (A == 0.0 && D == 0.0) // 1 pole + { + Arg = 4.0 * B * Q + 2.0 * C * T + B * Q * T * T; + a2[k] = (B * Q * T * T + 4.0 * B * Q - 2.0 * C * T) / Arg; + a1[k] = (2.0 * B * Q * T * T - 8.0 * B * Q) / Arg; + a0[k] = 1.0; + + b2[k] = (E * Q * T * T + 4.0 * E * Q - 2.0 * F * T) / Arg * C + / F; + b1[k] = (2.0 * E * Q * T * T - 8.0 * E * Q) / Arg * C / F; + b0[k] = (4.0 * E * Q + 2.0 * F * T + E * Q * T * T) / Arg * C + / F; + k++; + } else //2 Poles + { + IIR.a4[j] = (16.0 * A * Q * Q + A * Q * Q * T * T * T * T + + 8.0 * A * Q * Q * T * T - 2.0 * B * Q * T * T * T + - 8.0 * B * Q * T + 4.0 * C * T * T) * F; + IIR.a3[j] = (4.0 * T * T * T * T * A * Q * Q + - 4.0 * Q * T * T * T * B + 16.0 * Q * B * T + - 64.0 * A * Q * Q) * F; + IIR.a2[j] = (96.0 * A * Q * Q - 16.0 * A * Q * Q * T * T + + 6.0 * A * Q * Q * T * T * T * T - 8.0 * C * T * T) + * F; + IIR.a1[j] = (4.0 * T * T * T * T * A * Q * Q + + 4.0 * Q * T * T * T * B - 16.0 * Q * B * T + - 64.0 * A * Q * Q) * F; + IIR.a0[j] = (16.0 * A * Q * Q + A * Q * Q * T * T * T * T + + 8.0 * A * Q * Q * T * T + 2.0 * B * Q * T * T * T + + 8.0 * B * Q * T + 4.0 * C * T * T) * F; + + // With all pole filters, our BPF numerator is (z-1)^2 * (z+1)^2 so the zeros come back as +/- 1 pairs + IIR.b4[j] = (8.0 * D * Q * Q * T * T - 8.0 * E * Q * T + + 16.0 * D * Q * Q - 2.0 * E * Q * T * T * T + + D * Q * Q * T * T * T * T + 4.0 * F * T * T) * C; + IIR.b3[j] = (16.0 * E * Q * T - 4.0 * E * Q * T * T * T + - 64.0 * D * Q * Q + 4.0 * D * Q * Q * T * T * T * T) + * C; + IIR.b2[j] = (96.0 * D * Q * Q - 8.0 * F * T * T + + 6.0 * D * Q * Q * T * T * T * T + - 16.0 * D * Q * Q * T * T) * C; + IIR.b1[j] = (4.0 * D * Q * Q * T * T * T * T - 64.0 * D * Q * Q + + 4.0 * E * Q * T * T * T - 16.0 * E * Q * T) * C; + IIR.b0[j] = (16.0 * D * Q * Q + 8.0 * E * Q * T + + 8.0 * D * Q * Q * T * T + 2.0 * E * Q * T * T * T + + 4.0 * F * T * T + D * Q * Q * T * T * T * T) * C; + + // T = 2 makes these values approach 0.0 (~ 1.0E-12) The root solver needs 0.0 for numerical reasons. + if (fabs(T - 2.0) < 0.0005) { + IIR.a3[j] = 0.0; + IIR.a1[j] = 0.0; + IIR.b3[j] = 0.0; + IIR.b1[j] = 0.0; + } + + // We now have a 4th order poly in the form a4*s^4 + a3*s^3 + a2*s^2 + a2*s + a0 + // We find the roots of this so we can form two 2nd order polys. + Coeff[0] = IIR.a4[j]; + Coeff[1] = IIR.a3[j]; + Coeff[2] = IIR.a2[j]; + Coeff[3] = IIR.a1[j]; + Coeff[4] = IIR.a0[j]; + FindRoots(4, Coeff, Roots); + + // In effect, the root finder scales the poly by 1/a4 so we have to apply this factor back into + // the two 2nd order equations we are forming. + Scalar = sqrt(fabs(IIR.a4[j])); + + // Form the two 2nd order polys from the roots. + a2[k] = Scalar; + a1[k] = -(Roots[0] + Roots[1]).real() * Scalar; + a0[k] = (Roots[0] * Roots[1]).real() * Scalar; + k++; + a2[k] = Scalar; + a1[k] = -(Roots[2] + Roots[3]).real() * Scalar; + a0[k] = (Roots[2] * Roots[3]).real() * Scalar; + k--; + + // Now do the same with the numerator. + Coeff[0] = IIR.b4[j]; + Coeff[1] = IIR.b3[j]; + Coeff[2] = IIR.b2[j]; + Coeff[3] = IIR.b1[j]; + Coeff[4] = IIR.b0[j]; + + if (IIRFilt.ProtoType == INVERSE_CHEBY + || IIRFilt.ProtoType == ELLIPTIC) { + FindRoots(4, Coeff, Roots); + } else // With all pole filters (Butter, Cheb, etc), we know we have these 4 real roots. The root finder won't necessarily pair real roots the way we need, so rather than compute these, we simply set them. + { + Roots[0] = std::complex(-1.0, 0.0); + Roots[1] = std::complex(1.0, 0.0); + Roots[2] = std::complex(-1.0, 0.0); + Roots[3] = std::complex(1.0, 0.0); + } + + Scalar = sqrt(fabs(IIR.b4[j])); + + b2[k] = Scalar; + if (IIRFilt.ProtoType == INVERSE_CHEBY + || IIRFilt.ProtoType == ELLIPTIC) { + b1[k] = -(Roots[0] + Roots[1]).real() * Scalar; // = 0.0 + } else // else the prototype is an all pole filter + { + b1[k] = 0.0; // b1 = 0 for all pole filters, but the addition above won't always equal zero exactly. + } + b0[k] = (Roots[0] * Roots[1]).real() * Scalar; + + k++; + + b2[k] = Scalar; + if (IIRFilt.ProtoType == INVERSE_CHEBY + || IIRFilt.ProtoType == ELLIPTIC) { + b1[k] = -(Roots[2] + Roots[3]).real() * Scalar; + } else // All pole + { + b1[k] = 0.0; + } + b0[k] = (Roots[2] * Roots[3]).real() * Scalar; + k++; + // Go below to see where we store these 2nd order polys back into IIR + } + } + + if (IIRFilt.IIRPassType == iirNOTCH) // Notch + { + if (A == 0.0 && D == 0.0) // 1 pole + { + Arg = 2.0 * B * T + C * Q * T * T + 4.0 * C * Q; + a2[k] = (4.0 * C * Q - 2.0 * B * T + C * Q * T * T) / Arg; + a1[k] = (2.0 * C * Q * T * T - 8.0 * C * Q) / Arg; + a0[k] = 1.0; + + b2[k] = (4.0 * F * Q - 2.0 * E * T + F * Q * T * T) / Arg * C + / F; + b1[k] = (2.0 * F * Q * T * T - 8.0 * F * Q) / Arg * C / F; + b0[k] = (2.0 * E * T + F * Q * T * T + 4.0 * F * Q) / Arg * C + / F; + k++; + } else { + IIR.a4[j] = (4.0 * A * T * T - 2.0 * B * T * T * T * Q + + 8.0 * C * Q * Q * T * T - 8.0 * B * T * Q + + C * Q * Q * T * T * T * T + 16.0 * C * Q * Q) * -F; + IIR.a3[j] = (16.0 * B * T * Q + 4.0 * C * Q * Q * T * T * T * T + - 64.0 * C * Q * Q - 4.0 * B * T * T * T * Q) * -F; + IIR.a2[j] = (96.0 * C * Q * Q - 8.0 * A * T * T + - 16.0 * C * Q * Q * T * T + + 6.0 * C * Q * Q * T * T * T * T) * -F; + IIR.a1[j] = (4.0 * B * T * T * T * Q - 16.0 * B * T * Q + - 64.0 * C * Q * Q + 4.0 * C * Q * Q * T * T * T * T) + * -F; + IIR.a0[j] = (4.0 * A * T * T + 2.0 * B * T * T * T * Q + + 8.0 * C * Q * Q * T * T + 8.0 * B * T * Q + + C * Q * Q * T * T * T * T + 16.0 * C * Q * Q) * -F; + + // Our Notch Numerator isn't simple. [ (4+T^2)*z^2 - 2*(4-T^2)*z + (4+T^2) ]^2 + IIR.b4[j] = (2.0 * E * T * T * T * Q - 4.0 * D * T * T + - 8.0 * F * Q * Q * T * T + 8.0 * E * T * Q + - 16.0 * F * Q * Q - F * Q * Q * T * T * T * T) * C; + IIR.b3[j] = (64.0 * F * Q * Q + 4.0 * E * T * T * T * Q + - 16.0 * E * T * Q - 4.0 * F * Q * Q * T * T * T * T) + * C; + IIR.b2[j] = (8.0 * D * T * T - 96.0 * F * Q * Q + + 16.0 * F * Q * Q * T * T + - 6.0 * F * Q * Q * T * T * T * T) * C; + IIR.b1[j] = (16.0 * E * T * Q - 4.0 * E * T * T * T * Q + + 64.0 * F * Q * Q - 4.0 * F * Q * Q * T * T * T * T) + * C; + IIR.b0[j] = (-4.0 * D * T * T - 2.0 * E * T * T * T * Q + - 8.0 * E * T * Q - 8.0 * F * Q * Q * T * T + - F * Q * Q * T * T * T * T - 16.0 * F * Q * Q) * C; + + // T = 2 (OmegaC = 0.5) makes these values approach 0.0 (~ 1.0E-12). The root solver wants 0.0 for numerical reasons. + if (fabs(T - 2.0) < 0.0005) { + IIR.a3[j] = 0.0; + IIR.a1[j] = 0.0; + IIR.b3[j] = 0.0; + IIR.b1[j] = 0.0; + } + + // We now have a 4th order poly in the form a4*s^4 + a3*s^3 + a2*s^2 + a2*s + a0 + // We find the roots of this so we can form two 2nd order polys. + Coeff[0] = IIR.a4[j]; + Coeff[1] = IIR.a3[j]; + Coeff[2] = IIR.a2[j]; + Coeff[3] = IIR.a1[j]; + Coeff[4] = IIR.a0[j]; + + // In effect, the root finder scales the poly by 1/a4 so we have to apply this factor back into + // the two 2nd order equations we are forming. + FindRoots(4, Coeff, Roots); + Scalar = sqrt(fabs(IIR.a4[j])); + a2[k] = Scalar; + a1[k] = -(Roots[0] + Roots[1]).real() * Scalar; + a0[k] = (Roots[0] * Roots[1]).real() * Scalar; + + k++; + a2[k] = Scalar; + a1[k] = -(Roots[2] + Roots[3]).real() * Scalar; + a0[k] = (Roots[2] * Roots[3]).real() * Scalar; + k--; + + // Now do the same with the numerator. + Coeff[0] = IIR.b4[j]; + Coeff[1] = IIR.b3[j]; + Coeff[2] = IIR.b2[j]; + Coeff[3] = IIR.b1[j]; + Coeff[4] = IIR.b0[j]; + FindRoots(4, Coeff, Roots); + + Scalar = sqrt(fabs(IIR.b4[j])); + b2[k] = Scalar; + b1[k] = -(Roots[0] + Roots[1]).real() * Scalar; + b0[k] = (Roots[0] * Roots[1]).real() * Scalar; + + k++; + b2[k] = Scalar; + b1[k] = -(Roots[2] + Roots[3]).real() * Scalar; + b0[k] = (Roots[2] * Roots[3]).real() * Scalar; + k++; + } + } + } + + if (IIRFilt.IIRPassType == iirBPF || IIRFilt.IIRPassType == iirNOTCH) { + // In the calcs above for the BPF and Notch, we didn't set a0=1, so we do it here. + for (j = 0; j < IIR.NumSections; j++) { + b2[j] /= a0[j]; + b1[j] /= a0[j]; + b0[j] /= a0[j]; + a2[j] /= a0[j]; + a1[j] /= a0[j]; + a0[j] = 1.0; + } + + for (j = 0; j < IIR.NumSections; j++) { + IIR.a0[j] = a0[j]; + IIR.a1[j] = a1[j]; + IIR.a2[j] = a2[j]; + IIR.b0[j] = b0[j]; + IIR.b1[j] = b1[j]; + IIR.b2[j] = b2[j]; + } + } + + // Adjust the b's or a0 for the desired Gain. + SectionGain = pow(10.0, IIRFilt.dBGain / 20.0); + SectionGain = pow(SectionGain, 1.0 / (double) IIR.NumSections); + for (j = 0; j < IIR.NumSections; j++) { + IIR.b0[j] *= SectionGain; + IIR.b1[j] *= SectionGain; + IIR.b2[j] *= SectionGain; + // This is an alternative to adjusting the b's + // IIR.a0[j] = SectionGain; + } + + return (IIR); +} + +//--------------------------------------------------------------------------- + +// This code implements an IIR filter as a Form 1 Biquad. +// It uses 2 sets of shift registers, RegX on the input side and RegY on the output side. +// There are many ways to implement an IIR filter, some very good, and some extremely bad. +// For numerical reasons, a Form 1 Biquad implementation is among the best. +void FilterWithIIR(TIIRCoeff IIRCoeff, double *Signal, double *FilteredSignal, + int NumSigPts) { + double y; + int j, k; + + for (j = 0; j < NumSigPts; j++) { + k = 0; + y = SectCalc(j, k, Signal[j], IIRCoeff); + for (k = 1; k < IIRCoeff.NumSections; k++) { + y = SectCalc(j, k, y, IIRCoeff); + } + FilteredSignal[j] = y; + } + +} +//--------------------------------------------------------------------------- + +// This gets used with the function above, FilterWithIIR() +// Note the use of MaxRegVal to avoid a math overflow condition. +double SectCalc(int j, int k, double x, TIIRCoeff IIRCoeff) { + double y, CenterTap; + static double RegX1[ARRAY_DIM], RegX2[ARRAY_DIM], RegY1[ARRAY_DIM], + RegY2[ARRAY_DIM], MaxRegVal; + static bool MessageShown = false; + + // Zero the regiisters on the 1st call or on an overflow condition. The overflow limit used + // here is small for double variables, but a filter that reaches this threshold is broken. + if ((j == 0 && k == 0) || MaxRegVal > OVERFLOW_LIMIT) { + if (MaxRegVal > OVERFLOW_LIMIT && !MessageShown) { + // ShowMessage("ERROR: Math Over Flow in IIR Section Calc. \nThe register values exceeded 1.0E20 \n"); + MessageShown = true; // So this message doesn't get shown thousands of times. + } + + MaxRegVal = 1.0E-12; + for (int i = 0; i < ARRAY_DIM; i++) { + RegX1[i] = 0.0; + RegX2[i] = 0.0; + RegY1[i] = 0.0; + RegY2[i] = 0.0; + } + } + + CenterTap = x * IIRCoeff.b0[k] + IIRCoeff.b1[k] * RegX1[k] + + IIRCoeff.b2[k] * RegX2[k]; + y = IIRCoeff.a0[k] * CenterTap - IIRCoeff.a1[k] * RegY1[k] + - IIRCoeff.a2[k] * RegY2[k]; + + RegX2[k] = RegX1[k]; + RegX1[k] = x; + RegY2[k] = RegY1[k]; + RegY1[k] = y; + + // MaxRegVal is used to prevent overflow. Overflow seldom occurs, but will + // if the filter has faulty coefficients. MaxRegVal is usually less than 100.0 + if (fabs(CenterTap) > MaxRegVal) + MaxRegVal = fabs(CenterTap); + if (fabs(y) > MaxRegVal) + MaxRegVal = fabs(y); + return (y); +} +//--------------------------------------------------------------------------- + +// This function calculates the frequency response of an IIR filter. +// Probably the easiest way to determine the frequency response of an IIR filter is to send +// an impulse through the filter and do an FFT on the output. This method does a DFT on +// the coefficients of each biquad section. The results from the cascaded sections are +// then multiplied together. + +// This approach works better than an FFT when the filter is very narrow. To analyze highly selective +// filters with an FFT can require a very large number of points, which can be quite cumbersome. +// This approach allows you to set the range of frequencies to be analyzed by modifying the statement +// Arg = M_PI * (double)j / (double)NumPts; . +void IIRFreqResponse(TIIRCoeff IIR, int NumSections, double *RealHofZ, + double *ImagHofZ, int NumPts) { + int j, n; + double Arg; + std::complex z1, z2, HofZ, Denom; + for (j = 0; j < NumPts; j++) { + Arg = M_PI * (double) j / (double) NumPts; + z1 = std::complex(cos(Arg), -sin(Arg)); // z = e^(j*omega) + z2 = z1 * z1; // z squared + + HofZ = std::complex(1.0, 0.0); + for (n = 0; n < NumSections; n++) { + HofZ *= IIR.a0[n]; // This can be in the denominator, but only if a0=1. a0 can be other than 1.0 to adjust the filter's gain. See the bottom of the CalcIIRFilterCoeff() function. + HofZ *= IIR.b0[n] + IIR.b1[n] * z1 + IIR.b2[n] * z2; // Numerator + Denom = 1.0 + IIR.a1[n] * z1 + IIR.a2[n] * z2; // Denominator + if (std::abs(Denom) < 1.0E-12) + Denom = 1.0E-12; // A pole on the unit circle would cause this to be zero, so this should never happen. It could happen however if the filter also has a zero at this frequency. Then H(z) needs to be determined by L'Hopitals rule at this freq. + HofZ /= Denom; + } + RealHofZ[j] = HofZ.real(); + ImagHofZ[j] = HofZ.imag(); + } +} + +//--------------------------------------------------------------------------- + +} // namespace diff --git a/kitiirfir/IIRFilterCode.h b/kitiirfir/IIRFilterCode.h new file mode 100644 index 000000000..d917ead22 --- /dev/null +++ b/kitiirfir/IIRFilterCode.h @@ -0,0 +1,38 @@ +//--------------------------------------------------------------------------- + +#ifndef IIRFilterCodeH +#define IIRFilterCodeH +//--------------------------------------------------------------------------- +#include "LowPassPrototypes.h" // defines TFilterPoly and ARRAY_DIM +#define OVERFLOW_LIMIT 1.0E20 + +namespace kitiirfir +{ + +enum TIIRPassTypes {iirLPF, iirHPF, iirBPF, iirNOTCH, iirALLPASS}; + +struct TIIRCoeff {double a0[ARRAY_DIM]; double a1[ARRAY_DIM]; double a2[ARRAY_DIM]; double a3[ARRAY_DIM]; double a4[ARRAY_DIM]; + double b0[ARRAY_DIM]; double b1[ARRAY_DIM]; double b2[ARRAY_DIM]; double b3[ARRAY_DIM]; double b4[ARRAY_DIM]; + int NumSections; }; + +struct TIIRFilterParams { TIIRPassTypes IIRPassType; // Defined above: Low pass, High Pass, etc. + double OmegaC; // The IIR filter's 3 dB corner freq for low pass and high pass, the center freq for band pass and notch. + double BW; // The IIR filter's 3 dB bandwidth for band pass and notch filters. + double dBGain; // Sets the Gain of the filter + + // These define the low pass prototype to be used + TFilterPoly ProtoType; // Butterworth, Cheby, etc. + int NumPoles; // Pole count + double Ripple; // Passband Ripple for the Elliptic and Chebyshev + double StopBanddB; // Stop Band Attenuation in dB for the Elliptic and Inverse Chebyshev + double Gamma; // Controls the transition bandwidth on the Adjustable Gauss. -1 <= Gamma <= 1 + }; + +TIIRCoeff CalcIIRFilterCoeff(TIIRFilterParams IIRFilt); +void FilterWithIIR(TIIRCoeff IIRCoeff, double *Signal, double *FilteredSignal, int NumSigPts); +double SectCalc(int j, int k, double x, TIIRCoeff IIRCoeff); +void IIRFreqResponse(TIIRCoeff IIRCoeff, int NumSections, double *RealHofZ, double *ImagHofZ, int NumPts); + +} // namespace + +#endif diff --git a/kitiirfir/LowPassPrototypes.cpp b/kitiirfir/LowPassPrototypes.cpp new file mode 100644 index 000000000..084351e9f --- /dev/null +++ b/kitiirfir/LowPassPrototypes.cpp @@ -0,0 +1,469 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + May 1, 2016 + + ShowMessage is a C++ Builder function, and it usage has been commented out. + If you are using C++ Builder, include vcl.h for ShowMessage. + Otherwise replace ShowMessage with something appropriate for your compiler. + + This code generates 2nd order S plane coefficients for the following low pass filter prototypes. + Butterworth, Chebyshev, Bessel, Gauss, Adjustable Gauss, Inverse Chebyshev, Papoulis, and Elliptic. + This code does 3 things. + 1. Gets the filter's roots from the code in LowPassRoots.cpp + 2. Uses the left hand plane roots to form 2nd order polynomials. + 3. Frequency scales the polynomials so the 3 dB corner frequency is at omege = 1 rad/sec. + + Note that we range check the Filt struct variables here, but we don't report + back any changes we make. + */ + +#include +#include "LowPassPrototypes.h" +#include "PFiftyOneRevE.h" +#include "LowPassRoots.h" + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- + +// TLowPassParams defines the low pass prototype (NumPoles, Ripple, etc.). +// We return SPlaneCoeff filled with the 2nd order S plane coefficients. +TSPlaneCoeff CalcLowPassProtoCoeff(TLowPassParams Filt) { + int j, DenomCount = 0, NumerCount, NumRoots, ZeroCount; + std::complex Poles[ARRAY_DIM], Zeros[ARRAY_DIM]; + TSPlaneCoeff Coeff; // The return value. + + // Init the S Plane Coeff. H(s) = (N2*s^2 + N1*s + N0) / (D2*s^2 + D1*s + D0) + for (j = 0; j < ARRAY_DIM; j++) { + Coeff.N2[j] = 0.0; + Coeff.N1[j] = 0.0; + Coeff.N0[j] = 1.0; + Coeff.D2[j] = 0.0; + Coeff.D1[j] = 0.0; + Coeff.D0[j] = 1.0; + } + Coeff.NumSections = 0; + + // We need to range check the various argument values here. + // These are the practical limits the max number of poles. + if (Filt.NumPoles < 1) + Filt.NumPoles = 1; + if (Filt.NumPoles > MAX_POLE_COUNT) + Filt.NumPoles = MAX_POLE_COUNT; + if (Filt.ProtoType == ELLIPTIC || Filt.ProtoType == INVERSE_CHEBY) { + if (Filt.NumPoles > 15) + Filt.NumPoles = 15; + } + if (Filt.ProtoType == GAUSSIAN || Filt.ProtoType == BESSEL) { + if (Filt.NumPoles > 12) + Filt.NumPoles = 12; + } + + // Gamma is used by the Adjustable Gauss. + if (Filt.Gamma < -1.0) + Filt.Gamma = -1.0; // -1 gives ~ Gauss response + if (Filt.Gamma > 1.0) + Filt.Gamma = 1.0; // +1 gives ~ Butterworth response. + + // Ripple is used by the Chebyshev and Elliptic + if (Filt.Ripple < 0.0001) + Filt.Ripple = 0.0001; + if (Filt.Ripple > 1.0) + Filt.Ripple = 1.0; + + // With the Chebyshev we need to use less ripple for large pole counts to keep the poles out of the RHP. + if (Filt.ProtoType == CHEBYSHEV && Filt.NumPoles > 15) { + double MaxRipple = 1.0; + if (Filt.NumPoles == 16) + MaxRipple = 0.5; + if (Filt.NumPoles == 17) + MaxRipple = 0.4; + if (Filt.NumPoles == 18) + MaxRipple = 0.25; + if (Filt.NumPoles == 19) + MaxRipple = 0.125; + if (Filt.NumPoles >= 20) + MaxRipple = 0.10; + if (Filt.Ripple > MaxRipple) + Filt.Ripple = MaxRipple; + } + + // StopBanddB is used by the Inverse Chebyshev and the Elliptic + // It is given in positive dB values. + if (Filt.StopBanddB < 20.0) + Filt.StopBanddB = 20.0; + if (Filt.StopBanddB > 120.0) + Filt.StopBanddB = 120.0; + + // There isn't such a thing as a 1 pole Chebyshev, or 1 pole Bessel, etc. + // A one pole filter is simply 1/(s+1). + NumerCount = 0; // init + if (Filt.NumPoles == 1) { + Coeff.D1[0] = 1.0; + DenomCount = 1; // DenomCount is the number of denominator factors (1st or 2nd order). + } else if (Filt.ProtoType == BUTTERWORTH) { + NumRoots = ButterworthPoly(Filt.NumPoles, Poles); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + // A Butterworth doesn't require frequncy scaling with SetCornerFreq(). + } + + else if (Filt.ProtoType == ADJUSTABLE) // Adjustable Gauss + { + NumRoots = AdjustablePoly(Filt.NumPoles, Poles, Filt.Gamma); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + else if (Filt.ProtoType == CHEBYSHEV) { + NumRoots = ChebyshevPoly(Filt.NumPoles, Filt.Ripple, Poles); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + else if (Filt.ProtoType == INVERSE_CHEBY) { + NumRoots = InvChebyPoly(Filt.NumPoles, Filt.StopBanddB, Poles, Zeros, + &ZeroCount); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + NumerCount = GetFilterCoeff(ZeroCount, Zeros, Coeff.N2, Coeff.N1, + Coeff.N0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + else if (Filt.ProtoType == ELLIPTIC) { + NumRoots = EllipticPoly(Filt.NumPoles, Filt.Ripple, Filt.StopBanddB, + Poles, Zeros, &ZeroCount); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + NumerCount = GetFilterCoeff(ZeroCount, Zeros, Coeff.N2, Coeff.N1, + Coeff.N0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + // Papoulis works OK, but it doesn't accomplish anything the Chebyshev can't. + else if (Filt.ProtoType == PAPOULIS) { + NumRoots = PapoulisPoly(Filt.NumPoles, Poles); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + else if (Filt.ProtoType == BESSEL) { + NumRoots = BesselPoly(Filt.NumPoles, Poles); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + else if (Filt.ProtoType == GAUSSIAN) { + NumRoots = GaussianPoly(Filt.NumPoles, Poles); + DenomCount = GetFilterCoeff(NumRoots, Poles, Coeff.D2, Coeff.D1, + Coeff.D0); + SetCornerFreq(DenomCount, Coeff.D2, Coeff.D1, Coeff.D0, Coeff.N2, + Coeff.N1, Coeff.N0); + } + + Coeff.NumSections = DenomCount; + + // If we have an odd pole count, there will be 1 less zero than poles, so we need to shift the + // zeros down in the arrays so the 1st zero (which is zero) and aligns with the real pole. + if (NumerCount != 0 && Filt.NumPoles % 2 == 1) { + for (j = NumerCount; j >= 0; j--) { + Coeff.N2[j + 1] = Coeff.N2[j]; // Coeff.N1's are always zero + Coeff.N0[j + 1] = Coeff.N0[j]; + } + Coeff.N2[0] = 0.0; // Set the 1st zero to zero for odd pole counts. + Coeff.N0[0] = 1.0; + } + + return (Coeff); + +} + +//--------------------------------------------------------------------------- + +// This sets the polynomial's the 3 dB corner to 1 rad/sec. This isn' required for a Butterworth, +// but the rest of the polynomials need correction. Esp the Adj Gauss, Inv Cheby and Bessel. +// Poly Count is the number of 2nd order sections. D and N are the Denom and Num coefficients. +// H(s) = (N2*s^2 + N1*s + N0) / (D2*s^2 + D1*s + D0) +void SetCornerFreq(int PolyCount, double *D2, double *D1, double *D0, + double *N2, double *N1, double *N0) { + int j, n; + double Omega, FreqScalar, Zeta, Gain; + std::complex s, H; + + Gain = 1.0; + for (j = 0; j < PolyCount; j++) + Gain *= D0[j] / N0[j]; + + // Evaluate H(s) by increasing Omega until |H(s)| < -3 dB + for (j = 1; j < 6000; j++) { + Omega = (double) j / 512.0; // The step size for Omega is 1/512 radians. + s = std::complex(0.0, Omega); + + H = std::complex(1.0, 0.0); + for (n = 0; n < PolyCount; n++) { + H = H * (N2[n] * s * s + N1[n] * s + N0[n]) + / (D2[n] * s * s + D1[n] * s + D0[n]); + } + H *= Gain; + if (std::abs(H) < 0.7071) + break; // -3 dB + } + + FreqScalar = 1.0 / Omega; + + // Freq scale the denominator. We hold the damping factor Zeta constant. + for (j = 0; j < PolyCount; j++) { + Omega = sqrt(D0[j]); + if (Omega == 0.0) + continue; // should never happen + Zeta = D1[j] / Omega / 2.0; + if (D2[j] != 0.0) // 2nd degree poly + { + D0[j] = Omega * Omega * FreqScalar * FreqScalar; + D1[j] = 2.0 * Zeta * Omega * FreqScalar; + } else // 1st degree poly + { + D0[j] *= FreqScalar; + } + } + + // Scale the numerator. H(s) = (N2*s^2 + N1*s + N0) / (D2*s^2 + D1*s + D0) + // N1 is always zero. N2 is either 1 or 0. If N2 = 0, then N0 = 1 and there isn't a zero to scale. + // For all pole filters (Butter, Cheby, etc) N2 = 0 and N0 = 1. + for (j = 0; j < PolyCount; j++) { + if (N2[j] == 0.0) + continue; + N0[j] *= FreqScalar * FreqScalar; + } + +} + +//--------------------------------------------------------------------------- + +// Some of the Polys generate both left hand and right hand plane roots. +// We use this function to get the left hand plane poles and imag axis zeros to +// create the 2nd order polynomials with coefficients A2, A1, A0. +// We return the Polynomial count. + +// We first sort the roots according the the real part (a zeta sort). Then all the left +// hand plane roots are grouped and in the correct order for IIR and Opamp filters. +// We then check for duplicate roots, and set an inconsequential real or imag part to zero. +// Then the 2nd order coefficients are calculated. +int GetFilterCoeff(int RootCount, std::complex *Roots, double *A2, double *A1, + double *A0) { + int PolyCount, j, k; + + SortRootsByZeta(Roots, RootCount, stMin); // stMin places the most negative real part 1st. + + // Check for duplicate roots. The Inv Cheby generates duplcate imag roots, and the + // Elliptic generates duplicate real roots. We set duplicates to a RHP value. + for (j = 0; j < RootCount - 1; j++) { + for (k = j + 1; k < RootCount; k++) { + if (fabs(Roots[j].real() - Roots[k].real()) < 1.0E-3 + && fabs(Roots[j].imag() - Roots[k].imag()) < 1.0E-3) { + Roots[k] = std::complex((double) k, 0.0); // RHP roots are ignored below, Use k is to prevent duplicate checks for matches. + } + } + } + + // This forms the 2nd order coefficients from the root value. + // We ignore roots in the Right Hand Plane. + PolyCount = 0; + for (j = 0; j < RootCount; j++) { + if (Roots[j].real() > 0.0) + continue; // Right Hand Plane + if (Roots[j].real() == 0.0 && Roots[j].imag() == 0.0) + continue; // At the origin. This should never happen. + + if (Roots[j].real() == 0.0) // Imag Root (A poly zero) + { + A2[PolyCount] = 1.0; + A1[PolyCount] = 0.0; + A0[PolyCount] = Roots[j].imag() * Roots[j].imag(); + j++; + PolyCount++; + } else if (Roots[j].imag() == 0.0) // Real Pole + { + A2[PolyCount] = 0.0; + A1[PolyCount] = 1.0; + A0[PolyCount] = -Roots[j].real(); + PolyCount++; + } else // Complex Pole + { + A2[PolyCount] = 1.0; + A1[PolyCount] = -2.0 * Roots[j].real(); + A0[PolyCount] = Roots[j].real() * Roots[j].real() + + Roots[j].imag() * Roots[j].imag(); + j++; + PolyCount++; + } + } + + return (PolyCount); + +} + +//--------------------------------------------------------------------------- + +// This rebuilds an Nth order poly from its 2nd order constituents. +// PolyCount is the number of 2nd order polys. e.g. NumPoles = 7 PolyCount = 4 +// PolyCoeff gets filled such that PolyCoeff[5]*s^5 + PolyCoeff[4]*s^4 + PolyCoeff[3]*s^3 + .. +// The poly order is returned. +int RebuildPoly(int PolyCount, double *PolyCoeff, double *A2, double *A1, + double *A0) { + int j, k, n; + double Sum[P51_ARRAY_SIZE]; + for (j = 0; j <= 2 * PolyCount; j++) + PolyCoeff[j] = 0.0; + for (j = 0; j < P51_ARRAY_SIZE; j++) + Sum[j] = 0.0; + + PolyCoeff[2] = A2[0]; + PolyCoeff[1] = A1[0]; + PolyCoeff[0] = A0[0]; + + for (j = 1; j < PolyCount; j++) { + for (n = 0; n <= 2 * j; n++) { + Sum[n + 2] += PolyCoeff[n] * A2[j]; + Sum[n + 1] += PolyCoeff[n] * A1[j]; + Sum[n + 0] += PolyCoeff[n] * A0[j]; + } + for (k = 0; k <= 2 * j + 2; k++) + PolyCoeff[k] = Sum[k]; + for (k = 0; k < P51_ARRAY_SIZE; k++) + Sum[k] = 0.0; + } + + // Want to return the poly order. This will be 2 * PolyCount if there aren't any 1st order polys. + // 1st order Polys create leading zeros. N 1st order polys Gives N leading zeros. + for (j = 2 * PolyCount; j >= 0; j--) + if (PolyCoeff[j] != 0.0) + break; + return (j); +} + +//--------------------------------------------------------------------------- +// This sorts on the real part if the real part of the 1st root != 0 (a Zeta sort) +// else we sort on the imag part. If SortType == stMin for both the poles and zeros, then +// the poles and zeros will be properly matched. +// This also sets an inconsequential real or imag part to zero. +// A matched pair of z plane real roots, such as +/- 1, don't come out together. +// Used above in GetFilterCoeff and the FIR zero plot. +void SortRootsByZeta(std::complex *Roots, int Count, TOurSortTypes SortType) { + if (Count >= P51_MAXDEGREE) { + //ShowMessage("Count > P51_MAXDEGREE in TPolyForm::SortRootsByZeta()"); + return; + } + + int j, k, RootJ[P51_ARRAY_SIZE]; + double SortValue[P51_ARRAY_SIZE]; + std::complex TempRoots[P51_ARRAY_SIZE]; + + // Set an inconsequential real or imag part to zero. + for (j = 0; j < Count; j++) { + if (fabs(Roots[j].real()) * 1.0E3 < fabs(Roots[j].imag())) + Roots[j].real(0.0); + if (fabs(Roots[j].imag()) * 1.0E3 < fabs(Roots[j].real())) + Roots[j].imag(0.0); + } + + // Sort the roots. + for (j = 0; j < Count; j++) + RootJ[j] = j; // Needed for HeapIndexSort + if (Roots[0].real() != 0.0) // Cplx roots + { + for (j = 0; j < Count; j++) + SortValue[j] = Roots[j].real(); + } else // Imag roots, so we sort on imag part. + { + for (j = 0; j < Count; j++) + SortValue[j] = fabs(Roots[j].imag()); + } + HeapIndexSort(SortValue, RootJ, Count, SortType); // stMin gives the most negative root on top + + for (j = 0; j < Count; j++) { + k = RootJ[j]; // RootJ is the sort index + TempRoots[j] = Roots[k]; + } + for (j = 0; j < Count; j++) { + Roots[j] = TempRoots[j]; + } + +} +//--------------------------------------------------------------------------- + +// Remember to set the Index array to 0, 1, 2, 3, ... N-1 +bool HeapIndexSort(double *Data, int *Index, int N, TOurSortTypes SortType) { + int i, j, k, m, IndexTemp; + long long FailSafe, NSquared; // need this for big sorts + + NSquared = (long long) N * (long long) N; + m = N / 2; + k = N - 1; + for (FailSafe = 0; FailSafe < NSquared; FailSafe++) // typical FailSafe value on return is N*log2(N) + { + if (m > 0) + IndexTemp = Index[--m]; + else { + IndexTemp = Index[k]; + Index[k] = Index[0]; + if (--k == 0) { + Index[0] = IndexTemp; + return (true); + } + } + + i = m + 1; + j = 2 * i; + + if (SortType == stMax) + while (j < k + 2) { + FailSafe++; + if (j <= k && Data[Index[j - 1]] > Data[Index[j]]) + j++; + if (Data[IndexTemp] > Data[Index[j - 1]]) { + Index[i - 1] = Index[j - 1]; + i = j; + j += i; + } else + break; + } + + else + // SortType == stMin + while (j < k + 2) { + FailSafe++; + if (j <= k && Data[Index[j - 1]] < Data[Index[j]]) + j++; + if (Data[IndexTemp] < Data[Index[j - 1]]) { + Index[i - 1] = Index[j - 1]; + i = j; + j += i; + } else + break; + } + + Index[i - 1] = IndexTemp; + } + return (false); +} + +//---------------------------------------------------------------------------------- + +} // namespace + diff --git a/kitiirfir/LowPassPrototypes.h b/kitiirfir/LowPassPrototypes.h new file mode 100644 index 000000000..d463f8e38 --- /dev/null +++ b/kitiirfir/LowPassPrototypes.h @@ -0,0 +1,45 @@ +//--------------------------------------------------------------------------- + +#ifndef LowPassPrototypesH +#define LowPassPrototypesH + +#include + +#define MAX_POLE_COUNT 20 +#define ARRAY_DIM 50 // This MUST be at least 2*MAX_POLE_COUNT because some filter polys are defined in terms of 2 * NumPoles + +namespace kitiirfir +{ + +enum TOurSortTypes{stMax, stMin}; + +// These coeff form H(s) = (N2*s^2 + N1*s + N0) / (D2*s^2 + D1*s + D0) +// NumSections is the number of 1st and 2nd order polynomial factors . +struct TSPlaneCoeff { double N2[ARRAY_DIM]; double N1[ARRAY_DIM]; double N0[ARRAY_DIM]; + double D2[ARRAY_DIM]; double D1[ARRAY_DIM]; double D0[ARRAY_DIM]; + int NumSections; }; + +// These are the available filter polynomials. NOT_IIR is for code testing. +enum TFilterPoly {BUTTERWORTH, GAUSSIAN, BESSEL, ADJUSTABLE, CHEBYSHEV, + INVERSE_CHEBY, PAPOULIS, ELLIPTIC, NOT_IIR}; + +// This structure defines the low pass filter prototype. +// The 3 dB corner frequency is 1 rad/sec for all filters. +struct TLowPassParams {TFilterPoly ProtoType; // Butterworth, Cheby, etc. + int NumPoles; // Pole count + double Ripple; // Passband Ripple for the Elliptic and Chebyshev + double StopBanddB; // Stop Band Attenuation in dB for the Elliptic and Inverse Cheby + double Gamma; // Controls the transition bandwidth on the Adjustable Gauss. -1 <= Gamma <= 1 + }; + + +TSPlaneCoeff CalcLowPassProtoCoeff(TLowPassParams Filt); +void SetCornerFreq(int Count, double *D2, double *D1, double *D0, double *N2, double *N1, double *N0); +int GetFilterCoeff(int RootCount, std::complex *Roots, double *A2, double *A1, double *A0); +int RebuildPoly(int PolyCount, double *PolyCoeff, double *A2, double *A1, double *A0 ); +void SortRootsByZeta(std::complex *Roots, int Count, TOurSortTypes SortType); +bool HeapIndexSort(double *Data, int *Index, int N, TOurSortTypes SortType); + +} // namespace + +#endif diff --git a/kitiirfir/LowPassRoots.cpp b/kitiirfir/LowPassRoots.cpp new file mode 100644 index 000000000..d74332cd4 --- /dev/null +++ b/kitiirfir/LowPassRoots.cpp @@ -0,0 +1,683 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + May 1, 2016 + + + This code calculates the roots for the following filter polynomials. + Butterworth, Chebyshev, Bessel, Gauss, Adjustable Gauss, Inverse Chebyshev, Papoulis, and Elliptic. + + These filters are described in most filter design texts, except the Papoulis and Adjustable Gauss. + + The Adjustable Gauss is a transitional filter invented by Iowa Hills that can generate a response + anywhere between a Gauss and a Butterworth. Use Gamma to control the response. + Gamma = -1.0 nearly a Gauss + Gamma = -0.7 nearly a Bessel + Gamma = 1.0 nearly a Butterworth + + The Papoulis (Classic L) provides a response between the Butterworth the Chebyshev. + It has a faster roll off than the Butterworth but without the Chebyshev ripple. + It does however have some roll off in the pass band, which can be controlled by the RollOff parameter. + + The limits on the arguments for these functions (such as pole count or ripple) are not + checked here. See the LowPassPrototypes.cpp for argument limits. + */ + +#include +#include +#include "LowPassRoots.h" +#include "PFiftyOneRevE.h" + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- +// This used by several of the functions below. It returns a double so +// we don't overflow and because we need a double for subsequent calculations. +double Factorial(int N) { + int j; + double Fact = 1.0; + for (j = 1; j <= N; j++) + Fact *= (double) j; + return (Fact); +} + +//--------------------------------------------------------------------------- + +// Some of the code below generates the coefficients in reverse order +// needed for the root finder. This reverses the poly. +void ReverseCoeff(double *P, int N) { + int j; + double Temp; + for (j = 0; j <= N / 2; j++) { + Temp = P[j]; + P[j] = P[N - j]; + P[N - j] = Temp; + } + + for (j = N; j >= 1; j--) { + if (P[0] != 0.0) + P[j] /= P[0]; + } + P[0] = 1.0; + +} + +//--------------------------------------------------------------------------- + +// We calculate the roots for a Butterwoth filter directly. (No root finder needed) +// We fill the array Roots[] and return the number of roots. +int ButterworthPoly(int NumPoles, std::complex *Roots) { + int j, n, N; + double Theta; + + N = NumPoles; + n = 0; + for (j = 0; j < N / 2; j++) { + Theta = M_PI * (double) (2 * j + N + 1) / (double) (2 * N); + Roots[n++] = std::complex(cos(Theta), sin(Theta)); + Roots[n++] = std::complex(cos(Theta), -sin(Theta)); + } + if (N % 2 == 1) + Roots[n++] = std::complex(-1.0, 0.0); // The real root for odd pole counts. + return (N); +} + +//--------------------------------------------------------------------------- + +// This calculates the roots for a Chebyshev filter directly. (No root finder needed) +int ChebyshevPoly(int NumPoles, double Ripple, std::complex *Roots) { + int j, n, N; + double Sigma, Omega; + double Arg, Theta, Epsilon; + + N = NumPoles; + Epsilon = pow(10.0, Ripple / 10.0) - 1.0; + Epsilon = sqrt(Epsilon); + if (Epsilon < 0.00001) + Epsilon = 0.00001; + if (Epsilon > 0.996) + Epsilon = 0.996; + Epsilon = 1.0 / Epsilon; + Arg = log(Epsilon + sqrt(Epsilon * Epsilon + 1.0)) / (double) N; // = asinh(Epsilon) / (double)N; + n = 0; + for (j = 0; j < N / 2; j++) { + Theta = (2 * j + 1) * M_PI_2 / (double) N; + Sigma = -sinh(Arg) * sin(Theta); + Omega = cosh(Arg) * cos(Theta); + Roots[n++] = std::complex(Sigma, Omega); + Roots[n++] = std::complex(Sigma, -Omega); + } + if (N % 2 == 1) + Roots[n++] = std::complex(-sinh(Arg), 0.0); // The real root for odd pole counts. + return (N); +} + +//--------------------------------------------------------------------------- + +// The Gaussian Poly is simply 1 - s^2 + s^4 /2! - s^6 / 3! .... seeHumpherys p. 414. +int GaussianPoly(int NumPoles, std::complex *Roots) { + int j, N, RootsCount; + double GaussCoeff[P51_ARRAY_SIZE]; + + N = NumPoles; + GaussCoeff[0] = 1.0; + GaussCoeff[1] = 0.0; + for (j = 2; j <= 2 * N; j += 2) { + GaussCoeff[j] = 1.0 / Factorial(j / 2); + GaussCoeff[j + 1] = 0.0; + if ((j / 2) % 2 == 1) + GaussCoeff[j] *= -1.0; + } + + // The coefficients are generated in reverse order needed for P51. + ReverseCoeff(GaussCoeff, N * 2); + RootsCount = FindRoots(N * 2, GaussCoeff, Roots); + return (RootsCount); +} + +//--------------------------------------------------------------------------- + +// This function starts with the Gauss and modifies the coefficients. +// The Gaussian Poly is simply 1 - s^2 + s^4 /2! - s^6 / 3! .... seeHumpherys p. 414. +int AdjustablePoly(int NumPoles, std::complex *Roots, double Gamma) { + int j, N, RootsCount; + double GaussCoeff[P51_ARRAY_SIZE]; + + N = NumPoles; + if (Gamma > 0.0) + Gamma *= 2.0; // Gamma < 0 is the orig Gauss and Bessel responses. Gamma > 0 has an asymptotic response, so we double it, which also makes the user interface a bit nicer. i.e. -1 <= Gamma <= 1 + + GaussCoeff[0] = 1.0; + GaussCoeff[1] = 0.0; + for (j = 2; j <= 2 * N; j += 2) { + GaussCoeff[j] = pow(Factorial(j / 2), Gamma); // Gamma = -1 is orig Gauss poly, Gamma = 1 approaches a Butterworth response. + GaussCoeff[j + 1] = 0.0; + if ((j / 2) % 2 == 1) + GaussCoeff[j] *= -1.0; + } + + // The coefficients are generated in reverse order needed for P51. + ReverseCoeff(GaussCoeff, N * 2); + RootsCount = FindRoots(N * 2, GaussCoeff, Roots); + + // Scale the imag part of the root by 1.1 to get a response closer to a Butterworth when Gamma = -2 + for (j = 0; j < N * 2; j++) + Roots[j] = std::complex(Roots[j].real(), Roots[j].imag() * 1.10); + return (RootsCount); +} + +//--------------------------------------------------------------------------- + +// These Bessel coefficients are calc'd with the formula given in Johnson & Moore. Page 164. eq 7-26 +// The highet term is 1, the rest of the terms are calc'd +int BesselPoly(int NumPoles, std::complex *Roots) { + int k, N, RootsCount; + double b, PolyCoeff[P51_ARRAY_SIZE]; + + N = NumPoles; + for (k = N - 1; k >= 0; k--) { + // b is calc'd as a double because of all the division, but the result is essentially a large int. + b = Factorial(2 * N - k) / Factorial(k) / Factorial(N - k) + / pow(2.0, (double) (N - k)); + PolyCoeff[k] = b; + } + PolyCoeff[N] = 1.0; + + // The coefficients are generated in reverse order needed for P51. + ReverseCoeff(PolyCoeff, N); + RootsCount = FindRoots(N, PolyCoeff, Roots); + return (RootsCount); +} + +//--------------------------------------------------------------------------- + +// This Inverse Cheby code is identical to the Cheby code, except for two things. +// First, epsilon represents stop band atten, not ripple, so this epsilon is 1/epsilon. +// More importantly, the inverse cheby is defined in terms of F(1/s) instead of the usual F(s); +// After a bit of algebra, it is easy to see that the 1/s terms can be made s terms by simply +// multiplying the num and denom by s^2N. Then the 0th term becomes the highest term, so the +// coefficients are simply fed to the root finder in reverse order. +// Since 1 doesn't get added to the numerator, the lowest 1 or 2 terms will be 0, but we can't +// feed the root finder 0's as the highest term, so we have to be sure not to feed them to the root finder. + +int InvChebyPoly(int NumPoles, double StopBanddB, std::complex *ChebyPoles, + std::complex *ChebyZeros, int *ZeroCount) { + int j, k, N, PolesCount; + double Arg, Epsilon, ChebPolyCoeff[P51_ARRAY_SIZE], + PolyCoeff[P51_ARRAY_SIZE]; + std::complex SquaredPolyCoeff[P51_ARRAY_SIZE], A, B; + + N = NumPoles; + Epsilon = 1.0 / (pow(10.0, StopBanddB / 10.0) - 1.0); // actually Epsilon Squared + + // This algorithm is from the paper by Richard J Mathar. It generates the coefficients for the Cheb poly. + // It stores the Nth order coefficient in ChebPolyCoeff[N], and so on. Every other Cheb coeff is 0. See Wikipedia for a table that this code will generate. + for (j = 0; j <= N / 2; j++) { + Arg = Factorial(N - j - 1) / Factorial(j) / Factorial(N - 2 * j); + if (j % 2 == 1) + Arg *= -1.0; + Arg *= pow(2.0, (double) (N - 2 * j)) * (double) N / 2.0; + ChebPolyCoeff[N - 2 * j] = Arg; + if (N - (2 * j + 1) >= 0) { + ChebPolyCoeff[N - (2 * j + 1)] = 0.0; + } + } + + // Now square the Chebshev polynomial where we assume s = jw. To get the signs correct, + // we need to take j to the power. Then its a simple matter of adding powers and + // multiplying coefficients. j and k represent the exponents. That is, j=3 is the x^3 coeff, and so on. + for (j = 0; j <= 2 * N; j++) + SquaredPolyCoeff[j] = std::complex(0.0, 0.0); + + for (j = 0; j <= N; j++) + for (k = 0; k <= N; k++) { + A = pow(std::complex(0.0, 1.0), (double) j) * ChebPolyCoeff[j]; + B = pow(std::complex(0.0, 1.0), (double) k) * ChebPolyCoeff[k]; + SquaredPolyCoeff[j + k] = SquaredPolyCoeff[j + k] + A * B; // these end up entirely real. + } + + // Denominator + // Now we multiply the coefficients by Epsilon and add 1 to the denominator poly. + k = 0; + for (j = 0; j <= 2 * N; j++) + ChebPolyCoeff[j] = SquaredPolyCoeff[j].real() * Epsilon; + ChebPolyCoeff[0] += 1.0; + for (j = 0; j <= 2 * N; j++) + PolyCoeff[k++] = ChebPolyCoeff[j]; // Note this order is reversed from the Chebyshev routine. + k--; + PolesCount = FindRoots(k, PolyCoeff, ChebyPoles); + + // Numerator + k = 0; + for (j = 0; j <= 2 * N; j++) + ChebPolyCoeff[j] = SquaredPolyCoeff[j].real(); // Not using Epsilon here so the check for 0 on the next line is easier. Since the root finder normalizes the poly, it gets factored out anyway. + for (j = 0; j <= 2 * N; j++) + if (fabs(ChebPolyCoeff[j]) > 0.01) + break; // Get rid of the high order zeros. There will be eithe 0ne or two zeros to delete. + for (; j <= 2 * N; j++) + PolyCoeff[k++] = ChebPolyCoeff[j]; + k--; + *ZeroCount = FindRoots(k, PolyCoeff, ChebyZeros); + + return (PolesCount); + +} + +//--------------------------------------------------------------------------- + +// The complete Papouls poly is 1 + Epsilon * P(n) where P(n) is the 2*N Legendre poly. +int PapoulisPoly(int NumPoles, std::complex *Roots) { + int j, N, RootsCount; + double Epsilon, PolyCoeff[P51_ARRAY_SIZE]; + + N = NumPoles; + for (j = 0; j < 2 * N; j++) + PolyCoeff[j] = 0.0; // so we don't have to fill all the zero's. + + switch (N) { + case 1: // 1 pole + PolyCoeff[2] = 1.0; + break; + + case 2: // 2 pole + PolyCoeff[4] = 1.0; + break; + + case 3: // 3 pole + PolyCoeff[6] = 3.0; + PolyCoeff[4] = -3.0; + PolyCoeff[2] = 1.0; + break; + + case 4: + PolyCoeff[8] = 6.0; + PolyCoeff[6] = -8.0; + PolyCoeff[4] = 3.0; + break; + + case 5: + PolyCoeff[10] = 20.0; + PolyCoeff[8] = -40.0; + PolyCoeff[6] = 28.0; + PolyCoeff[4] = -8.0; + PolyCoeff[2] = 1.0; + break; + + case 6: + PolyCoeff[12] = 50.0; + PolyCoeff[10] = -120.0; + PolyCoeff[8] = 105.0; + PolyCoeff[6] = -40.0; + PolyCoeff[4] = 6.0; + break; + + case 7: + PolyCoeff[14] = 175.0; + PolyCoeff[12] = -525.0; + PolyCoeff[10] = 615.0; + PolyCoeff[8] = -355.0; + PolyCoeff[6] = 105.0; + PolyCoeff[4] = -15.0; + PolyCoeff[2] = 1.0; + break; + + case 8: + PolyCoeff[16] = 490.0; + PolyCoeff[14] = -1680.0; + PolyCoeff[12] = 2310.0; + PolyCoeff[10] = -1624.0; + PolyCoeff[8] = 615.0; + PolyCoeff[6] = -120.0; + PolyCoeff[4] = 10.0; + break; + + case 9: + PolyCoeff[18] = 1764.0; + PolyCoeff[16] = -7056.0; + PolyCoeff[14] = 11704.0; + PolyCoeff[12] = -10416.0; + PolyCoeff[10] = 5376.0; + PolyCoeff[8] = -1624.0; + PolyCoeff[6] = 276.0; + PolyCoeff[4] = -24.0; + PolyCoeff[2] = 1.0; + break; + + case 10: + PolyCoeff[20] = 5292.0; + PolyCoeff[18] = -23520.0; + PolyCoeff[16] = 44100.0; + PolyCoeff[14] = -45360.0; + PolyCoeff[12] = 27860.0; + PolyCoeff[10] = -10416.0; + PolyCoeff[8] = 2310.0; + PolyCoeff[6] = -280.0; + PolyCoeff[4] = 15.0; + break; + + case 11: + PolyCoeff[22] = 19404; + PolyCoeff[20] = -97020.0; + PolyCoeff[18] = 208740.0; + PolyCoeff[16] = -252840.0; + PolyCoeff[14] = 189420.0; + PolyCoeff[12] = -90804.0; + PolyCoeff[10] = 27860.0; + PolyCoeff[8] = -5320.0; + PolyCoeff[6] = 595.0; + PolyCoeff[4] = -35.0; + PolyCoeff[2] = 1.0; + break; + + case 12: + PolyCoeff[24] = 60984.0; + PolyCoeff[22] = -332640.0; + PolyCoeff[20] = 790020.0; + PolyCoeff[18] = -1071840.0; + PolyCoeff[16] = 916020.0; + PolyCoeff[14] = -512784.0; + PolyCoeff[12] = 189420.0; + PolyCoeff[10] = -45360.0; + PolyCoeff[8] = 6720.0; + PolyCoeff[6] = -560.0; + PolyCoeff[4] = 21.0; + break; + + case 13: + PolyCoeff[26] = 226512.0; + PolyCoeff[24] = -1359072.0; + PolyCoeff[22] = 3597264.0; + PolyCoeff[20] = -5528160.0; + PolyCoeff[18] = 5462820.0; + PolyCoeff[16] = -3632112.0; + PolyCoeff[14] = 1652232.0; + PolyCoeff[12] = -512784.0; + PolyCoeff[10] = 106380.0; + PolyCoeff[8] = -14160.0; + PolyCoeff[6] = 1128.0; + PolyCoeff[4] = -48.0; + PolyCoeff[2] = 1.0; + break; + + case 14: + PolyCoeff[28] = 736164.0; + PolyCoeff[26] = -4756752.0; + PolyCoeff[24] = 13675662.0; + PolyCoeff[22] = -23063040.0; + PolyCoeff[20] = 25322220.0; + PolyCoeff[18] = -18993744.0; + PolyCoeff[16] = 9934617.0; + PolyCoeff[14] = -3632112.0; + PolyCoeff[12] = 916020.0; + PolyCoeff[10] = -154560.0; + PolyCoeff[8] = 16506.0; + PolyCoeff[6] = -1008.0; + PolyCoeff[4] = 28.0; + break; + + case 15: + PolyCoeff[30] = 2760615.0; + PolyCoeff[28] = -19324305.0; + PolyCoeff[26] = 60747687.0; + PolyCoeff[24] = -113270157.0; + PolyCoeff[22] = 139378239.0; + PolyCoeff[20] = -119144025.0; + PolyCoeff[18] = 72539775.0; + PolyCoeff[16] = -31730787.0; + PolyCoeff[14] = 9934617.0; + PolyCoeff[12] = -2191959.0; + PolyCoeff[10] = 331065.0; + PolyCoeff[8] = -32655.0; + PolyCoeff[6] = 1953.0; + PolyCoeff[4] = -63.0; + PolyCoeff[2] = 1.0; + break; + + case 16: + PolyCoeff[32] = 9202050.0; + PolyCoeff[30] = -68708640.0; + PolyCoeff[28] = 231891660.0; + PolyCoeff[26] = -467747280.0; + PolyCoeff[24] = 628221594.0; + PolyCoeff[22] = -592431840.0; + PolyCoeff[20] = 403062660.0; + PolyCoeff[18] = -200142800.0; + PolyCoeff[16] = 72539775.0; + PolyCoeff[14] = -18993744.0; + PolyCoeff[12] = 3515820.0; + PolyCoeff[10] = -443520.0; + PolyCoeff[8] = 35910.0; + PolyCoeff[6] = -1680.0; + PolyCoeff[4] = 36.0; + break; + + case 17: + PolyCoeff[34] = 34763300.0; + PolyCoeff[32] = -278106400.0; + PolyCoeff[30] = 1012634480.0; + PolyCoeff[28] = -2221579360.0; + PolyCoeff[26] = 3276433160.0; + PolyCoeff[24] = -3431908480.0; + PolyCoeff[22] = 2629731104.0; + PolyCoeff[20] = -1496123200.0; + PolyCoeff[18] = 634862800.0; + PolyCoeff[16] = -200142800.0; + PolyCoeff[14] = 46307800.0; + PolyCoeff[12] = -7696304.0; + PolyCoeff[10] = 888580.0; + PolyCoeff[8] = -67760.0; + PolyCoeff[6] = 3160.0; + PolyCoeff[4] = -80.0; + PolyCoeff[2] = 1.0; + break; + + case 18: + PolyCoeff[36] = 118195220.0; + PolyCoeff[34] = -1001183040.0; + PolyCoeff[32] = 3879584280.0; + PolyCoeff[30] = -9110765664.0; + PolyCoeff[28] = 14480345880.0; + PolyCoeff[26] = -16474217760.0; + PolyCoeff[24] = 13838184360.0; + PolyCoeff[22] = -8725654080.0; + PolyCoeff[20] = 4158224928.0; + PolyCoeff[18] = -1496123200.0; + PolyCoeff[16] = 403062660.0; + PolyCoeff[14] = -79999920.0; + PolyCoeff[12] = 11397540.0; + PolyCoeff[10] = -1119888.0; + PolyCoeff[8] = 71280.0; + PolyCoeff[6] = -2640.0; + PolyCoeff[4] = 45.0; + break; + + case 19: + PolyCoeff[38] = 449141836.0; + PolyCoeff[36] = -4042276524.0; + PolyCoeff[34] = 16732271556.0; + PolyCoeff[32] = -42233237904.0; + PolyCoeff[30] = 72660859128.0; + PolyCoeff[28] = -90231621480.0; + PolyCoeff[26] = 83545742280.0; + PolyCoeff[24] = -58751550000.0; + PolyCoeff[22] = 31671113760.0; + PolyCoeff[20] = -13117232128.0; + PolyCoeff[18] = 4158224928.0; + PolyCoeff[16] = -999092952.0; + PolyCoeff[14] = 178966788.0; + PolyCoeff[12] = -23315292.0; + PolyCoeff[10] = 2130876.0; + PolyCoeff[8] = -129624.0; + PolyCoeff[6] = 4851.0; + PolyCoeff[4] = -99.0; + PolyCoeff[2] = 1.0; + break; + + case 20: + PolyCoeff[40] = 1551580888.0; + PolyCoeff[38] = -14699187360.0; + PolyCoeff[36] = 64308944700.0; + PolyCoeff[34] = -172355177280.0; + PolyCoeff[32] = 316521742680.0; + PolyCoeff[30] = -422089668000.0; + PolyCoeff[28] = 422594051880.0; + PolyCoeff[26] = -323945724960.0; + PolyCoeff[24] = 192167478360.0; + PolyCoeff[22] = -88572527680.0; + PolyCoeff[20] = 31671113760.0; + PolyCoeff[18] = -8725654080.0; + PolyCoeff[16] = 1829127300.0; + PolyCoeff[14] = -286125840.0; + PolyCoeff[12] = 32458140.0; + PolyCoeff[10] = -2560272.0; + PolyCoeff[8] = 131670.0; + PolyCoeff[6] = -3960.0; + PolyCoeff[4] = 55.0; + break; + } + + Epsilon = 0.1; // This controls the amount of pass band roll off. 0.01 < Epsilon < 0.250 + + // The poly is in terms of omega, but we need it in term of s = jw. So we need to + // multiply the approp coeff by neg 1 to account for j. Then mult by epsilon. + for (j = 0; j <= 2 * N; j++) { + if ((j / 2) % 2 == 1) + PolyCoeff[j] *= -1.0; + PolyCoeff[j] *= Epsilon; + } + + // Now add 1 to the poly. + PolyCoeff[0] = 1.0; + + // The coefficients are in reverse order needed for P51. + ReverseCoeff(PolyCoeff, N * 2); + RootsCount = FindRoots(N * 2, PolyCoeff, Roots); + + return (RootsCount); +} + +//--------------------------------------------------------------------------- + +// This code was described in "Elliptic Functions for Filter Design" +// H J Orchard and Alan N Willson IEE Trans on Circuits and Systems April 97 +// The equation numbers in the comments are from the paper. +// As the stop band attenuation -> infinity, the Elliptic -> Chebyshev. +int EllipticPoly(int FiltOrder, double Ripple, double DesiredSBdB, + std::complex *EllipPoles, std::complex *EllipZeros, int *ZeroCount) { + int j, k, n, LastK; + double K[ELLIPARRAYSIZE], G[ELLIPARRAYSIZE], Epsilon[ELLIPARRAYSIZE]; + double A, D, SBdB, dBErr, RealPart, ImagPart; + double DeltaK, PrevErr, Deriv; + std::complex C; + + for (j = 0; j < ELLIPARRAYSIZE; j++) + K[j] = G[j] = Epsilon[j] = 0.0; + if (Ripple < 0.001) + Ripple = 0.001; + if (Ripple > 1.0) + Ripple = 1.0; + Epsilon[0] = sqrt(pow(10.0, Ripple / 10.0) - 1.0); + + // Estimate K[0] to get the algorithm started. + K[0] = (double) (FiltOrder - 2) * 0.1605 + 0.016; + if (K[0] < 0.01) + K[0] = 0.01; + if (K[0] > 0.7) + K[0] = 0.7; + + // This loop calculates K[0] for the desired stopband attenuation. It typically loops < 5 times. + for (j = 0; j < MAX_ELLIP_ITER; j++) { + // Compute K with a forward Landen Transformation. + for (k = 1; k < 10; k++) { + K[k] = pow(K[k - 1] / (1.0 + sqrt(1.0 - K[k - 1] * K[k - 1])), 2.0); // eq. 10 + if (K[k] <= 1.0E-6) + break; + } + LastK = k; + + // Compute G with a backwards Landen Transformation. + G[LastK] = 4.0 * pow(K[LastK] / 4.0, (double) FiltOrder); + for (k = LastK; k >= 1; k--) { + G[k - 1] = 2.0 * sqrt(G[k]) / (1.0 + G[k]); // eq. 9 + } + + if (G[0] <= 0.0) + G[0] = 1.0E-10; + SBdB = 10.0 * log10(1.0 + pow(Epsilon[0] / G[0], 2.0)); // Current stopband attenuation dB + dBErr = DesiredSBdB - SBdB; + + if (fabs(dBErr) < 0.1) + break; + if (j == 0) // Do this on the 1st loop so we can calc a derivative. + { + if (dBErr > 0) + DeltaK = 0.005; + else + DeltaK = -0.005; + PrevErr = dBErr; + } else { + // Use Newtons Method to adjust K[0]. + Deriv = (PrevErr - dBErr) / DeltaK; + PrevErr = dBErr; + if (Deriv == 0.0) + break; // This happens when K[0] hits one of the limits set below. + DeltaK = dBErr / Deriv; + if (DeltaK > 0.1) + DeltaK = 0.1; + if (DeltaK < -0.1) + DeltaK = -0.1; + } + K[0] -= DeltaK; + if (K[0] < 0.001) + K[0] = 0.001; // must not be < 0.0 + if (K[0] > 0.990) + K[0] = 0.990; // if > 0.990 we get a pole in the RHP. This means we were unable to set the stop band atten to the desired level (the Ripple is too large for the Pole Count). + } + + // Epsilon[0] was calulated above, now calculate Epsilon[LastK] from G + for (j = 1; j <= LastK; j++) { + A = (1.0 + G[j]) * Epsilon[j - 1] / 2.0; // eq. 37 + Epsilon[j] = A + sqrt(A * A + G[j]); + } + + // Calulate the poles and zeros. + ImagPart = log( + (1.0 + sqrt(1.0 + Epsilon[LastK] * Epsilon[LastK])) + / Epsilon[LastK]) / (double) FiltOrder; // eq. 22 + n = 0; + for (j = 1; j <= FiltOrder / 2; j++) { + RealPart = (double) (2 * j - 1) * M_PI_2 / (double) FiltOrder; // eq. 19 + C = std::complex(0.0, -1.0) / cos(std::complex(-RealPart, ImagPart)); // eq. 20 + D = 1.0 / cos(RealPart); + for (k = LastK; k >= 1; k--) { + C = (C - K[k] / C) / (1.0 + K[k]); // eq. 36 + D = (D + K[k] / D) / (1.0 + K[k]); + } + + EllipPoles[n] = 1.0 / C; + EllipPoles[n + 1] = std::conj(EllipPoles[n]); + EllipZeros[n] = std::complex(0.0, D / K[0]); + EllipZeros[n + 1] = std::conj(EllipZeros[n]); + n += 2; + } + *ZeroCount = n; // n is the num zeros + + if (FiltOrder % 2 == 1) // The real pole for odd pole counts + { + A = 1.0 / sinh(ImagPart); + for (k = LastK; k >= 1; k--) { + A = (A - K[k] / A) / (1.0 + K[k]); // eq. 38 + } + EllipPoles[n] = std::complex(-1.0 / A, 0.0); + n++; + } + + return (n); // n is the num poles. There will be 1 more pole than zeros for odd pole counts. + +} +//--------------------------------------------------------------------------- + +} // namespace diff --git a/kitiirfir/LowPassRoots.h b/kitiirfir/LowPassRoots.h new file mode 100644 index 000000000..5c614a57f --- /dev/null +++ b/kitiirfir/LowPassRoots.h @@ -0,0 +1,27 @@ +//--------------------------------------------------------------------------- + +#ifndef LowPassRootsH +#define LowPassRootsH + +#include +#define MAX_ELLIP_ITER 15 +#define ELLIPARRAYSIZE 20 // needs to be > 10 and >= Max Num Poles + 1 + +namespace kitiirfir +{ + +void ReverseCoeff(double *P, int N); +int ButterworthPoly(int NumPoles, std::complex *Roots); +int GaussianPoly(int NumPoles, std::complex *Roots); +int AdjustablePoly(int NumPoles, std::complex *Roots, double Gamma); +int ChebyshevPoly(int NumPoles, double Ripple, std::complex *Roots); +int BesselPoly(int NumPoles, std::complex *Roots); +int InvChebyPoly(int NumPoles, double StopBanddB, std::complex *ChebyPoles, std::complex *ChebyZeros, int *ZeroCount); +int PapoulisPoly(int NumPoles, std::complex *Roots); +int EllipticPoly(int FiltOrder, double Ripple, double DesiredSBdB, std::complex *EllipPoles, std::complex *EllipZeros, int *ZeroCount); + +} // namespace + +#endif + + diff --git a/kitiirfir/NewParksMcClellan.cpp b/kitiirfir/NewParksMcClellan.cpp new file mode 100644 index 000000000..507b766b8 --- /dev/null +++ b/kitiirfir/NewParksMcClellan.cpp @@ -0,0 +1,627 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + May 1, 2016 + + ShowMessage is a C++ Builder function, and it usage has been commented out. + If you are using C++ Builder, include vcl.h for ShowMessage. + Otherwise replace ShowMessage with something appropriate for your compiler. + + This is a C translation of the Parks McClellan algorithm originally done in Fortran. + + The original fortran code came from the Parks McClellan article on Wikipedia. + http://en.wikipedia.org/wiki/Parks%E2%80%93McClellan_filter_design_algorithm + + This code is quite different from the original. The original code had 69 goto statements, + which made it nearly impossible to follow. And of course, it was Fortran code, so many changes + had to be made regardless of style. + + Apparently, Fortran doesn't use global variables. Instead, is uses something called + common memory space. e.g. COMMON PI2,AD,DEV,X,Y,GRID,DES,WT,ALPHA,IEXT,NFCNS,NGRID + I simply converted these to globals. It isn't pretty, but for this purpose, who cares? + + The first step was to get a C version of the code working with as few changes as possible. + That version is also available on: http://www.iowahills.com/A7ExampleCodePage.html + Then, in our desire to see if the code could be made more understandable, we decided to + remove as many goto statements as possible. We checked our work by comparing the coefficients + between this code and our original translation on more than 1000 filters while varying all the parameters. + + Ultimately, we were able to reduce the goto count from 69 to 7, all of which are in the Remez + function. Of the 7 remaining, 3 of these are at the very bottom of the function, and go + back to the very top of the function. These could have been removed, but our goal was to + clarify the code, not restyle it, and since they are clear, we let them be. + + The other 4 goto statements are intertwined in a rather nasty way. We recommend you print out + the Remez code, tape the sheets end to end, and trace out the goto's. It wasn't apparent to + us that they can be removed without an extensive study of the code. + + For better or worse, we also removed any code that was obviously related to Hilbert transforms + and Differentiators. We did this because we aren't interested in these, and we also don't + believe this algorithm does a very good job with them (far too much ripple). + + We added the functions CalcCoefficients() and ErrTest() as a way to simplify things a bit. + + We also found 3 sections of code that never executed. Two of the sections were just a few lines + that the goto's always went around. The third section represented nearly half of the CalcCoefficients() + function. This statement always tested the same, which never allowed the code to execute. + if(GRID[1] < 0.01 && GRID[NGRID] > 0.49) KKK = 1; + This may be due to the 0.01 minimum width limit we set for the bands. + + Note our use of MIN_TEST_VAL. The original code wasn't guarding against division by zero. + Limiting the return values as we have also helped the algorithm's convergence behavior. + + In an effort to improve readability, we made a large number of variable name changes and also + deleted a large number of variables. We left many variable names in tact, in part as an aid when + comparing to the original code, and in part because a better name wasn't obvious. + + This code is essentially straight c, and should compile with few, if any changes. Note the error + message in CalcParkCoeff2. It warns of the possibility of convergence failure, but you will + find that the iteration count NITER, isn't always an indicator of convergence problems when + it is less than 3, as stated in the original Fortran code comments. + + If you find a problem with this code, please leave us a note on: + http://www.iowahills.com/feedbackcomments.html + + */ + +#include "NewParksMcClellan.h" +#include +#define M_2PI 6.28318530717958647692 + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- + +// Global variables. +int HalfTapCount, ExchangeIndex[PARKS_SMALL]; +double LeGrangeD[PARKS_SMALL], Alpha[PARKS_SMALL], CosOfGrid[PARKS_SMALL], + DesPlus[PARKS_SMALL]; +double Coeff[PARKS_SMALL], Edge[PARKS_SMALL], BandMag[PARKS_SMALL], + InitWeight[PARKS_SMALL]; +double DesiredMag[PARKS_BIG], Grid[PARKS_BIG], Weight[PARKS_BIG]; + +void NewParksMcClellan(double *FirCoeff, int NumTaps, TFIRPassTypes PassType, + double OmegaC, double BW, double ParksWidth) { + if (NumTaps > MAX_NUM_PARKS_TAPS) + return; + int j, NumBands; + + // Note: There is no feedback to the caller if ParksWidth or NumTaps are modified here. + if (PassType == firBPF || PassType == firNOTCH || NumTaps > 70) { + if (ParksWidth > 0.15) + ParksWidth = 0.15; // Else we have covergence problems. + } + + if (PassType == firNOTCH || PassType == firHPF) { + if (NumTaps % 2 == 0) + NumTaps++; // High pass and notch filters must have odd tap counts. + } + + if (NumTaps > MAX_NUM_PARKS_TAPS) + NumTaps = MAX_NUM_PARKS_TAPS; + + // It helps the algorithm a great deal if each band is at least 0.01 wide. + // The weights used here came from the orig PM code. + if (PassType == firLPF) { + NumBands = 2; + Edge[1] = 0.0; // Omega = 0 + Edge[2] = OmegaC; // Pass band edge + if (Edge[2] < 0.01) + Edge[2] = 0.01; + if (Edge[2] > 0.98) + Edge[2] = 0.98; + Edge[3] = Edge[2] + ParksWidth; // Stop band edge + if (Edge[3] > 0.99) + Edge[3] = 0.99; + Edge[4] = 1.0; // Omega = Pi + BandMag[1] = 1.0; + BandMag[2] = 0.0; + InitWeight[1] = 1.0; + InitWeight[2] = 10.0; + } + + if (PassType == firHPF) { + NumBands = 2; + Edge[1] = 0.0; // Omega = 0 + Edge[3] = OmegaC; // Pass band edge + if (Edge[3] > 0.99) + Edge[3] = 0.99; + if (Edge[3] < 0.02) + Edge[3] = 0.02; + Edge[2] = Edge[3] - ParksWidth; // Stop band edge + if (Edge[2] < 0.01) + Edge[2] = 0.01; + Edge[4] = 1.0; // Omega = Pi + BandMag[1] = 0.0; + BandMag[2] = 1.0; + InitWeight[1] = 10.0; + InitWeight[2] = 1.0; + } + + if (PassType == firBPF) { + NumBands = 3; + Edge[1] = 0.0; // Omega = 0 + Edge[3] = OmegaC - BW / 2.0; // Left pass band edge. + if (Edge[3] < 0.02) + Edge[3] = 0.02; + Edge[2] = Edge[3] - ParksWidth; // Left stop band edge + if (Edge[2] < 0.01) + Edge[2] = 0.01; + Edge[4] = OmegaC + BW / 2.0; // Right pass band edge + if (Edge[4] > 0.98) + Edge[4] = 0.98; + Edge[5] = Edge[4] + ParksWidth; // Right stop band edge + if (Edge[5] > 0.99) + Edge[5] = 0.99; + Edge[6] = 1.0; // Omega = Pi + + BandMag[1] = 0.0; + BandMag[2] = 1.0; + BandMag[3] = 0.0; + InitWeight[1] = 10.0; + InitWeight[2] = 1.0; + InitWeight[3] = 10.0; + } + + // This algorithm tends to have problems with narrow band notch filters. + if (PassType == firNOTCH) { + NumBands = 3; + Edge[1] = 0.0; // Omega = 0 + Edge[3] = OmegaC - BW / 2.0; // Left stop band edge. + if (Edge[3] < 0.02) + Edge[3] = 0.02; + Edge[2] = Edge[3] - ParksWidth; // Left pass band edge + if (Edge[2] < 0.01) + Edge[2] = 0.01; + Edge[4] = OmegaC + BW / 2.0; // Right stop band edge + if (Edge[4] > 0.98) + Edge[4] = 0.98; + Edge[5] = Edge[4] + ParksWidth; // Right pass band edge + if (Edge[5] > 0.99) + Edge[5] = 0.99; + Edge[6] = 1.0; // Omega = Pi + + BandMag[1] = 1.0; + BandMag[2] = 0.0; + BandMag[3] = 1.0; + InitWeight[1] = 1.0; + InitWeight[2] = 10.0; + InitWeight[3] = 1.0; + } + + // Parks McClellan's edges are based on 2Pi, we are based on Pi. + for (j = 1; j <= 2 * NumBands; j++) + Edge[j] /= 2.0; + + CalcParkCoeff2(NumBands, NumTaps, FirCoeff); + +} +//--------------------------------------------------------------------------- + +void CalcParkCoeff2(int NumBands, int TapCount, double *FirCoeff) { + int j, k, GridCount, GridIndex, BandIndex, NumIterations; + double LowFreqEdge, UpperFreq, TempVar, Change; + bool OddNumTaps; + GridCount = 16; // Grid Density + + if (TapCount % 2) + OddNumTaps = true; + else + OddNumTaps = false; + + HalfTapCount = TapCount / 2; + if (OddNumTaps) + HalfTapCount++; + + Grid[1] = Edge[1]; + LowFreqEdge = GridCount * HalfTapCount; + LowFreqEdge = 0.5 / LowFreqEdge; + j = 1; + k = 1; + BandIndex = 1; + while (BandIndex <= NumBands) { + UpperFreq = Edge[k + 1]; + while (Grid[j] <= UpperFreq) { + TempVar = Grid[j]; + DesiredMag[j] = BandMag[BandIndex]; + Weight[j] = InitWeight[BandIndex]; + j++; + ; + Grid[j] = TempVar + LowFreqEdge; + } + + Grid[j - 1] = UpperFreq; + DesiredMag[j - 1] = BandMag[BandIndex]; + Weight[j - 1] = InitWeight[BandIndex]; + k += 2; + BandIndex++; + if (BandIndex <= NumBands) + Grid[j] = Edge[k]; + } + + GridIndex = j - 1; + if (!OddNumTaps && Grid[GridIndex] > (0.5 - LowFreqEdge)) + GridIndex--; + + if (!OddNumTaps) { + for (j = 1; j <= GridIndex; j++) { + Change = cos(M_PI * Grid[j]); + DesiredMag[j] = DesiredMag[j] / Change; + Weight[j] = Weight[j] * Change; + } + } + + TempVar = (double) (GridIndex - 1) / (double) HalfTapCount; + for (j = 1; j <= HalfTapCount; j++) { + ExchangeIndex[j] = (int) ((double) (j - 1) * TempVar + 1.0); + } + ExchangeIndex[HalfTapCount + 1] = GridIndex; + + NumIterations = Remez2(GridIndex); + CalcCoefficients(); + + // Calculate the impulse response. + if (OddNumTaps) { + for (j = 1; j <= HalfTapCount - 1; j++) { + Coeff[j] = 0.5 * Alpha[HalfTapCount + 1 - j]; + } + Coeff[HalfTapCount] = Alpha[1]; + } else { + Coeff[1] = 0.25 * Alpha[HalfTapCount]; + for (j = 2; j <= HalfTapCount - 1; j++) { + Coeff[j] = + 0.25 + * (Alpha[HalfTapCount + 1 - j] + + Alpha[HalfTapCount + 2 - j]); + } + Coeff[HalfTapCount] = 0.5 * Alpha[1] + 0.25 * Alpha[2]; + } + + // Output section. + for (j = 1; j <= HalfTapCount; j++) + FirCoeff[j - 1] = Coeff[j]; + if (OddNumTaps) + for (j = 1; j < HalfTapCount; j++) + FirCoeff[HalfTapCount + j - 1] = Coeff[HalfTapCount - j]; + else + for (j = 1; j <= HalfTapCount; j++) + FirCoeff[HalfTapCount + j - 1] = Coeff[HalfTapCount - j + 1]; + + // Display the iteration count. + if (NumIterations < 3) { + // ShowMessage("Parks McClellan unable to coverge"); + } + +} + +//--------------------------------------------------------------------------------------- +int Remez2(int GridIndex) { + int j, JET, K, k, NU, JCHNGE, K1, KNZ, KLOW, NUT, KUP; + int NUT1 = 0, LUCK, KN, NITER; + double Deviation, DNUM, DDEN, TempVar; + double DEVL, COMP = 0.0, YNZ = 0.0, Y1 = 0.0, ERR; + + LUCK = 0; + DEVL = -1.0; + NITER = 1; // Init this to 1 to be consistent with the orig code. + + TOP_LINE: // We come back to here from 3 places at the bottom. + ExchangeIndex[HalfTapCount + 2] = GridIndex + 1; + + for (j = 1; j <= HalfTapCount + 1; j++) { + TempVar = Grid[ExchangeIndex[j]]; + CosOfGrid[j] = cos(TempVar * M_2PI); + } + + JET = (HalfTapCount - 1) / 15 + 1; + for (j = 1; j <= HalfTapCount + 1; j++) { + LeGrangeD[j] = LeGrangeInterp2(j, HalfTapCount + 1, JET); + } + + DNUM = 0.0; + DDEN = 0.0; + K = 1; + for (j = 1; j <= HalfTapCount + 1; j++) { + k = ExchangeIndex[j]; + DNUM += LeGrangeD[j] * DesiredMag[k]; + DDEN += (double) K * LeGrangeD[j] / Weight[k]; + K = -K; + } + Deviation = DNUM / DDEN; + + NU = 1; + if (Deviation > 0.0) + NU = -1; + Deviation = -(double) NU * Deviation; + K = NU; + for (j = 1; j <= HalfTapCount + 1; j++) { + k = ExchangeIndex[j]; + TempVar = (double) K * Deviation / Weight[k]; + DesPlus[j] = DesiredMag[k] + TempVar; + K = -K; + } + + if (Deviation <= DEVL) + return (NITER); // Ouch + + DEVL = Deviation; + JCHNGE = 0; + K1 = ExchangeIndex[1]; + KNZ = ExchangeIndex[HalfTapCount + 1]; + KLOW = 0; + NUT = -NU; + + //Search for the extremal frequencies of the best approximation. + + j = 1; + while (j < HalfTapCount + 2) { + KUP = ExchangeIndex[j + 1]; + k = ExchangeIndex[j] + 1; + NUT = -NUT; + if (j == 2) + Y1 = COMP; + COMP = Deviation; + + if (k < KUP && !ErrTest(k, NUT, COMP, &ERR)) { + L210: COMP = (double) NUT * ERR; + for (k++; k < KUP; k++) { + if (ErrTest(k, NUT, COMP, &ERR)) + break; // for loop + COMP = (double) NUT * ERR; + } + + ExchangeIndex[j] = k - 1; + j++; + KLOW = k - 1; + JCHNGE++; + continue; // while loop + } + + k--; + + L225: k--; + if (k <= KLOW) { + k = ExchangeIndex[j] + 1; + if (JCHNGE > 0) { + ExchangeIndex[j] = k - 1; + j++; + KLOW = k - 1; + JCHNGE++; + continue; // while loop + } else // JCHNGE <= 0 + { + for (k++; k < KUP; k++) { + if (ErrTest(k, NUT, COMP, &ERR)) + continue; // for loop + goto L210; + } + + KLOW = ExchangeIndex[j]; + j++; + continue; // while loop + } + } + // Can't use a do while loop here, it would outdent the two continue statements. + if (ErrTest(k, NUT, COMP, &ERR) && JCHNGE <= 0) + goto L225; + + if (ErrTest(k, NUT, COMP, &ERR)) { + KLOW = ExchangeIndex[j]; + j++; + continue; // while loop + } + + COMP = (double) NUT * ERR; + + L235: for (k--; k > KLOW; k--) { + if (ErrTest(k, NUT, COMP, &ERR)) + break; // for loop + COMP = (double) NUT * ERR; + } + + KLOW = ExchangeIndex[j]; + ExchangeIndex[j] = k + 1; + j++; + JCHNGE++; + } // end while(j ExchangeIndex[1]) + K1 = ExchangeIndex[1]; + if (KNZ < ExchangeIndex[HalfTapCount + 1]) + KNZ = ExchangeIndex[HalfTapCount + 1]; + NUT1 = NUT; + NUT = -NU; + k = 0; + KUP = K1; + COMP = YNZ * 1.00001; + LUCK = 1; + + for (k++; k < KUP; k++) { + if (ErrTest(k, NUT, COMP, &ERR)) + continue; // for loop + j = HalfTapCount + 2; + goto L210; + } + LUCK = 2; + break; // break while(j <= HalfTapCount+2) loop + } // end while(j <= HalfTapCount+2) + + if (LUCK == 1 || LUCK == 2) { + if (LUCK == 1) { + if (COMP > Y1) + Y1 = COMP; + K1 = ExchangeIndex[HalfTapCount + 2]; + } + + k = GridIndex + 1; + KLOW = KNZ; + NUT = -NUT1; + COMP = Y1 * 1.00001; + + for (k--; k > KLOW; k--) { + if (ErrTest(k, NUT, COMP, &ERR)) + continue; // for loop + j = HalfTapCount + 2; + COMP = (double) NUT * ERR; + LUCK = 3; // last time in this if(LUCK == 1 || LUCK == 2) + goto L235; + } + + if (LUCK == 2) { + if (JCHNGE > 0 && NITER++ < ITRMAX) + goto TOP_LINE; + else + return (NITER); + } + + for (j = 1; j <= HalfTapCount; j++) { + ExchangeIndex[HalfTapCount + 2 - j] = ExchangeIndex[HalfTapCount + 1 + - j]; + } + ExchangeIndex[1] = K1; + if (NITER++ < ITRMAX) + goto TOP_LINE; + } // end if(LUCK == 1 || LUCK == 2) + + KN = ExchangeIndex[HalfTapCount + 2]; + for (j = 1; j <= HalfTapCount; j++) { + ExchangeIndex[j] = ExchangeIndex[j + 1]; + } + ExchangeIndex[HalfTapCount + 1] = KN; + if (NITER++ < ITRMAX) + goto TOP_LINE; + + return (NITER); + +} + +//----------------------------------------------------------------------- +// Function to calculate the lagrange interpolation coefficients for use in the function gee. +double LeGrangeInterp2(int K, int N, int M) // D + { + int j, k; + double Dee, Q; + Dee = 1.0; + Q = CosOfGrid[K]; + for (k = 1; k <= M; k++) + for (j = k; j <= N; j += M) { + if (j != K) + Dee = 2.0 * Dee * (Q - CosOfGrid[j]); + } + if (fabs(Dee) < MIN_TEST_VAL) { + if (Dee < 0.0) + Dee = -MIN_TEST_VAL; + else + Dee = MIN_TEST_VAL; + } + return (1.0 / Dee); +} + +//----------------------------------------------------------------------- +// Function to evaluate the frequency response using the Lagrange interpolation +// formula in the barycentric form. +double GEE2(int K, int N) { + int j; + double P, C, Dee, XF; + P = 0.0; + XF = Grid[K]; + XF = cos(M_2PI * XF); + Dee = 0.0; + for (j = 1; j <= N; j++) { + C = XF - CosOfGrid[j]; + if (fabs(C) < MIN_TEST_VAL) { + if (C < 0.0) + C = -MIN_TEST_VAL; + else + C = MIN_TEST_VAL; + } + C = LeGrangeD[j] / C; + Dee = Dee + C; + P = P + C * DesPlus[j]; + } + if (fabs(Dee) < MIN_TEST_VAL) { + if (Dee < 0.0) + Dee = -MIN_TEST_VAL; + else + Dee = MIN_TEST_VAL; + } + return (P / Dee); +} + +//----------------------------------------------------------------------- + +bool ErrTest(int k, int Nut, double Comp, double *Err) { + *Err = GEE2(k, HalfTapCount + 1); + *Err = (*Err - DesiredMag[k]) * Weight[k]; + if ((double) Nut * *Err - Comp <= 0.0) + return (true); + else + return (false); +} + +//----------------------------------------------------------------------- + +// Calculation of the coefficients of the best approximation using the inverse discrete fourier transform. +void CalcCoefficients(void) { + int j, k, n; + double GTempVar, OneOverNumTaps; + double Omega, TempVar, FreqN, TempX, GridCos; + double GeeArray[PARKS_SMALL]; + + GTempVar = Grid[1]; + CosOfGrid[HalfTapCount + 2] = -2.0; + OneOverNumTaps = 1.0 / (double) (2 * HalfTapCount - 1); + k = 1; + + for (j = 1; j <= HalfTapCount; j++) { + FreqN = (double) (j - 1) * OneOverNumTaps; + TempX = cos(M_2PI * FreqN); + + GridCos = CosOfGrid[k]; + if (TempX <= GridCos) { + while (TempX <= GridCos && (GridCos - TempX) >= MIN_TEST_VAL) // MIN_TEST_VAL = 1.0E-6 + { + k++; + ; + GridCos = CosOfGrid[k]; + } + } + if (TempX <= GridCos || (TempX - GridCos) < MIN_TEST_VAL) { + GeeArray[j] = DesPlus[k]; // Desired Response + } else { + Grid[1] = FreqN; + GeeArray[j] = GEE2(1, HalfTapCount + 1); + } + if (k > 1) + k--; + } + + Grid[1] = GTempVar; + for (j = 1; j <= HalfTapCount; j++) { + TempVar = 0.0; + Omega = (double) (j - 1) * M_2PI * OneOverNumTaps; + for (n = 1; n <= HalfTapCount - 1; n++) { + TempVar += GeeArray[n + 1] * cos(Omega * (double) n); + } + TempVar = 2.0 * TempVar + GeeArray[1]; + Alpha[j] = TempVar; + } + + Alpha[1] = Alpha[1] * OneOverNumTaps; + for (j = 2; j <= HalfTapCount; j++) { + Alpha[j] = 2.0 * Alpha[j] * OneOverNumTaps; + } + +} + +//----------------------------------------------------------------------- + +} // namespace + diff --git a/kitiirfir/NewParksMcClellan.h b/kitiirfir/NewParksMcClellan.h new file mode 100644 index 000000000..16af86df1 --- /dev/null +++ b/kitiirfir/NewParksMcClellan.h @@ -0,0 +1,31 @@ +//--------------------------------------------------------------------------- + +#ifndef NewParksMcClellanH +#define NewParksMcClellanH + +//--------------------------------------------------------------------------- +#define PARKS_BIG 1100 // Per the original code, this must be > 8 * MaxNumTaps = 8 * 128 = 1024 +#define PARKS_SMALL 256 // This needs to be greater than or equal to MAX_NUM_PARKS_TAPS +#define MAX_NUM_PARKS_TAPS 127 // This was the limit set in the original code. +#define ITRMAX 50 // Max Number of Iterations. Some Notch and BPF are running ~ 43 +#define MIN_TEST_VAL 1.0E-6 // Min value used in LeGrangeInterp and GEE + +// If the FIRFilterCode.cpp file is in the project along with this NewParksMcClellan file, +// we need to include FIRFilterCode.h for the TFIRPassTypes enum. +#include "FIRFilterCode.h" +//enum TFIRPassTypes {firLPF, firHPF, firBPF, firNOTCH, ftNOT_FIR}; + +namespace kitiirfir +{ + +void NewParksMcClellan(double *FirCoeff, int NumTaps, TFIRPassTypes PassType, double OmegaC, double BW, double ParksWidth); +void CalcParkCoeff2(int NBANDS, int NFILT, double *FirCoeff); +double LeGrangeInterp2(int K, int N, int M); +double GEE2(int K, int N); +int Remez2(int GridIndex); +bool ErrTest(int k, int Nut, double Comp, double *Err); +void CalcCoefficients(void); + +} // namespace + +#endif diff --git a/kitiirfir/PFiftyOneRevE.cpp b/kitiirfir/PFiftyOneRevE.cpp new file mode 100644 index 000000000..8d03e94af --- /dev/null +++ b/kitiirfir/PFiftyOneRevE.cpp @@ -0,0 +1,649 @@ + +//--------------------------------------------------------------------------- + +#include "PFiftyOneRevE.h" +#include "QuadRootsRevH.h" +#include +#include + +//--------------------------------------------------------------------------- +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + Sept 21, 2016 Rev E + + If you find a problem with this code, please leave a note on: + http://www.iowahills.com/feedbackcomments.html + + This is for polynomials with real coefficients, up to 100th order. + + PFiftyOne has 4 loops. The outermost while loop runs until the polynomial's order + N, has been reduced to 1 or 2. + + The TypeOfK loop controls how the K polynomial is initialized. About 99.999% of all + roots are found with TypeOfK = 0. + + The AngleNumber loop controls how we initialize the 2nd order polynomial TUV. + The angles move between the 1st and 2nd quadrants and are defined in the + SetTUVandK() function. About 99% of roots are found in the 1st 4 angles. + + The Iter loop first calls the QuadIterate routine, which finds quadratic factors, + real or complex. If QuadIterate fails to converge in QUAD_ITER_MAX iterations, + the RealIterate routine is called to try to extract a single real root. + + The input array Coeff[] contains the coefficients arranged in descending order. + For example, a 4th order poly would be: + P(x) = Coeff[0]*x^4 + Coeff[1]*x^3 + Coeff[2]*x^2 + Coeff[3]*x + Coeff[4] + + On return, there is no particular ordering to the roots, but complex root pairs + will be in adjacent array cells. It may be helpful to know that the complex root pairs + are exact cunjugates of each other, meaning that you can use the == operator + to search for complex pairs in the arrays.F + + This returns Degree on success, 0 on failure. + */ + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- +// None of the code uses long double variables except for the P51 root finder code +// where long doubles were essential. FindRoots is an interface to P51 for code that is +// using complex variables CplxD. +int FindRoots(int N, double *Coeff, std::complex *Roots) { + int j; + long double P[P51_ARRAY_SIZE], RealRoot[P51_ARRAY_SIZE], + ImagRoot[P51_ARRAY_SIZE]; + + for (j = 0; j <= N; j++) + P[j] = Coeff[j]; // double to long double + N = PFiftyOne(P, N, RealRoot, ImagRoot); + for (j = 0; j < N; j++) + Roots[j] = std::complex((double) RealRoot[j], + (double) ImagRoot[j]); // long double to double + return (N); +} + +//--------------------------------------------------------------------------- + +int PFiftyOne(long double *Coeff, int Degree, long double *RealRoot, + long double *ImagRoot) { + if (Degree > P51_MAXDEGREE || Degree < 0) { + //ShowMessage("Poly Degree is out of range in the P51 Root Finder."); + return (0); + } + + TUpdateStatus UpdateStatus; + int N, NZ, j, Iter, AngleNumber, TypeOfK; + long double RealZero, QuadX; + long double TUV[3]; + long double *P, *QuadQP, *RealQP, *QuadK, *RealK, *QK; + + N = Degree; // N is decremented as roots are found. + + P = new (std::nothrow) long double[N + 2]; + QuadQP = new (std::nothrow) long double[N + 2]; + RealQP = new (std::nothrow) long double[N + 2]; + QuadK = new (std::nothrow) long double[N + 2]; + RealK = new (std::nothrow) long double[N + 2]; + QK = new (std::nothrow) long double[N + 2]; + if (P == NULL || QuadQP == NULL || RealQP == NULL || QuadK == NULL + || RealK == NULL || QK == NULL) { + //ShowMessage("Memory not Allocated in PFiftyOne root finder."); + return (0); + } + + for (j = 0; j <= N; j++) + P[j] = Coeff[j]; // Copy the coeff. P gets modified. + for (j = 0; j < N; j++) + RealRoot[j] = ImagRoot[j] = 0.0; // Init to zero, in case any leading or trailing zeros are removed. + + // Remove trailing zeros. A tiny P[N] relative to P[N-1] is not allowed. + while (N > 0 && fabsl(P[N]) <= TINY_VALUE * fabsl(P[N - 1])) { + N--; + } + + // Remove leading zeros. + while (N > 0 && P[0] == 0.0) { + for (j = 0; j < N; j++) + P[j] = P[j + 1]; + N--; + } + + // P[0] must = 1 + if (P[0] != 1.0) { + for (j = 1; j <= N; j++) + P[j] /= P[0]; + P[0] = 1.0; + } + + TypeOfK = 0; + while (N > 4 && TypeOfK < MAX_NUM_K) { + NZ = 0; // Num Zeros found. (Used in the loop controls below.) + QuadX = powl(fabsl(P[N]), 1.0 / (long double) N) / 2.0; // QuadX is used to init TUV + + for (TypeOfK = 0; TypeOfK < MAX_NUM_K && NZ == 0; TypeOfK++) // Iterate on the different possible QuadK inits. + { + for (AngleNumber = 0; AngleNumber < MAX_NUM_ANGLES && NZ == 0; + AngleNumber++) // Iterate on the angle used to init TUV. + { + SetTUVandK(P, N, TUV, RealK, QuadK, QuadX, AngleNumber, + TypeOfK); // Init TUV and both K's + for (Iter = 0; Iter < N && NZ == 0; Iter++) // Allow N calls to QuadIterate for N*QUAD_ITER_MAX iterations, then try a different angle. + { + NZ = QuadIterate(Iter, P, QuadQP, QuadK, QK, N, TUV, + &UpdateStatus); // NZ = 2 for a pair of complex roots or 2 real roots. + + if (NZ == 0) // Try for a real root. + { + if (fabsl(QuadK[N - 2]) > TINY_VALUE * fabsl(P[N])) + RealZero = -P[N] / QuadK[N - 2]; // This value gets refined by QuadIterate. + else + RealZero = 0.0; + NZ = RealIterate(Iter, P, RealQP, RealK, QK, N, + &RealZero); // NZ = 1 for a single real root. + } + + if (NZ == 0 && UpdateStatus == BAD_ANGLE) + break; // If RealIterate failed and UpdateTUV called this a bad angle, it's pointless to iterate further on this angle. + + } // Iter loop Note the use of NZ in the loop controls. + } // AngleNumber loop + } // TypeOfK loop + + // Done iterating. If NZ==0 at this point, we failed and will exit below. + // Decrement N, and set P to the quotient QP. QP = P/TUV or QP = P/(x-RealZero) + if (NZ == 2) // Store a pair of complex roots or 2 real roots. + { + j = Degree - N; + QuadRoots(TUV, &RealRoot[j], &ImagRoot[j]); + N -= NZ; + for (j = 0; j <= N; j++) + P[j] = QuadQP[j]; + TypeOfK = 0; + } + + if (NZ == 1) // Store a single real root + { + j = Degree - N; + RealRoot[j] = RealZero; + ImagRoot[j] = 0.0; + N -= NZ; + for (j = 0; j <= N; j++) + P[j] = RealQP[j]; + TypeOfK = 0; + } + + // Remove any trailing zeros on P. P[N] should never equal zero, but can approach zero + // because of roundoff errors. If P[N] is zero or tiny relative to P[N-1], we take the hit, + // and place a root at the origin. This needs to be checked, but virtually never happens. + while (fabsl(P[N]) <= TINY_VALUE * fabsl(P[N - 1]) && N > 0) { + j = Degree - N; + RealRoot[j] = 0.0; + ImagRoot[j] = 0.0; + N--; + //ShowMessage("Removed a zero at the origin."); + } + + } // The outermost loop while(N > 2) + + delete[] QuadQP; + delete[] RealQP; + delete[] QuadK; + delete[] RealK; + delete[] QK; + + // Done, except for the last 1 or 2 roots. If N isn't 1 or 2 at this point, we failed. + if (N == 1) { + j = Degree - N; + RealRoot[j] = -P[1] / P[0]; + ImagRoot[j] = 0.0; + delete[] P; + return (Degree); + } + + if (N == 2) { + j = Degree - N; + QuadRoots(P, &RealRoot[j], &ImagRoot[j]); + delete[] P; + return (Degree); + } + + if (N == 3) { + j = Degree - N; + CubicRoots(P, &RealRoot[j], &ImagRoot[j]); + delete[] P; + return (Degree); + } + + if (N == 4) { + j = Degree - N; + BiQuadRoots(P, &RealRoot[j], &ImagRoot[j]); + delete[] P; + return (Degree); + } + + // ShowMessage("The P51 root finder failed to converge on a solution."); + return (0); + +} + +//--------------------------------------------------------------------------- +// The purpose of this function is to find a 2nd order polynomial, TUV, which is a factor of P. +// When called, the TUV and K polynomials have been initialized to our best guess. +// This function can call UpdateTUV() at most QUAD_ITER_MAX times. This returns 2 if we find +// 2 roots, else 0. UpdateStatus has two possible values on return, UPDATED and BAD_ANGLE. +// UPDATED means the UpdateTUV function is able to do its calculations with TUV at this angle. +// BAD_ANGLE tells P51 to move TUV to a different angle before calling this function again. +// We use ErrScalar to account for the wide variations in N and P[N]. ErrScalar can vary by +// as much as 1E50 if all the root locations are much less than 1 or much greater than 1. +// The ErrScalar equation was determined empirically. If this root finder is used in an app +// where the root locations and poly orders fall within a narrow range, esp low order, then +// ErrScalar can be modified to give more accurate results. + +int QuadIterate(int P51_Iter, long double *P, long double *QP, long double *K, + long double *QK, int N, long double *TUV, TUpdateStatus *UpdateStatus) { + int Iter; + long double Err, MinErr, ErrScalar, QKCheck; + + ErrScalar = 1.0 / (16.0 * powl((long double) N, 3.0) * fabsl(P[N])); + + P51_Iter *= QUAD_ITER_MAX; + Err = MinErr = 1.0E100; + *UpdateStatus = UPDATED; + QuadSynDiv(P, N, TUV, QP); // Init QP + QuadSynDiv(K, N - 1, TUV, QK); // Init QK + + for (Iter = 0; Iter < QUAD_ITER_MAX; Iter++) { + UpdateTUV(P51_Iter + Iter, P, N, QP, K, QK, TUV, UpdateStatus); + if (*UpdateStatus == BAD_ANGLE) { + return (0); // Failure, need a different angle. + } + + Err = fabsl(QP[N - 1]) + fabsl(QP[N + 1]); // QP[N-1] & QP[N+1] are the remainder terms of P/TUV. + Err *= ErrScalar; // Normalize the error. + + if (Err < LDBL_EPSILON) { + return (2); // Success!! 2 roots have been found. + } + + // ZERO_DEL means both DelU and DelV were ~ 0 in UpdateTUV which means the algorithm has stalled. + // It might be stalled in a dead zone with large error, or stalled because it can't adjust u and v with a resolution fine enough to meet our convergence criteria. + if (*UpdateStatus == ZERO_DEL) { + if (Err < 4.0 * (long double) N * LDBL_EPSILON) // Small error, this is the best we can do. + { + *UpdateStatus = UPDATED; + return (2); + } else // Large error, get a different angle + { + *UpdateStatus = BAD_ANGLE; + return (0); + } + } + + QKCheck = fabsl(QK[N - 2]) + fabsl(QK[N]); // QK[N-2] & QK[N] are the remainder terms of K/TUV. + QKCheck *= ErrScalar; + + // Huge values indicate the algorithm is diverging and overflow is imminent. This can indicate + // a single real root, or that we simply need a different angle on TUV. This happens frequently. + if (Err > HUGE_VALUE || QKCheck > HUGE_VALUE) { + *UpdateStatus = BAD_ANGLE; + return (0); + } + + // Record our best result thus far. We turn on the damper in UpdateTUV if the errs increase. + if (Err < MinErr) { + *UpdateStatus = DAMPER_OFF; + MinErr = Err; + } else if (Iter > 2) { + *UpdateStatus = DAMPER_ON; + } + } + + // If we get here, we didn't converge, but TUV is getting updated. + // If RealIterate can't find a real zero, this function will get called again. + *UpdateStatus = UPDATED; + return (0); +} + +//--------------------------------------------------------------------------- + +// This function updates TUV[1] and TUV[2] using the J-T mathematics. +// When called, UpdateStatus equals either DAMPER_ON or DAMPER_OFF, which controls the damping factor. +// On return UpdateStatus equals UPDATED, BAD_ANGLE or ZERO_DEL; +// BAD_ANGLE indicates imminent overflow, or TUV[2] is going to zero. In either case the QuadIterate +// function will immediately return to P51 which will change the angle on TUV. ZERO_DEL indicates +// we have either stalled in a dead zone, or we lack the needed precision to further refine TUV. +// TUV = tx^2 + ux + v t = 1 always. v = 0 (a root at the origin) isn't allowed. +// The poly degrees are P[N] QP[N-2] K[N-1] QK[N-3] +void UpdateTUV(int Iter, long double *P, int N, long double *QP, long double *K, + long double *QK, long double *TUV, TUpdateStatus *UpdateStatus) { + int j; + long double DelU, DelV, Denom; + long double E2, E3, E4, E5; + static int FirstDamperlIter; + + if (Iter == 0) + FirstDamperlIter = 0; // Reset this static var. + if (*UpdateStatus == DAMPER_ON) + FirstDamperlIter = Iter; + + // Update K, unless E3 is tiny relative to E2. The algorithm will work its way out of a tiny E3. + // These equations are from the Jenkins and Traub paper "A Three Stage Algorithm for Real Polynomials Using Quadratic Iteration" Equation 9.8 + E2 = QP[N] * QP[N] + TUV[1] * QP[N] * QP[N - 1] + + TUV[2] * QP[N - 1] * QP[N - 1]; + E3 = QP[N] * QK[N - 1] + TUV[1] * QP[N] * QK[N - 2] + + TUV[2] * QP[N - 1] * QK[N - 2]; + + if (fabsl(E3) * HUGE_VALUE > fabsl(E2)) { + E2 /= -E3; + for (j = 1; j <= N - 2; j++) + K[j] = E2 * QK[j - 1] + QP[j]; // At covergence, E2 ~ 0, so K ~ QP. + } else { + for (j = 1; j <= N - 2; j++) + K[j] = QK[j - 1]; + } + K[0] = QP[0]; // QP[0] = 1.0 always + K[N - 1] = 0.0; + + QuadSynDiv(K, N - 1, TUV, QK); // Update QK QK = K/TUV + + // These equations are modified versions of those used in the original Jenkins Traub Fortran algorithm RealPoly, found at www.netlib.org/toms/493 + E3 = QP[N] * QK[N - 1] + TUV[1] * QP[N] * QK[N - 2] + + TUV[2] * QP[N - 1] * QK[N - 2]; + E4 = QK[N - 1] * QK[N - 1] + TUV[1] * QK[N - 1] * QK[N - 2] + + TUV[2] * QK[N - 2] * QK[N - 2]; + E5 = QP[N - 1] * QK[N - 1] - QP[N] * QK[N - 2]; + + Denom = E5 * K[N - 2] * TUV[2] + E4 * P[N]; + if (fabsl(Denom) * HUGE_VALUE < fabsl(P[N])) { + *UpdateStatus = BAD_ANGLE; // Denom is tiny, overflow is imminent, get a new angle. + return; + } + + // Calc DelU and DelV. If they are effectively zero, bump them by epsilon. + DelU = E3 * K[N - 2] * TUV[2] / Denom; + if (fabsl(DelU) < LDBL_EPSILON * fabsl(TUV[1])) { + if (DelU < 0.0) + DelU = -fabsl(TUV[1]) * LDBL_EPSILON; + else + DelU = fabsl(TUV[1]) * LDBL_EPSILON; + } + + DelV = -E5 * K[N - 2] * TUV[2] * TUV[2] / Denom; + if (fabsl(DelV) < LDBL_EPSILON * fabsl(TUV[2])) { + if (DelV < 0.0) + DelV = -fabsl(TUV[2]) * LDBL_EPSILON; + else + DelV = fabsl(TUV[2]) * LDBL_EPSILON; + } + + // If we haven't converged by QUAD_ITER_MAX iters, we need to test DelU and DelV for effectiveness. + if (Iter >= QUAD_ITER_MAX - 1) { + // We can't improve u and v any further because both DelU and DelV ~ 0 This usually happens when we are near a solution, but we don't have the precision needed to ine u and v enough to meet our convergence criteria. This can also happen in a dead zone where the errors are large, which means we need a different angle on TUV. We test for this in the QuadIterate function. + if (fabsl(DelU) < 8.0 * LDBL_EPSILON * fabsl(TUV[1]) + && fabsl(DelV) < 8.0 * LDBL_EPSILON * fabsl(TUV[2])) { + *UpdateStatus = ZERO_DEL; + return; + } + // A big change after this many iterations means we are wasting our time on this angle. + if (fabsl(DelU) > 10.0 * fabsl(TUV[1]) + || fabsl(DelV) > 10.0 * fabsl(TUV[2])) { + *UpdateStatus = BAD_ANGLE; + return; + } + } + + // Dampen the changes for 3 iterations after Damper was set in QuadIterate. + if (Iter - FirstDamperlIter < 3) { + DelU *= 0.75; + DelV *= 0.75; + } + + // Update U and V + TUV[1] += DelU; + if (fabsl(TUV[2] + DelV) < TINY_VALUE) + DelV *= 0.9; // If this, we would set TUV[2] = 0 which we can't allow, so we use 90% of DelV. + TUV[2] += DelV; + + if (fabsl(TUV[2]) < fabsl(TUV[1]) * TINY_VALUE) { + *UpdateStatus = BAD_ANGLE; // TUV[2] is effectively 0, which is never allowed. + return; + } + + *UpdateStatus = UPDATED; // TUV was updated. + QuadSynDiv(P, N, TUV, QP); // Update QP QP = P/TUV +} + +//--------------------------------------------------------------------------- + +// This function is used to find single real roots. It is similar to Newton's Method. +// Horners method is used to calculate QK and QP. For an explanation of these methods, see these 2 links. +// http://mathworld.wolfram.com/NewtonsMethod.html http://en.wikipedia.org/wiki/Horner%27s_method + +// When called, RealZero contains our best guess for a root, and K is init to the 1st derivative of P. +// The return value is either 1 or 0, the number of roots found. On return, RealZero contains +// the root, and QP contains the next P (i.e. P with this root removed). +// As with QuadIterate, at convergence, K = QP +int RealIterate(int P51_Iter, long double *P, long double *QP, long double *K, + long double *QK, int N, long double *RealZero) { + int Iter, k; + long double X, DelX, Damper, Err, ErrScalar; + static long double PrevQPN; + + ErrScalar = 1.0 / (16.0 * powl((long double) N, 2.0) * fabsl(P[N])); + + X = *RealZero; // Init with our best guess for X. + if (P51_Iter == 0) + PrevQPN = 0.0; + QK[0] = K[0]; + for (k = 1; k <= N - 1; k++) { + QK[k] = QK[k - 1] * X + K[k]; + } + + for (Iter = 0; Iter < REAL_ITER_MAX; Iter++) { + // Calculate a new QP. This is poly division QP = P/(x+X) QP[0] to QP[N-1] is the quotient. + // The remainder is QP[N], which is P(X), the error term. + QP[0] = P[0]; + for (k = 1; k <= N; k++) { + QP[k] = QP[k - 1] * X + P[k]; + } + Err = fabsl(QP[N]) * ErrScalar; // QP[N] is the error. ErrScalar accounts for the wide variations in N and P[N]. + + if (Err < LDBL_EPSILON) { + *RealZero = X; + return (1); // Success!! + } else if (Err > HUGE_VALUE) + return (0); // Overflow is imminent. + + // Calculate a new K. QK[N-1] is the remainder of K /(x-X). + // QK[N-1] is approximately P'(X) when P(X) = QP[N] ~ 0 + if (fabsl(QK[N - 1]) > fabsl(P[N] * TINY_VALUE)) { + DelX = -QP[N] / QK[N - 1]; + K[0] = QP[0]; + for (k = 1; k <= N - 1; k++) { + K[k] = DelX * QK[k - 1] + QP[k]; + } + } else // Use this if QK[N-1] ~ 0 + { + K[0] = 0.0; + for (k = 1; k <= N - 1; k++) + K[k] = QK[k - 1]; + } + + if (fabsl(K[N - 1]) > HUGE_VALUE) + return (0); // Overflow is imminent. + + // Calculate a new QK. This is poly division QK = K /(x+X). QK[0] to QK[N-2] is the quotient. + QK[0] = K[0]; + for (k = 1; k <= N - 1; k++) { + QK[k] = QK[k - 1] * X + K[k]; + } + if (fabsl(QK[N - 1]) <= TINY_VALUE * fabsl(P[N])) + return (0); // QK[N-1] ~ 0 will cause overflow below. + + // This algorithm routinely oscillates back and forth about a zero with nearly equal pos and neg error magnitudes. + // If the current and previous error add to give a value less than the current error, we dampen the change. + Damper = 1.0; + if (fabsl(QP[N] + PrevQPN) < fabsl(QP[N])) + Damper = 0.5; + PrevQPN = QP[N]; + + // QP[N] is P(X) and at convergence QK[N-1] ~ P'(X), so this is ~ Newtons Method. + DelX = QP[N] / QK[N - 1] * Damper; + if (X != 0.0) { + if (fabsl(DelX) < LDBL_EPSILON * fabsl(X)) // If true, the algorithm is stalled, so bump X by 2 epsilons. + { + if (DelX < 0.0) + DelX = -2.0 * X * LDBL_EPSILON; + else + DelX = 2.0 * X * LDBL_EPSILON; + } + } else // X = 0 + { + if (DelX == 0.0) + return (0); // Stalled at the origin, so exit. + } + X -= DelX; // Update X + + } // end of loop + + // If we get here, we failed to converge. + return (0); +} + +//--------------------------------------------------------------------------- + +// Derivative of P. Called from SetTUVandK(). +void DerivOfP(long double *P, int N, long double *dP) { + int j; + long double Power; + for (j = 0; j < N; j++) { + Power = N - j; + dP[j] = Power * P[j]; + } + dP[N] = 0.0; + +} +//--------------------------------------------------------------------------- + +// Synthetic Division of P by x^2 + ux + v (TUV) +// The qotient is Q[0] to Q[N-2]. The actual poly remainder terms are Q[N-1] and Q[N+1] +// The JT math requires the values Q[N-1] and Q[N] to calculate K. +// These form a 1st order remainder b*x + (b*u + a). +void QuadSynDiv(long double *P, int N, long double *TUV, long double *Q) { + int j; + Q[0] = P[0]; + Q[1] = P[1] - TUV[1] * Q[0]; + for (j = 2; j <= N; j++) { + Q[j] = P[j] - TUV[1] * Q[j - 1] - TUV[2] * Q[j - 2]; + } + +// Here we calculate the final remainder term used to calculate the convergence error. +// This and Q[N-1] are the remainder values you get if you do this poly division manually. + Q[N + 1] = Q[N - 1] * TUV[1] + Q[N]; // = b*u + a +} + +//--------------------------------------------------------------------------- + +// This function intializes the TUV and K polynomials. +void SetTUVandK(long double *P, int N, long double *TUV, long double *RealK, + long double *QuadK, long double X, int AngleNumber, int TypeOfQuadK) { + int j; + long double a, Theta; + + // These angles define our search pattern in the complex plane. We start in the 1st quadrant, + // go to the 2nd, then the real axis, etc. The roots are conjugates, so there isn't a need + // to go to the 3rd or 4th quadrants. The first 2 angles find about 99% of all roots. + const long double Angle[] = { 45.0, 135.0, 0.0, 90.0, 15.0, 30.0, 60.0, + 75.0, 105.0, 120.0, 150.0, + 165.0, // 12 angles + 6.0, 51.0, 96.0, 141.0, 12.0, 57.0, 102.0, 147.0, 21.0, 66.0, 111.0, + 156.0, 27.0, 72.0, 117.0, 162.0, 36.0, 81.0, 126.0, 171.0, 42.0, + 87.0, 132.0, 177.0, 3.0, 48.0, 93.0, 138.0, 9.0, 54.0, 99.0, 144.0, + 18.0, 63.0, 108.0, 153.0, 24.0, 69.0, 114.0, 159.0, 33.0, 78.0, + 123.0, 168.0, 39.0, 84.0, 129.0, + 174.0, // 60 angles + 46.0, 136.0, 91.0, 1.0, 16.0, 31.0, 61.0, 76.0, 106.0, 121.0, 151.0, + 166.0, 7.0, 52.0, 97.0, 142.0, 13.0, 58.0, 103.0, 148.0, 22.0, 67.0, + 112.0, 157.0, 28.0, 73.0, 118.0, 163.0, 37.0, 82.0, 127.0, 172.0, + 43.0, 88.0, 133.0, 178.0, 4.0, 49.0, 94.0, 139.0, 10.0, 55.0, 100.0, + 145.0, 19.0, 64.0, 109.0, 154.0, 25.0, 70.0, 115.0, 160.0, 34.0, + 79.0, 124.0, 169.0, 40.0, 85.0, 130.0, 175.0, 47.0, 137.0, 92.0, + 2.0, 17.0, 32.0, 62.0, 77.0, 107.0, 122.0, 152.0, 167.0, 8.0, 53.0, + 98.0, 143.0, 14.0, 59.0, 104.0, 149.0, 23.0, 68.0, 113.0, 158.0, + 29.0, 74.0, 119.0, 164.0, 38.0, 83.0, 128.0, 173.0, 44.0, 89.0, + 134.0, 179.0, 5.0, 50.0, 95.0, 140.0, 11.0, 56.0, 101.0, 146.0, + 20.0, 65.0, 110.0, 155.0, 26.0, 71.0, 116.0, 161.0, 35.0, 80.0, + 125.0, 170.0, 41.0, 86.0, 131.0, 176.0 }; // 180 angles + + // Initialize TUV to form (x - (a + jb)) * (x - (a - jb)) = x^2 - 2ax + a^2 + b^2 + // We init TUV for complex roots, except at angle 0, where we use real roots at +/- X + if (AngleNumber == 2) // At 0 degrees we int to 2 real roots at +/- X. + { + TUV[0] = 1.0; // t + TUV[1] = 0.0; // u + TUV[2] = -(X * X); // v + } else // We init to a complex root at a +/- jb + { + Theta = Angle[AngleNumber] / 180.0 * M_PI; + a = X * cosl(Theta); + //b = X * sinl(Theta); + TUV[0] = 1.0; + TUV[1] = -2.0 * a; + TUV[2] = X * X; // = a*a + b*b because cos^2 + sin^2 = 1 + } + + // The code below initializes the K polys used by RealIterate and QuadIterate. + + // Initialize the K poly used in RealIterate to P'. + DerivOfP(P, N, RealK); + RealK[N] = 0.0; + + // Initialize the K poly used in QuadIterate. Initializing QuadK to P" works virtually + // 100% of the time, but if P51 stalls on a difficult root, these alternate inits give + // us a way to work out of the stall. All these inits work almost as well as P". + if (TypeOfQuadK == 0) // Init QuadK 2nd derivative of P + { + long double *Temp = new (std::nothrow) long double[N + 2]; + if (Temp == NULL) { + //ShowMessage("Memory not Allocated in PFiftyOne SetTUVandK."); + return; + } + + DerivOfP(P, N, Temp); + DerivOfP(Temp, N - 1, QuadK); + QuadK[N] = QuadK[N - 1] = 0.0; + delete[] Temp; + } + + else if (TypeOfQuadK == 1) // Set QuadK to QP, because K = QP at convergence. + { + QuadSynDiv(P, N, TUV, QuadK); + QuadK[N] = QuadK[N - 1] = 0.0; + } + + else if (TypeOfQuadK == 2) // Set QuadK to the 1st derivative of P + { + for (j = 0; j <= N - 2; j++) + QuadK[j] = RealK[j + 1]; + QuadK[N] = QuadK[N - 1] = 0.0; + } + + else // Set QuadK to zero, except QuadK[0]. + { + for (j = 1; j <= N; j++) + QuadK[j] = 0.0; + QuadK[0] = 1.0; + } + + if (QuadK[0] == 0.0) + QuadK[0] = 1.0; // This can happen if TypeOfQuadK == 2 and P[1] == 0.0 + for (j = N - 2; j > 0; j--) + QuadK[j] /= QuadK[0]; + QuadK[0] = 1.0; +} + +//--------------------------------------------------------------------------- + +} // namespace diff --git a/kitiirfir/PFiftyOneRevE.h b/kitiirfir/PFiftyOneRevE.h new file mode 100644 index 000000000..7b34144ed --- /dev/null +++ b/kitiirfir/PFiftyOneRevE.h @@ -0,0 +1,43 @@ +//--------------------------------------------------------------------------- + +#ifndef PFiftyOneRevEH +#define PFiftyOneRevEH +//--------------------------------------------------------------------------- + +#include + +// TUpdateStatus represent the status of the UpdateTUV function. +// The first 3 are returned from UpdateTUV. The last two are sent to UpdateTUV. +enum TUpdateStatus {UPDATED, BAD_ANGLE, ZERO_DEL, DAMPER_ON, DAMPER_OFF}; + + +// This value for LDBL_EPSILON is for an 80 bit variable (64 bit significand). It should be in float.h +#define LDBL_EPSILON 1.084202172485504434E-19L // = 2^-63 +#define P51_MAXDEGREE 100 // The max poly order allowed. Used at the top of P51. This was set arbitrarily. +#define P51_ARRAY_SIZE 102 // P51 uses the new operator. P51 arrays must be MaxDegree + 2 +#define MAX_NUM_ANGLES 180 // The number of defined angles for initializing TUV. +#define REAL_ITER_MAX 20 // Max number of iterations in RealIterate. +#define QUAD_ITER_MAX 20 // Max number of iterations in QuadIterate. +#define MAX_NUM_K 4 // This is the number of methods we have to define K. +#define MAX_NEWT_ITERS 10 // Used to limit the Newton Iterations in the GetX function. +#define TINY_VALUE 1.0E-30 // This is what we use to test for zero. Usually to avoid divide by zero. +#define HUGE_VALUE 1.0E200 // This gets used to test for imminent overflow. + +namespace kitiirfir +{ + +int FindRoots(int N, double *Coeff, std::complex *Roots); +int PFiftyOne(long double *Coeff, int Degree, long double *RealRoot, long double *ImagRoot); +int QuadIterate(int Iter, long double *P, long double *QP, long double *K, long double *QK, int N, long double *TUV, TUpdateStatus *UpdateStatus); +void UpdateTUV(int Iter, long double *P, int N, long double *QP, long double *K, long double *QK, long double *TUV, TUpdateStatus *UpdateStatus); +int RealIterate(int P51_Iter, long double *P, long double *QP, long double *K, long double *QK, int N, long double *RealZero); +void QuadraticFormula(long double *TUV, long double *RealRoot, long double *ImagRoot); +void QuadSynDiv(long double *P, int N, long double *TUV, long double *Q); +void DerivOfP(long double *P, int N, long double *dP); +void SetTUVandK(long double *P, int N, long double *TUV, long double *RealK, long double *QuadK, long double X, int AngleNumber, int TypeOfQuadK); + +} // namespace + +#endif + + diff --git a/kitiirfir/QuadRootsRevH.cpp b/kitiirfir/QuadRootsRevH.cpp new file mode 100644 index 000000000..d51393341 --- /dev/null +++ b/kitiirfir/QuadRootsRevH.cpp @@ -0,0 +1,344 @@ +/* + By Daniel Klostermann + Iowa Hills Software, LLC IowaHills.com + If you find a problem, please leave a note at: + http://www.iowahills.com/feedbackcomments.html + Sept 12, 2016 Rev H + + This root solver code finds 1st, 2nd, 3rd, and 4th order roots algebraically, as + opposed to iteration. + + It is composed of the functions: QuadCubicRoots, QuadRoots, CubicRoots, and BiQuadRoots. + This code originated at: http://www.netlib.org/toms/ Algorithm 326 + Roots of Low Order Polynomials by Terence R.F.Nonweiler CACM (Apr 1968) p269 + Original C translation by: M.Dow Australian National University, Canberra, Australia + + We use the same basic mathematics used in that code, but in order to improve numerical + accuracy we made extensive modifications to the test conditions that control the flow of + the algorithm, and also added scaling and reversals. + + The input array Coeff[] contains the coefficients arranged in descending order. + For example, a 4th order poly would be: + P(x) = Coeff[0]*x^4 + Coeff[1]*x^3 + Coeff[2]*x^2 + Coeff[3]*x + Coeff[4] + + The roots are returned in RootsReal and RootsImag. N is the polynomial's order. 1 <= N <= 4 + N is modified if there are leading or trailing zeros. N is returned. + Coeff needs to be length N+1. RealRoot and ImagRoot need to be length N. + + Do not call QuadRoots, CubicRoots, and BiQuadRoots directly. They assume that QuadCubicRoots + has removed leading and trailing zeros and normalized P. + + On a 2 GHz Pentium it takes about 2 micro sec for QuadCubicRoots to return. + + ShowMessage is a C++ Builder function, and it usage has been commented out. + If you are using C++ Builder, include vcl.h for ShowMessage. + Otherwise replace ShowMessage with something appropriate for your compiler. + */ + +#include "QuadRootsRevH.h" +#include + +namespace kitiirfir +{ + +//--------------------------------------------------------------------------- + +// Same interface as P51. +int QuadCubicRoots(long double *Coeff, int N, long double *RealRoot, + long double *ImagRoot) { + if (N < 1 || N > 4) { + //ShowMessage("Invalid Poly Order in QuadCubicRoots()"); + return (0); + } + + int j; + long double P[5]; + + // Must init to zero, in case N is reduced. + for (j = 0; j < N; j++) + RealRoot[j] = ImagRoot[j] = 0.0; + for (j = 0; j < 5; j++) + P[j] = 0.0; + + // The functions below modify the coeff array, so we pass P instead of Coeff. + for (j = 0; j <= N; j++) + P[j] = (long double) Coeff[j]; + + // Remove trailing zeros. A tiny P[N] relative to P[N-1] is not allowed. + while (N > 0 && fabsl(P[N]) <= TINY_VALUE * fabsl(P[N - 1])) { + N--; + } + + // Remove leading zeros. + while (N > 0 && P[0] == 0.0) { + for (j = 0; j < N; j++) + P[j] = P[j + 1]; + N--; + } + + // P[0] must = 1 + if (P[0] != 1.0) { + for (j = 1; j <= N; j++) + P[j] /= P[0]; + P[0] = 1.0; + } + + // Calculate the roots. + if (N == 4) + BiQuadRoots(P, RealRoot, ImagRoot); + else if (N == 3) + CubicRoots(P, RealRoot, ImagRoot); + else if (N == 2) + QuadRoots(P, RealRoot, ImagRoot); + else if (N == 1) { + RealRoot[0] = -P[1] / P[0]; + ImagRoot[0] = 0.0; + } + + return (N); +} + +//--------------------------------------------------------------------------- + +// This function is the quadratic formula with P[0] = 1 +// y = P[0]*x^2 + P[1]*x + P[2] +// Normally, we don't allow P[2] = 0, but this can happen in a call from BiQuadRoots. +// If P[2] = 0, the zero is returned in RealRoot[0]. +void QuadRoots(long double *P, long double *RealRoot, long double *ImagRoot) { + long double D; + D = P[1] * P[1] - 4.0 * P[2]; + if (D >= 0.0) // 1 or 2 real roots + { + RealRoot[0] = (-P[1] - sqrtl(D)) * 0.5; // = -P[1] if P[2] = 0 + RealRoot[1] = (-P[1] + sqrtl(D)) * 0.5; // = 0 if P[2] = 0 + ImagRoot[0] = ImagRoot[1] = 0.0; + } else // 2 complex roots + { + RealRoot[0] = RealRoot[1] = -P[1] * 0.5; + ImagRoot[0] = sqrtl(-D) * 0.5; + ImagRoot[1] = -ImagRoot[0]; + } +} + +//--------------------------------------------------------------------------- +// This finds the roots of y = P0x^3 + P1x^2 + P2x+ P3 P[0] = 1 +void CubicRoots(long double *P, long double *RealRoot, long double *ImagRoot) { + int j; + long double s, t, b, c, d, Scalar; + bool CubicPolyReversed = false; + + // Scale the polynomial so that P[N] = +/-1. This moves the roots toward unit circle. + Scalar = powl(fabsl(P[3]), 1.0 / 3.0); + for (j = 1; j <= 3; j++) + P[j] /= powl(Scalar, (long double) j); + + if (fabsl(P[3]) < fabsl(P[2]) && P[2] > 0.0) { + ReversePoly(P, 3); + CubicPolyReversed = true; + } + + s = P[1] / 3.0; + b = (6.0 * P[1] * P[1] * P[1] - 27.0 * P[1] * P[2] + 81.0 * P[3]) / 162.0; + t = (P[1] * P[1] - 3.0 * P[2]) / 9.0; + c = t * t * t; + d = 2.0 * P[1] * P[1] * P[1] - 9.0 * P[1] * P[2] + 27.0 * P[3]; + d = d * d / 2916.0 - c; + + // if(d > 0) 1 complex and 1 real root. We use LDBL_EPSILON to account for round off err. + if (d > LDBL_EPSILON) { + d = powl((sqrtl(d) + fabsl(b)), 1.0 / 3.0); + if (d != 0.0) { + if (b > 0) + b = -d; + else + b = d; + c = t / b; + } + d = M_SQRT3_2 * (b - c); + b = b + c; + c = -b / 2.0 - s; + + RealRoot[0] = (b - s); + ImagRoot[0] = 0.0; + RealRoot[1] = RealRoot[2] = c; + ImagRoot[1] = d; + ImagRoot[2] = -ImagRoot[1]; + } + + else // d < 0.0 3 real roots + { + if (b == 0.0) + d = M_PI_2 / 3.0; // b can be as small as 1.0E-25 + else + d = atanl(sqrtl(fabsl(d)) / fabsl(b)) / 3.0; + + if (b < 0.0) + b = 2.0 * sqrtl(fabsl(t)); + else + b = -2.0 * sqrtl(fabsl(t)); + + c = cosl(d) * b; + t = -M_SQRT3_2 * sinl(d) * b - 0.5 * c; + + RealRoot[0] = (t - s); + RealRoot[1] = -(t + c + s); + RealRoot[2] = (c - s); + ImagRoot[0] = 0.0; + ImagRoot[1] = 0.0; + ImagRoot[2] = 0.0; + } + + // If we reversed the poly, the roots need to be inverted. + if (CubicPolyReversed) + InvertRoots(3, RealRoot, ImagRoot); + + // Apply the Scalar to the roots. + for (j = 0; j < 3; j++) + RealRoot[j] *= Scalar; + for (j = 0; j < 3; j++) + ImagRoot[j] *= Scalar; +} + +//--------------------------------------------------------------------------- + +// This finds the roots of y = P0x^4 + P1x^3 + P2x^2 + P3x + P4 P[0] = 1 +// This function calls CubicRoots and QuadRoots +void BiQuadRoots(long double *P, long double *RealRoot, long double *ImagRoot) { + int j; + long double a, b, c, d, e, Q3Limit, Scalar, Q[4], MinRoot; + bool QuadPolyReversed = false; + + // Scale the polynomial so that P[N] = +/- 1. This moves the roots toward unit circle. + Scalar = powl(fabsl(P[4]), 0.25); + for (j = 1; j <= 4; j++) + P[j] /= powl(Scalar, (long double) j); + + // Having P[1] < P[3] helps with the Q[3] calculation and test. + if (fabsl(P[1]) > fabsl(P[3])) { + ReversePoly(P, 4); + QuadPolyReversed = true; + } + + a = P[2] - P[1] * P[1] * 0.375; + b = P[3] + P[1] * P[1] * P[1] * 0.125 - P[1] * P[2] * 0.5; + c = P[4] + 0.0625 * P[1] * P[1] * P[2] + - 0.01171875 * P[1] * P[1] * P[1] * P[1] - 0.25 * P[1] * P[3]; + e = P[1] * 0.25; + + Q[0] = 1.0; + Q[1] = P[2] * 0.5 - P[1] * P[1] * 0.1875; + Q[2] = (P[2] * P[2] - P[1] * P[1] * P[2] + + 0.1875 * P[1] * P[1] * P[1] * P[1] - 4.0 * P[4] + P[1] * P[3]) + * 0.0625; + Q[3] = -b * b * 0.015625; + + /* The value of Q[3] can cause problems when it should have calculated to zero (just above) but + is instead ~ -1.0E-17 because of round off errors. Consequently, we need to determine whether + a tiny Q[3] is due to roundoff, or if it is legitimately small. It can legitimately have values + of ~ -1E-28. When this happens, we assume Q[2] should also be small. Q[3] can also be tiny with + 2 sets of equal real roots. Then P[1] and P[3], are approx equal. */ + + Q3Limit = ZERO_MINUS; + if (fabsl(fabsl(P[1]) - fabsl(P[3])) >= ZERO_PLUS && Q[3] > ZERO_MINUS + && fabsl(Q[2]) < 1.0E-5) + Q3Limit = 0.0; + + if (Q[3] < Q3Limit && fabsl(Q[2]) < 1.0E20 * fabsl(Q[3])) { + CubicRoots(Q, RealRoot, ImagRoot); + + // Find the smallest positive real root. One of the real roots is always positive. + MinRoot = 1.0E100; + for (j = 0; j < 3; j++) { + if (ImagRoot[j] == 0.0 && RealRoot[j] > 0 && RealRoot[j] < MinRoot) + MinRoot = RealRoot[j]; + } + + d = 4.0 * MinRoot; + a += d; + if (a * b < 0.0) + Q[1] = -sqrtl(d); + else + Q[1] = sqrtl(d); + b = 0.5 * (a + b / Q[1]); + } else { + if (Q[2] < 0.0) // 2 sets of equal imag roots + { + b = sqrtl(fabsl(c)); + d = b + b - a; + if (d > 0.0) + Q[1] = sqrtl(fabsl(d)); + else + Q[1] = 0.0; + } else { + if (Q[1] > 0.0) + b = 2.0 * sqrtl(fabsl(Q[2])) + Q[1]; + else + b = -2.0 * sqrtl(fabsl(Q[2])) + Q[1]; + Q[1] = 0.0; + } + } + + // Calc the roots from two 2nd order polys and subtract e from the real part. + if (fabsl(b) > 1.0E-8) { + Q[2] = c / b; + QuadRoots(Q, RealRoot, ImagRoot); + + Q[1] = -Q[1]; + Q[2] = b; + QuadRoots(Q, RealRoot + 2, ImagRoot + 2); + + for (j = 0; j < 4; j++) + RealRoot[j] -= e; + } else // b==0 with 4 equal real roots + { + for (j = 0; j < 4; j++) + RealRoot[j] = -e; + for (j = 0; j < 4; j++) + ImagRoot[j] = 0.0; + } + + // If we reversed the poly, the roots need to be inverted. + if (QuadPolyReversed) + InvertRoots(4, RealRoot, ImagRoot); + + // Apply the Scalar to the roots. + for (j = 0; j < 4; j++) + RealRoot[j] *= Scalar; + for (j = 0; j < 4; j++) + ImagRoot[j] *= Scalar; +} + +//--------------------------------------------------------------------------- + +// A reversed polynomial has its roots at the same angle, but reflected about the unit circle. +void ReversePoly(long double *P, int N) { + int j; + long double Temp; + for (j = 0; j <= N / 2; j++) { + Temp = P[j]; + P[j] = P[N - j]; + P[N - j] = Temp; + } + if (P[0] != 0.0) { + for (j = N; j >= 0; j--) + P[j] /= P[0]; + } +} + +//--------------------------------------------------------------------------- +// This is used in conjunction with ReversePoly +void InvertRoots(int N, long double *RealRoot, long double *ImagRoot) { + int j; + long double Mag; + for (j = 0; j < N; j++) { + // complex math for 1/x + Mag = RealRoot[j] * RealRoot[j] + ImagRoot[j] * ImagRoot[j]; + if (Mag != 0.0) { + RealRoot[j] /= Mag; + ImagRoot[j] /= -Mag; + } + } +} +//--------------------------------------------------------------------------- + +} // namespace diff --git a/kitiirfir/QuadRootsRevH.h b/kitiirfir/QuadRootsRevH.h new file mode 100644 index 000000000..f7e0e7544 --- /dev/null +++ b/kitiirfir/QuadRootsRevH.h @@ -0,0 +1,27 @@ +//--------------------------------------------------------------------------- + +#ifndef QuadRootsRevHH +#define QuadRootsRevHH +//--------------------------------------------------------------------------- + +#define LDBL_EPSILON 1.084202172485504434E-19L +// #define M_SQRT3 1.7320508075688772935274463L // sqrt(3) +#define M_SQRT3_2 0.8660254037844386467637231L // sqrt(3)/2 +// #define DBL_EPSILON 2.2204460492503131E-16 // 2^-52 typically defined in the compiler's float.h +#define ZERO_PLUS 8.88178419700125232E-16 // 2^-50 = 4*DBL_EPSILON +#define ZERO_MINUS -8.88178419700125232E-16 +#define TINY_VALUE 1.0E-30 // This is what we use to test for zero. Usually to avoid divide by zero. + +namespace kitiirfir +{ + +void QuadRoots(long double *P, long double *RealPart, long double *ImagPart); +void CubicRoots(long double *P, long double *RealPart, long double *ImagPart); +void BiQuadRoots(long double *P, long double *RealPart, long double *ImagPart); +void ReversePoly(long double *P, int N); +void InvertRoots(int N, long double *RealRoot, long double *ImagRoot); + +} // namespace + +#endif + diff --git a/kitiirfir/readme.md b/kitiirfir/readme.md new file mode 100644 index 000000000..86fff210b --- /dev/null +++ b/kitiirfir/readme.md @@ -0,0 +1,8 @@ +

IIR and FIR filter kit

+ +This is the code for IIR and FIR filter design that can be found [here](http://www.iowahills.com/) +Thnks to Iowa Hills Software for providing this code for free. + +This is only the filter design part that can be downloaded [here](http://www.iowahills.com/8DownloadPage.html) Under "IR FIR Source Code Kit". It also includes some dependencies found on the other files in this page. + +Code has been re-implemented in more modern C++ and moving it into a kitiirfir namespace \ No newline at end of file diff --git a/sdrbase/dsp/phaselockcomplex.cpp b/sdrbase/dsp/phaselockcomplex.cpp index 22693e257..16d32327a 100644 --- a/sdrbase/dsp/phaselockcomplex.cpp +++ b/sdrbase/dsp/phaselockcomplex.cpp @@ -128,22 +128,6 @@ void PhaseLockComplex::reset() m_lockCount = 0; } -void PhaseLockComplex::feedFLL(float re, float im) -{ - m_yRe = cos(m_phiHat); - m_yIm = sin(m_phiHat); - std::complex y(m_yRe, m_yIm); - std::complex x(re, im); - std::complex p = x * m_y; - float cross = m_p.real()*p.imag() - p.real()*m_p.imag(); - float dot = m_p.real()*p.real() + m_p.imag()*p.imag(); - float eF = cross * (dot < 0 ? -1 : 1); // frequency error - - m_freq += eF; // correct instantaneous frequency - m_phiHat += eF; // advance phase with instantaneous frequency - m_p = p; // store previous product -} - void PhaseLockComplex::feed(float re, float im) { m_yRe = cos(m_phiHat); @@ -210,7 +194,7 @@ void PhaseLockComplex::feed(float re, float im) else{ if (m_lockCount > 0) { m_lockCount--; - } + } } m_freqPrev = m_freq; diff --git a/sdrbase/dsp/phaselockcomplex.h b/sdrbase/dsp/phaselockcomplex.h index 4e2e64962..56f455328 100644 --- a/sdrbase/dsp/phaselockcomplex.h +++ b/sdrbase/dsp/phaselockcomplex.h @@ -47,8 +47,6 @@ public: void reset(); /** Feed PLL with a new signa sample */ void feed(float re, float im); - /** Same but turns into a FLL using the same filtering structure and NCO output. No lock condition. */ - void feedFLL(float re, float im); const std::complex& getComplex() const { return m_y; } float getReal() const { return m_yRe; } float getImag() const { return m_yIm; }