VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2025-04-04 18:48:34 -04:00
Code Issues Projects Releases Wiki Activity

Imported Iowa Hills Software IIR and FIR calculator

Browse Source
This commit is contained in:
f4exb 2018-05-17 00:09:56 +02:00
parent d38d926a87
commit c495f82235
23 changed files with 5069 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -219,6 +219,7 @@ endif()
##############################################################################
# base libraries
add_subdirectory(kitiirfir)
add_subdirectory(sdrbase)
add_subdirectory(sdrgui)
add_subdirectory(sdrsrv)

38
kitiirfir/CMakeLists.txt Normal file
View File

@ -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)

770
kitiirfir/FFTCode.cpp Normal file
View File

@ -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 <math.h>
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<N; j++) Input[j] = Buffer[j];
// We start with a swap because of the swap in ReArrangeInput.
TempPointer = InputR;
InputR = BufferR;
BufferR = TempPointer;
TempPointer = InputI;
InputI = BufferI;
BufferI = TempPointer;
I = 0;
for (j = 0; j < J; j++) {
T = 0;
for (k = 0; k < K; k++) // Loops N/2 times for every value of J and K
{
TempR = InputR[K + I] * TwiddleR[T]
- InputI[K + I] * TwiddleI[T];
TempI = InputR[K + I] * TwiddleI[T]
+ InputI[K + I] * TwiddleR[T];
BufferR[I] = InputR[I] + TempR;
BufferI[I] = InputI[I] + TempI;
BufferR[I + K] = InputR[I] - TempR;
BufferI[I + K] = InputI[I] - TempI;
I++;
T += J;
}
I += K;
}
K *= 2;
J /= 2;
}
}
//-----------------------------------------------------------------------------------------------
/*
The only difficulty in writing an FFT is figuring out how to calculate the various array indexes.
This shows how the index values change when doing a 16 pt FFT.
This print only has value if you compare it to a butterfly chart. Then you can
see how an FFT works. Use a 16 point decimation in time butterfly chart. We have one here:
http://www.iowahills.com/FFTCode.html
Note: The code above uses real variables. This print out came from code using complex variables as shown here.
Buffer[I] = Input[I] + Input[K+I] * Twiddle[T];
Buffer[I+K] = Input[I] - Input[K+I] * Twiddle[T];
N = 16
J = 8 K = 1
Buffer[0] = Input[0] + Input[1] * Twiddle[0] I = 0
Buffer[1] = Input[0] - Input[1] * Twiddle[0] I = 0
Buffer[2] = Input[2] + Input[3] * Twiddle[0] I = 2
Buffer[3] = Input[2] - Input[3] * Twiddle[0] I = 2
Buffer[4] = Input[4] + Input[5] * Twiddle[0] etc.
Buffer[5] = Input[4] - Input[5] * Twiddle[0]
Buffer[6] = Input[6] + Input[7] * Twiddle[0]
Buffer[7] = Input[6] - Input[7] * Twiddle[0]
Buffer[8] = Input[8] + Input[9] * Twiddle[0]
Buffer[9] = Input[8] - Input[9] * Twiddle[0]
Buffer[10] = Input[10] + Input[11] * Twiddle[0]
Buffer[11] = Input[10] - Input[11] * Twiddle[0]
Buffer[12] = Input[12] + Input[13] * Twiddle[0]
Buffer[13] = Input[12] - Input[13] * Twiddle[0]
Buffer[14] = Input[14] + Input[15] * Twiddle[0]
Buffer[15] = Input[14] - Input[15] * Twiddle[0]
J = 4 K = 2
Buffer[0] = Input[0] + Input[2] * Twiddle[0]
Buffer[2] = Input[0] - Input[2] * Twiddle[0]
Buffer[1] = Input[1] + Input[3] * Twiddle[4]
Buffer[3] = Input[1] - Input[3] * Twiddle[4]
Buffer[4] = Input[4] + Input[6] * Twiddle[0]
Buffer[6] = Input[4] - Input[6] * Twiddle[0]
Buffer[5] = Input[5] + Input[7] * Twiddle[4]
Buffer[7] = Input[5] - Input[7] * Twiddle[4]
Buffer[8] = Input[8] + Input[10] * Twiddle[0]
Buffer[10] = Input[8] - Input[10] * Twiddle[0]
Buffer[9] = Input[9] + Input[11] * Twiddle[4]
Buffer[11] = Input[9] - Input[11] * Twiddle[4]
Buffer[12] = Input[12] + Input[14] * Twiddle[0]
Buffer[14] = Input[12] - Input[14] * Twiddle[0]
Buffer[13] = Input[13] + Input[15] * Twiddle[4]
Buffer[15] = Input[13] - Input[15] * Twiddle[4]
J = 2 K = 4
Buffer[0] = Input[0] + Input[4] * Twiddle[0]
Buffer[4] = Input[0] - Input[4] * Twiddle[0]
Buffer[1] = Input[1] + Input[5] * Twiddle[2]
Buffer[5] = Input[1] - Input[5] * Twiddle[2]
Buffer[2] = Input[2] + Input[6] * Twiddle[4]
Buffer[6] = Input[2] - Input[6] * Twiddle[4]
Buffer[3] = Input[3] + Input[7] * Twiddle[6]
Buffer[7] = Input[3] - Input[7] * Twiddle[6]
Buffer[8] = Input[8] + Input[12] * Twiddle[0]
Buffer[12] = Input[8] - Input[12] * Twiddle[0]
Buffer[9] = Input[9] + Input[13] * Twiddle[2]
Buffer[13] = Input[9] - Input[13] * Twiddle[2]
Buffer[10] = Input[10] + Input[14] * Twiddle[4]
Buffer[14] = Input[10] - Input[14] * Twiddle[4]
Buffer[11] = Input[11] + Input[15] * Twiddle[6]
Buffer[15] = Input[11] - Input[15] * Twiddle[6]
J = 1 K = 8
Buffer[0] = Input[0] + Input[8] * Twiddle[0]
Buffer[8] = Input[0] - Input[8] * Twiddle[0]
Buffer[1] = Input[1] + Input[9] * Twiddle[1]
Buffer[9] = Input[1] - Input[9] * Twiddle[1]
Buffer[2] = Input[2] + Input[10] * Twiddle[2]
Buffer[10] = Input[2] - Input[10] * Twiddle[2]
Buffer[3] = Input[3] + Input[11] * Twiddle[3]
Buffer[11] = Input[3] - Input[11] * Twiddle[3]
Buffer[4] = Input[4] + Input[12] * Twiddle[4]
Buffer[12] = Input[4] - Input[12] * Twiddle[4]
Buffer[5] = Input[5] + Input[13] * Twiddle[5]
Buffer[13] = Input[5] - Input[13] * Twiddle[5]
Buffer[6] = Input[6] + Input[14] * Twiddle[6]
Buffer[14] = Input[6] - Input[14] * Twiddle[6]
Buffer[7] = Input[7] + Input[15] * Twiddle[7]
Buffer[15] = Input[7] - Input[15] * Twiddle[7]
*/
//-----------------------------------------------------------------------------------------------
// Discrete Fourier Transform ( textbook code )
// This takes the same arguments as the FFT function.
// 256 pts in 1.720 ms
void DFT(double *InputR, double *InputI, int N, int Type) {
int j, k, n;
double *SumR, *SumI, Sign, Arg;
double *TwiddleReal, *TwiddleImag;
SumR = new double[N];
SumI = new double[N];
TwiddleReal = new double[N];
TwiddleImag = new double[N];
if (SumR == NULL || SumI == NULL || TwiddleReal == NULL
|| TwiddleImag == NULL || (Type != FORWARD && Type != INVERSE)) {
// ShowMessage("Incorrect DFT Type or unable to allocate memory");
return;
}
// Calculate the twiddle factors and initialize the Sum arrays.
if (Type == FORWARD)
Sign = -1.0;
else
Sign = 1.0;
for (j = 0; j < N; j++) {
Arg = M_2PI * (double) j / (double) N;
TwiddleReal[j] = cos(Arg);
TwiddleImag[j] = Sign * sin(Arg);
SumR[j] = SumI[j] = 0.0;
}
//Calculate the DFT
for (j = 0; j < N; j++) // Sum index
for (k = 0; k < N; k++) // Input index
{
n = (j * k) % N;
SumR[j] += TwiddleReal[n] * InputR[k] - TwiddleImag[n] * InputI[k];
SumI[j] += TwiddleReal[n] * InputI[k] + TwiddleImag[n] * InputR[k];
}
// Scale the result if doing a forward DFT, and move the result to the input arrays.
if (Type == FORWARD) {
for (j = 0; j < N; j++)
InputR[j] = SumR[j] / (double) N;
for (j = 0; j < N; j++)
InputI[j] = SumI[j] / (double) N;
} else // Inverse DFT
{
for (j = 0; j < N; j++)
InputR[j] = SumR[j];
for (j = 0; j < N; j++)
InputI[j] = SumI[j];
}
delete[] SumR;
delete[] SumI;
delete[] TwiddleReal;
delete[] TwiddleImag;
}
//-----------------------------------------------------------------------------------------------
// This is a DFT for real valued samples. Since Samples is real, it can only do a forward DFT.
// The results are returned in OutputR OutputI
// 256 pts in 700 us 30 us to calc the twiddles
void RealSigDFT(double *Samples, double *OutputR, double *OutputI, int N) {
int j, k;
double Arg, *TwiddleReal, *TwiddleImag;
TwiddleReal = new double[N];
TwiddleImag = new double[N];
if (TwiddleReal == NULL || TwiddleImag == NULL) {
// ShowMessage("Failed to allocate memory in RealSigDFT");
return;
}
for (j = 0; j < N; j++) {
Arg = M_2PI * (double) j / (double) N;
TwiddleReal[j] = cos(Arg);
TwiddleImag[j] = -sin(Arg);
}
// Compute the DFT.
// We have a real input, so only do the pos frequencies. i.e. j<N/2
for (j = 0; j <= N / 2; j++) {
OutputR[j] = 0.0;
OutputI[j] = 0.0;
for (k = 0; k < N; k++) {
OutputR[j] += Samples[k] * TwiddleReal[(j * k) % N];
OutputI[j] += Samples[k] * TwiddleImag[(j * k) % N];
}
// Scale the result
OutputR[j] /= (double) N;
OutputI[j] /= (double) N;
}
// The neg freq components are the conj of the pos components because the input signal is real.
for (j = 1; j < N / 2; j++) {
OutputR[N - j] = OutputR[j];
OutputI[N - j] = -OutputI[j];
}
delete[] TwiddleReal;
delete[] TwiddleImag;
}
//---------------------------------------------------------------------------
// This is a single frequency DFT.
// This code uses iteration to calculate the Twiddle factors.
// To evaluate the frequency response of an FIR filter at Omega, set
// Samples[] = FirCoeff[] N = NumTaps 0.0 <= Omega <= 1.0
// 256 pts in 15.6 us
double SingleFreqDFT(double *Samples, int N, double Omega) {
int k;
double SumR, SumI, zR, zI, TwiddleR, TwiddleI, Temp;
TwiddleR = cos(Omega * M_PI);
TwiddleI = -sin(Omega * M_PI);
zR = 1.0; // z, as in e^(j*omega)
zI = 0.0;
SumR = 0.0;
SumI = 0.0;
for (k = 0; k < N; k++) {
SumR += Samples[k] * zR;
SumI += Samples[k] * zI;
// Calculate the complex exponential z by taking it to the kth power.
Temp = zR * TwiddleR - zI * TwiddleI;
zI = zR * TwiddleI + zI * TwiddleR;
zR = Temp;
}
/*
// This is the more conventional implementation of the loop above.
// It is a bit more accurate, but slower.
for(k=0; k<N; k++)
{
SumR += Samples[k] * cos((double)k * Omega * M_PI);
SumI += Samples[k] * -sin((double)k * Omega * M_PI);
}
*/
return (sqrt(SumR * SumR + SumI * SumI));
// return( ComplexD(SumR, SumI) );// if phase is needed.
}
//---------------------------------------------------------------------------
// Goertzel is essentially a single frequency DFT, but without phase information.
// Its simplicity allows it to run about 3 times faster than a single frequency DFT.
// It is typically used to find a tone embedded in a signal. A DTMF tone for example.
// 256 pts in 6 us
double Goertzel(double *Samples, int N, double Omega) {
int j;
double Reg0, Reg1, Reg2; // 3 shift registers
double CosVal, Mag;
Reg1 = Reg2 = 0.0;
CosVal = 2.0 * cos(M_PI * Omega);
for (j = 0; j < N; j++) {
Reg0 = Samples[j] + CosVal * Reg1 - Reg2;
Reg2 = Reg1; // Shift the values.
Reg1 = Reg0;
}
Mag = Reg2 * Reg2 + Reg1 * Reg1 - CosVal * Reg1 * Reg2;
if (Mag > 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

60
kitiirfir/FFTCode.h Normal file
View File

@ -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

401
kitiirfir/FIRFilterCode.cpp Normal file
View File

@ -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 <math.h>
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

36
kitiirfir/FIRFilterCode.h Normal file
View File

@ -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

228
kitiirfir/FreqSamplingCode.cpp Normal file
View File

@ -0,0 +1,228 @@
#include <math.h>
#include <complex>
#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<double> 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<double>(0.0, Omega);
H = std::complex<double>(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

17
kitiirfir/FreqSamplingCode.h Normal file
View File

@ -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

526
kitiirfir/IIRFilterCode.cpp Normal file
View File

@ -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 <complex>
#include <math.h>
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<double> 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<double>(-1.0, 0.0);
Roots[1] = std::complex<double>(1.0, 0.0);
Roots[2] = std::complex<double>(-1.0, 0.0);
Roots[3] = std::complex<double>(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<double> z1, z2, HofZ, Denom;
for (j = 0; j < NumPts; j++) {
Arg = M_PI * (double) j / (double) NumPts;
z1 = std::complex<double>(cos(Arg), -sin(Arg)); // z = e^(j*omega)
z2 = z1 * z1; // z squared
HofZ = std::complex<double>(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

38
kitiirfir/IIRFilterCode.h Normal file
View File

@ -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

469
kitiirfir/LowPassPrototypes.cpp Normal file
View File

@ -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 <complex>
#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<double> 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<double> 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<double>(0.0, Omega);
H = std::complex<double>(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<double> *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>((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<double> *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<double> 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

45
kitiirfir/LowPassPrototypes.h Normal file
View File

@ -0,0 +1,45 @@
//---------------------------------------------------------------------------
#ifndef LowPassPrototypesH
#define LowPassPrototypesH
#include <complex>
#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<double> *Roots, double *A2, double *A1, double *A0);
int RebuildPoly(int PolyCount, double *PolyCoeff, double *A2, double *A1, double *A0 );
void SortRootsByZeta(std::complex<double> *Roots, int Count, TOurSortTypes SortType);
bool HeapIndexSort(double *Data, int *Index, int N, TOurSortTypes SortType);
} // namespace
#endif

683
kitiirfir/LowPassRoots.cpp Normal file
View File

@ -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 <math.h>
#include <complex>
#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<double> *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<double>(cos(Theta), sin(Theta));
Roots[n++] = std::complex<double>(cos(Theta), -sin(Theta));
}
if (N % 2 == 1)
Roots[n++] = std::complex<double>(-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<double> *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<double>(Sigma, Omega);
Roots[n++] = std::complex<double>(Sigma, -Omega);
}
if (N % 2 == 1)
Roots[n++] = std::complex<double>(-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<double> *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<double> *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<double>(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<double> *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<double> *ChebyPoles,
std::complex<double> *ChebyZeros, int *ZeroCount) {
int j, k, N, PolesCount;
double Arg, Epsilon, ChebPolyCoeff[P51_ARRAY_SIZE],
PolyCoeff[P51_ARRAY_SIZE];
std::complex<double> 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<double>(0.0, 0.0);
for (j = 0; j <= N; j++)
for (k = 0; k <= N; k++) {
A = pow(std::complex<double>(0.0, 1.0), (double) j) * ChebPolyCoeff[j];
B = pow(std::complex<double>(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<double> *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<double> *EllipPoles, std::complex<double> *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<double> 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<double>(0.0, -1.0) / cos(std::complex<double>(-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<double>(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<double>(-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

27
kitiirfir/LowPassRoots.h Normal file
View File

@ -0,0 +1,27 @@
//---------------------------------------------------------------------------
#ifndef LowPassRootsH
#define LowPassRootsH
#include <complex>
#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<double> *Roots);
int GaussianPoly(int NumPoles, std::complex<double> *Roots);
int AdjustablePoly(int NumPoles, std::complex<double> *Roots, double Gamma);
int ChebyshevPoly(int NumPoles, double Ripple, std::complex<double> *Roots);
int BesselPoly(int NumPoles, std::complex<double> *Roots);
int InvChebyPoly(int NumPoles, double StopBanddB, std::complex<double> *ChebyPoles, std::complex<double> *ChebyZeros, int *ZeroCount);
int PapoulisPoly(int NumPoles, std::complex<double> *Roots);
int EllipticPoly(int FiltOrder, double Ripple, double DesiredSBdB, std::complex<double> *EllipPoles, std::complex<double> *EllipZeros, int *ZeroCount);
} // namespace
#endif

627
kitiirfir/NewParksMcClellan.cpp Normal file
View File

@ -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 <math.h>
#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<HalfTapCount
if (j == HalfTapCount + 2)
YNZ = COMP;
while (j <= HalfTapCount + 2) {
if (K1 > 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

31
kitiirfir/NewParksMcClellan.h Normal file
View File

@ -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

649
kitiirfir/PFiftyOneRevE.cpp Normal file
View File

@ -0,0 +1,649 @@
//---------------------------------------------------------------------------
#include "PFiftyOneRevE.h"
#include "QuadRootsRevH.h"
#include <math.h>
#include <complex>
//---------------------------------------------------------------------------
/*
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<double> *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>((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

43
kitiirfir/PFiftyOneRevE.h Normal file
View File

@ -0,0 +1,43 @@
//---------------------------------------------------------------------------
#ifndef PFiftyOneRevEH
#define PFiftyOneRevEH
//---------------------------------------------------------------------------
#include <complex>
// 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<double> *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

344
kitiirfir/QuadRootsRevH.cpp Normal file
View File

@ -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 <math.h>
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

27
kitiirfir/QuadRootsRevH.h Normal file
View File

@ -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

8
kitiirfir/readme.md Normal file
View File

@ -0,0 +1,8 @@
<h1>IIR and FIR filter kit</h1>
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

18
sdrbase/dsp/phaselockcomplex.cpp
View File

@ -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<float> y(m_yRe, m_yIm);
std::complex<float> x(re, im);
std::complex<float> 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;

2
sdrbase/dsp/phaselockcomplex.h
View File

@ -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<float>& getComplex() const { return m_y; }
float getReal() const { return m_yRe; }
float getImag() const { return m_yIm; }