Main Archive Specials Wiki | FAQ Links Submit Forum


Poor Man's FIWIZ

Type : Filter Design Utility
References : Posted by mailbjl[at]yahoo[dot]com

Notes :
FIWIZ is a neat little filter design utility that uses a genetic programming technique called Differential Evolution. As useful as it is, it looks like it took about a week to write, and is thus very undeserving of the $70 license fee. So I decided to write my own. There's a freely available optimizer class that uses Differential Evolution and I patched it together with some filter-specific logic.

Use of the utility requires the ability to do simple coding in C, but you need only revise a single function, which basically describes your filter specification. There's a big comment in the main source file that clarifies things a bit more.

Although it's not as easy to use as FIWIZ, it's arguably more powerful because your specifications are limited only by what you can express in C, whereas FIWIZ is completely GUI based.

Another thing: I'm afraid that due to the use of _kbhit and _getch, the code is a bit Microsofty, but you can take those out and the code will still be basically usable.


Code :
// First File: DESolver.cpp

#include
#include
#include
#include "DESolver.h"

#define Element(a,b,c) a[b*nDim+c]
#define RowVector(a,b) (&a[b*nDim])
#define CopyVector(a,b) memcpy((a),(b),nDim*sizeof(double))

DESolver::DESolver(int dim,int popSize) :
nDim(dim), nPop(popSize),
generations(0), strategy(stRand1Exp),
scale(0.7), probability(0.5), bestEnergy(0.0),
trialSolution(0), bestSolution(0),
popEnergy(0), population(0)
{
trialSolution = new double[nDim];
bestSolution = new double[nDim];
popEnergy = new double[nPop];
population = new double[nPop * nDim];

return;
}

DESolver::~DESolver(void)
{
if (trialSolution) delete trialSolution;
if (bestSolution) delete bestSolution;
if (popEnergy) delete popEnergy;
if (population) delete population;

trialSolution = bestSolution = popEnergy = population = 0;
return;
}

void DESolver::Setup(double *min,double *max,
int deStrategy,double diffScale,double crossoverProb)
{
int i;

strategy = deStrategy;
scale = diffScale;
probability = crossoverProb;

for (i=0; i < nPop; i++)
{
for (int j=0; j < nDim; j++)
Element(population,i,j) = RandomUniform(min[j],max[j]);

popEnergy[i] = 1.0E20;
}

for (i=0; i < nDim; i++)
bestSolution[i] = 0.0;

switch (strategy)
{
case stBest1Exp:
calcTrialSolution = &DESolver::Best1Exp;
break;

case stRand1Exp:
calcTrialSolution = &DESolver::Rand1Exp;
break;

case stRandToBest1Exp:
calcTrialSolution = &DESolver::RandToBest1Exp;
break;

case stBest2Exp:
calcTrialSolution = &DESolver::Best2Exp;
break;

case stRand2Exp:
calcTrialSolution = &DESolver::Rand2Exp;
break;

case stBest1Bin:
calcTrialSolution = &DESolver::Best1Bin;
break;

case stRand1Bin:
calcTrialSolution = &DESolver::Rand1Bin;
break;

case stRandToBest1Bin:
calcTrialSolution = &DESolver::RandToBest1Bin;
break;

case stBest2Bin:
calcTrialSolution = &DESolver::Best2Bin;
break;

case stRand2Bin:
calcTrialSolution = &DESolver::Rand2Bin;
break;
}

return;
}

bool DESolver::Solve(int maxGenerations)
{
int generation;
int candidate;
bool bAtSolution;
int generationsPerLoop = 10;

bestEnergy = 1.0E20;
bAtSolution = false;

for (generation=0;
(generation < maxGenerations) && !bAtSolution && (0 == _kbhit());
generation++)
{
for (candidate=0; candidate < nPop; candidate++)
{
(this->*calcTrialSolution)(candidate);
trialEnergy = EnergyFunction(trialSolution,bAtSolution);

if (trialEnergy < popEnergy[candidate])
{
// New low for this candidate
popEnergy[candidate] = trialEnergy;
CopyVector(RowVector(population,candidate),trialSolution);

// Check if all-time low
if (trialEnergy < bestEnergy)
{
bestEnergy = trialEnergy;
CopyVector(bestSolution,trialSolution);
}
}
}

if ((generation % generationsPerLoop) == (generationsPerLoop - 1))
{
printf("Gens %u Cost %.15g\n", generation+1, Energy());
}
}

if (0 != _kbhit())
{
_getch();
}

generations = generation;
return(bAtSolution);
}

void DESolver::Best1Exp(int candidate)
{
int r1, r2;
int n;

SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = bestSolution[n]
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Rand1Exp(int candidate)
{
int r1, r2, r3;
int n;

SelectSamples(candidate,&r1,&r2,&r3);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
- Element(population,r3,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::RandToBest1Exp(int candidate)
{
int r1, r2;
int n;

SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] += scale * (bestSolution[n] - trialSolution[n])
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Best2Exp(int candidate)
{
int r1, r2, r3, r4;
int n;

SelectSamples(candidate,&r1,&r2,&r3,&r4);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = bestSolution[n] +
scale * (Element(population,r1,n)
+ Element(population,r2,n)
- Element(population,r3,n)
- Element(population,r4,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Rand2Exp(int candidate)
{
int r1, r2, r3, r4, r5;
int n;

SelectSamples(candidate,&r1,&r2,&r3,&r4,&r5);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; (RandomUniform(0.0,1.0) < probability) && (i < nDim); i++)
{
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
+ Element(population,r3,n)
- Element(population,r4,n)
- Element(population,r5,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Best1Bin(int candidate)
{
int r1, r2;
int n;

SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = bestSolution[n]
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Rand1Bin(int candidate)
{
int r1, r2, r3;
int n;

SelectSamples(candidate,&r1,&r2,&r3);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
- Element(population,r3,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::RandToBest1Bin(int candidate)
{
int r1, r2;
int n;

SelectSamples(candidate,&r1,&r2);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] += scale * (bestSolution[n] - trialSolution[n])
+ scale * (Element(population,r1,n)
- Element(population,r2,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Best2Bin(int candidate)
{
int r1, r2, r3, r4;
int n;

SelectSamples(candidate,&r1,&r2,&r3,&r4);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = bestSolution[n]
+ scale * (Element(population,r1,n)
+ Element(population,r2,n)
- Element(population,r3,n)
- Element(population,r4,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::Rand2Bin(int candidate)
{
int r1, r2, r3, r4, r5;
int n;

SelectSamples(candidate,&r1,&r2,&r3,&r4,&r5);
n = (int)RandomUniform(0.0,(double)nDim);

CopyVector(trialSolution,RowVector(population,candidate));
for (int i=0; i < nDim; i++)
{
if ((RandomUniform(0.0,1.0) < probability) || (i == (nDim - 1)))
trialSolution[n] = Element(population,r1,n)
+ scale * (Element(population,r2,n)
+ Element(population,r3,n)
- Element(population,r4,n)
- Element(population,r5,n));
n = (n + 1) % nDim;
}

return;
}

void DESolver::SelectSamples(int candidate,int *r1,int *r2,
int *r3,int *r4,int *r5)
{
if (r1)
{
do
{
*r1 = (int)RandomUniform(0.0,(double)nPop);
}
while (*r1 == candidate);
}

if (r2)
{
do
{
*r2 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r2 == candidate) || (*r2 == *r1));
}

if (r3)
{
do
{
*r3 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r3 == candidate) || (*r3 == *r2) || (*r3 == *r1));
}

if (r4)
{
do
{
*r4 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r4 == candidate) || (*r4 == *r3) || (*r4 == *r2) || (*r4 == *r1));
}

if (r5)
{
do
{
*r5 = (int)RandomUniform(0.0,(double)nPop);
}
while ((*r5 == candidate) || (*r5 == *r4) || (*r5 == *r3)
|| (*r5 == *r2) || (*r5 == *r1));
}

return;
}

/*------Constants for RandomUniform()---------------------------------------*/
#define SEED 3
#define IM1 2147483563
#define IM2 2147483399
#define AM (1.0/IM1)
#define IMM1 (IM1-1)
#define IA1 40014
#define IA2 40692
#define IQ1 53668
#define IQ2 52774
#define IR1 12211
#define IR2 3791
#define NTAB 32
#define NDIV (1+IMM1/NTAB)
#define EPS 1.2e-7
#define RNMX (1.0-EPS)

double DESolver::RandomUniform(double minValue,double maxValue)
{
long j;
long k;
static long idum;
static long idum2=123456789;
static long iy=0;
static long iv[NTAB];
double result;

if (iy == 0)
idum = SEED;

if (idum <= 0)
{
if (-idum < 1)
idum = 1;
else
idum = -idum;

idum2 = idum;

for (j=NTAB+7; j>=0; j--)
{
k = idum / IQ1;
idum = IA1 * (idum - k*IQ1) - k*IR1;
if (idum < 0) idum += IM1;
if (j < NTAB) iv[j] = idum;
}

iy = iv[0];
}

k = idum / IQ1;
idum = IA1 * (idum - k*IQ1) - k*IR1;

if (idum < 0)
idum += IM1;

k = idum2 / IQ2;
idum2 = IA2 * (idum2 - k*IQ2) - k*IR2;

if (idum2 < 0)
idum2 += IM2;

j = iy / NDIV;
iy = iv[j] - idum2;
iv[j] = idum;

if (iy < 1)
iy += IMM1;

result = AM * iy;

if (result > RNMX)
result = RNMX;

result = minValue + result * (maxValue - minValue);
return(result);
}

// END FIRST FILE

// BEGIN SECOND FILE: DESolver.h
// Differential Evolution Solver Class
// Based on algorithms developed by Dr. Rainer Storn & Kenneth Price
// Written By: Lester E. Godwin
// PushCorp, Inc.
// Dallas, Texas
// 972-840-0208 x102
// godwin@pushcorp.com
// Created: 6/8/98
// Last Modified: 6/8/98
// Revision: 1.0

#if !defined(_DESOLVER_H)
#define _DESOLVER_H

#define stBest1Exp 0
#define stRand1Exp 1
#define stRandToBest1Exp 2
#define stBest2Exp 3
#define stRand2Exp 4
#define stBest1Bin 5
#define stRand1Bin 6
#define stRandToBest1Bin 7
#define stBest2Bin 8
#define stRand2Bin 9

class DESolver;

typedef void (DESolver::*StrategyFunction)(int);

class DESolver
{
public:
DESolver(int dim,int popSize);
~DESolver(void);

// Setup() must be called before solve to set min, max, strategy etc.
void Setup(double min[],double max[],int deStrategy,
double diffScale,double crossoverProb);

// Solve() returns true if EnergyFunction() returns true.
// Otherwise it runs maxGenerations generations and returns false.
virtual bool Solve(int maxGenerations);

// EnergyFunction must be overridden for problem to solve
// testSolution[] is nDim array for a candidate solution
// setting bAtSolution = true indicates solution is found
// and Solve() immediately returns true.
virtual double EnergyFunction(double testSolution[],bool &bAtSolution) = 0;

int Dimension(void) { return(nDim); }
int Population(void) { return(nPop); }

// Call these functions after Solve() to get results.
double Energy(void) { return(bestEnergy); }
double *Solution(void) { return(bestSolution); }

int Generations(void) { return(generations); }

protected:
void SelectSamples(int candidate,int *r1,int *r2=0,int *r3=0,
int *r4=0,int *r5=0);
double RandomUniform(double min,double max);

int nDim;
int nPop;
int generations;

int strategy;
StrategyFunction calcTrialSolution;
double scale;
double probability;

double trialEnergy;
double bestEnergy;

double *trialSolution;
double *bestSolution;
double *popEnergy;
double *population;

private:
void Best1Exp(int candidate);
void Rand1Exp(int candidate);
void RandToBest1Exp(int candidate);
void Best2Exp(int candidate);
void Rand2Exp(int candidate);
void Best1Bin(int candidate);
void Rand1Bin(int candidate);
void RandToBest1Bin(int candidate);
void Best2Bin(int candidate);
void Rand2Bin(int candidate);
};


// I added the following stuff 19 March 2007
// Brent Lehman

struct ASpectrum
{
unsigned mNumValues;
double* mReals;
double* mImags;
};

bool ComputeSpectrum(double* evenZeros, unsigned numEvenZeros, double* oddZero,
double* evenPoles, unsigned numEvenPoles, double* oddPole,
double gain, ASpectrum* spectrum);

class FilterSolver : public DESolver
{
public:
FilterSolver(int dim, int popSize, int spectrumSize,
unsigned numZeros, unsigned numPoles, bool minimumPhase) :
DESolver(dim, popSize)
{
mSpectrum.mNumValues = spectrumSize;
mSpectrum.mReals = new double[spectrumSize];
mSpectrum.mImags = new double[spectrumSize];
mNumZeros = numZeros;
mNumPoles = numPoles;
mMinimumPhase = minimumPhase;
}
virtual ~FilterSolver()
{
delete[] mSpectrum.mReals;
delete[] mSpectrum.mImags;
}
virtual double EnergyFunction(double testSolution[], bool& bAtSolution);
virtual ASpectrum* Spectrum() {return &mSpectrum;}
private:
unsigned mNumZeros;
unsigned mNumPoles;
bool mMinimumPhase;
ASpectrum mSpectrum;
};


#endif // _DESOLVER_H

// END SECOND FILE DESolver.h

// BEGIN FINAL FILE: FilterDesign.cpp
/*
*
* Filter Design Utility
* Source
*
* Brent Lehman
* 16 March 2007
*
*
*/


////////////////////////////////////////////////////////////////////
// //
// The idea is that an optimization algorithm passes a bunch of //
// different filter specifications to the function //
// "EnergyFunction" below. That function is supposed to //
// compute an "error" or "cost" value for each specification //
// it receives, which the algorithm uses to decide on other //
// filter specifications to try. Over the course of several //
// thousand different specifications, the algorithm will //
// eventually converge on a single best one. This one has the //
// lowest error value of all possible specifications. Thus, //
// you effectively tell the optimization algorithm what it's //
// looking for through code that you put into EnergyFunction. //
// //
// Look for a note in the code like this one to see what part //
// you need to change for your own uses. //
// //
////////////////////////////////////////////////////////////////////


#include
#include
#include
#include
#include
#include
#include "DESolver.h"


#define kIntIsOdd(x) (((x) & 0x00000001) == 1)


double FilterSolver::EnergyFunction(double testSolution[], bool& bAtSolution)
{
unsigned i;
double tempReal;
double tempImag;

// You probably will want to keep this if statement and its contents
if (mMinimumPhase)
{
// Make sure there are no zeros outside the unit circle
unsigned lastEvenZero = (mNumZeros & 0xfffffffe) - 1;
for (i = 0; i <= lastEvenZero; i+=2)
{
tempReal = testSolution[i];
tempImag = testSolution[i+1];
if ((tempReal*tempReal + tempImag*tempImag) > 1.0)
{
return 1.0e+300;
}
}

if (kIntIsOdd(mNumZeros))
{
tempReal = testSolution[mNumZeros - 1];
if ((tempReal * tempReal) > 1.0)
{
return 1.0e+300;
}
}
}

// Make sure there are no poles on or outside the unit circle
// You probably will want to keep this too
unsigned lastEvenPole = mNumZeros + (mNumPoles & 0xfffffffe) - 2;
for (i = mNumZeros; i <= lastEvenPole; i+=2)
{
tempReal = testSolution[i];
tempImag = testSolution[i+1];
if ((tempReal*tempReal + tempImag*tempImag) > 0.999999999)
{
return 1.0e+300;
}
}

// If you keep the for loop above, keep this too
if (kIntIsOdd(mNumPoles))
{
tempReal = testSolution[mNumZeros + mNumPoles - 1];
if ((tempReal * tempReal) > 1.0)
{
return 1.0e+300;
}
}

double* evenZeros = &(testSolution[0]);
double* evenPoles = &(testSolution[mNumZeros]);
double* oddZero = NULL;
double* oddPole = NULL;
double gain = testSolution[mNumZeros + mNumPoles];

if (kIntIsOdd(mNumZeros))
{
oddZero = &(testSolution[mNumZeros - 1]);
}

if (kIntIsOdd(mNumPoles))
{
oddPole = &(testSolution[mNumZeros + mNumPoles - 1]);
}

ComputeSpectrum(evenZeros, mNumZeros & 0xfffffffe, oddZero,
evenPoles, mNumPoles & 0xfffffffe, oddPole,
gain, &mSpectrum);

unsigned numPoints = mSpectrum.mNumValues;

/////////////////////////////////////////////////////////////////
// //
// Use the impulse response, held in the variable //
// "mSpectrum", to compute a score for the solution that //
// has been passed into this function. You probably don't //
// want to touch any of the code above this point, but //
// from here to the end of this function, it's all you! //
// //
/////////////////////////////////////////////////////////////////

#define kLnTwoToThe127 88.02969193111305
#define kRecipLn10 0.4342944819032518

// Compute square sum of errors for magnitude
double magnitudeError = 0.0;
double magnitude = 0.0;
double logMagnitude = 0.0;
tempReal = mSpectrum.mReals[0];
tempImag = mSpectrum.mImags[0];
magnitude = tempReal*tempReal + tempImag*tempImag;
double baseMagnitude = 0.0;
if (0.0 == magnitude)
{
baseMagnitude = -kLnTwoToThe127;
}
else
{
baseMagnitude = log(magnitude) * kRecipLn10;
baseMagnitude *= 0.5;
}

for (i = 0; i < numPoints; i++)
{
tempReal = mSpectrum.mReals[i];
tempImag = mSpectrum.mImags[i];
magnitude = tempReal*tempReal + tempImag*tempImag;
if (0.0 == magnitude)
{
logMagnitude = -kLnTwoToThe127;
}
else
{
logMagnitude = log(magnitude) * kRecipLn10;
logMagnitude *= 0.5; // Half the log because it's mag squared
}

logMagnitude -= baseMagnitude;
magnitudeError += logMagnitude * logMagnitude;
}

// Compute errors for phase
double phaseError = 0.0;
double phase = 0.0;
double componentError = 0.0;
double degree = 1;//((mNumZeros + 1) & 0xfffffffe) - 1;
double angleSpacing = -3.141592653589793 * 0.5 / numPoints * degree;
double targetPhase = 0.0;
double oldPhase = 0.0;
double phaseDifference = 0;
double totalPhaseTraversal = 0.0;
double traversalError = 0.0;
for (i = 0; i < (numPoints - 5); i++)
{
tempReal = mSpectrum.mReals[i];
tempImag = mSpectrum.mImags[i];
oldPhase = phase;
phase = atan2(tempImag, tempReal);
phaseDifference = phase - oldPhase;
if (phaseDifference > 3.141592653589793)
{
phaseDifference -= 3.141592653589793;
phaseDifference -= 3.141592653589793;
}
else if (phaseDifference < -3.141592653589793)
{
phaseDifference += 3.141592653589793;
phaseDifference += 3.141592653589793;
}
totalPhaseTraversal += phaseDifference;
componentError = cosh(200.0*(phaseDifference - angleSpacing)) - 0.5;
phaseError += componentError * componentError;
targetPhase += angleSpacing;
if (targetPhase < -3.141592653589793)
{
targetPhase += 3.141592653589793;
targetPhase += 3.141592653589793;
}
}

traversalError = totalPhaseTraversal - angleSpacing * numPoints;
traversalError *= traversalError;

double baseMagnitudeError = baseMagnitude * baseMagnitude;

// Compute weighted sum of the two subtotals
// Take square root
return sqrt(baseMagnitudeError*1.0 + magnitudeError*100.0 +
phaseError*400.0 + traversalError*4000000.0);
}


///////////////////////////////
int main(int argc, char** argv)
{
srand((unsigned)time(NULL));

unsigned numZeros;
unsigned numPoles;
bool minimumPhase;

if (argc < 4)
{
printf("Usage: FilterDesign.exe \n");
return 0;
}
else
{
if (0 == atoi(argv[1]))
{
minimumPhase = false;
}
else
{
minimumPhase = true;
}

numZeros = (unsigned)atoi(argv[2]);
if (0 == numZeros)
{
numZeros = 1;
}

numPoles = (unsigned)atoi(argv[3]);
}

unsigned vectorLength = numZeros + numPoles + 1;
unsigned populationSize = vectorLength * 10;
FilterSolver theSolver(vectorLength, populationSize, 200,
numZeros, numPoles, minimumPhase);

double* minimumSolution = new double[vectorLength];
unsigned i;
if (minimumPhase)
{
for (i = 0; i < numZeros; i++)
{
minimumSolution[i] = -sqrt(0.5);
}
}
else
{
for (i = 0; i < numZeros; i++)
{
minimumSolution[i] = -10.0;
}
}

for (; i < (vectorLength - 1); i++)
{
minimumSolution[i] = -sqrt(0.5);
}

minimumSolution[vectorLength - 1] = 0.0;

double* maximumSolution = new double[vectorLength];
if (minimumPhase)
{
for (i = 0; i < numZeros; i++)
{
maximumSolution[i] = sqrt(0.5);
}
}
else
{
for (i = 0; i < numZeros; i++)
{
maximumSolution[i] = 10.0;
}
}

for (i = 0; i < (vectorLength - 1); i++)
{
maximumSolution[i] = sqrt(0.5);
}

maximumSolution[vectorLength - 1] = 2.0;

theSolver.Setup(minimumSolution, maximumSolution, 0, 0.5, 0.75);
theSolver.Solve(1000000);

double* bestSolution = theSolver.Solution();
printf("\nZeros:\n");
unsigned numEvenZeros = numZeros & 0xfffffffe;
for (i = 0; i < numEvenZeros; i+=2)
{
printf("%.10f +/- %.10fi\n", bestSolution[i], bestSolution[i+1]);
}

if (kIntIsOdd(numZeros))
{
printf("%.10f\n", bestSolution[numZeros-1]);
}

printf("Poles:\n");
unsigned lastEvenPole = numZeros + (numPoles & 0xfffffffe) - 2;
for (i = numZeros; i <= lastEvenPole; i+=2)
{
printf("%.10f +/- %.10fi\n", bestSolution[i], bestSolution[i+1]);
}

unsigned numRoots = numZeros + numPoles;
if (kIntIsOdd(numPoles))
{
printf("%.10f\n", bestSolution[numRoots-1]);
}

double gain = bestSolution[numRoots];
printf("Gain: %.10f\n", gain);

_getch();
unsigned j;
ASpectrum* spectrum = theSolver.Spectrum();
double logMagnitude;
printf("Magnitude Response, millibels:\n");
for (i = 0; i < 20; i++)
{
for (j = 0; j < 10; j++)
{
logMagnitude = kRecipLn10 *
log(spectrum->mReals[i*10 + j] * spectrum->mReals[i*10 + j] +
spectrum->mImags[i*10 + j] * spectrum->mImags[i*10 + j]);
if (logMagnitude < -9.999)
{
logMagnitude = -9.999;
}
printf("%+5.0f ", logMagnitude*1000);
}
printf("\n");
}

_getch();
double phase;
printf("Phase Response, milliradians:\n");
for (i = 0; i < 20; i++)
{
for (j = 0; j < 10; j++)
{
phase = atan2(spectrum->mImags[i*10 + j], spectrum->mReals[i*10 + j]);
printf("%+5.0f ", phase*1000);
}
printf("\n");
}

_getch();
printf("Biquad Sections:\n");
unsigned numBiquadSections =
(numZeros > numPoles) ? ((numZeros + 1) >> 1) : ((numPoles + 1) >> 1);
double x0, x1, x2;
double y0, y1, y2;
if (numZeros >=2)
{
x0 = (bestSolution[0]*bestSolution[0] + bestSolution[1]*bestSolution[1]) *
gain;
x1 = 2.0 * bestSolution[0] * gain;
x2 = gain;
}
else if (1 == numZeros)
{
x0 = bestSolution[0] * gain;
x1 = gain;
x2 = 0.0;
}
else
{
x0 = gain;
x1 = 0.0;
x2 = 0.0;
}

if (numPoles >= 2)
{
y0 = (bestSolution[numZeros]*bestSolution[numZeros] +
bestSolution[numZeros+1]*bestSolution[numZeros+1]);
y1 = 2.0 * bestSolution[numZeros];
y2 = 1.0;
}
else if (1 == numPoles)
{
y0 = bestSolution[numZeros];
y1 = 1.0;
y2 = 0.0;
}
else
{
y0 = 1.0;
y1 = 0.0;
y2 = 0.0;
}

x0 /= y0;
x1 /= y0;
x2 /= y0;
y1 /= y0;
y2 /= y0;

printf("y[n] = %.10fx[n]", x0);
if (numZeros > 0)
{
printf(" + %.10fx[n-1]", x1);
}
if (numZeros > 1)
{
printf(" + %.10fx[n-2]", x2);
}
printf("\n");

if (numPoles > 0)
{
printf(" + %.10fy[n-1]", y1);
}
if (numPoles > 1)
{
printf(" + &.10fy[n-2]", y2);
}
if (numPoles > 0)
{
printf("\n");
}

int numRemainingZeros = numZeros - 2;
int numRemainingPoles = numPoles - 2;
for (i = 1; i < numBiquadSections; i++)
{
if (numRemainingZeros >= 2)
{
x0 = (bestSolution[i*2] * bestSolution[i*2] +
bestSolution[i*2+1] * bestSolution[i*2+1]);
x1 = -2.0 * bestSolution[i*2];
x2 = 1.0;
}
else if (numRemainingZeros >= 1)
{
x0 = bestSolution[i*2];
x1 = 1.0;
x2 = 0.0;
}
else
{
x0 = 1.0;
x1 = 0.0;
x2 = 0.0;
}

if (numRemainingPoles >= 2)
{
y0 = (bestSolution[i*2+numZeros] * bestSolution[i*2+numZeros] +
bestSolution[i*2+numZeros+1] * bestSolution[i*2+numZeros+1]);
y1 = -2.0 * bestSolution[i*2+numZeros];
y2 = 1.0;
}
else if (numRemainingPoles >= 1)
{
y0 = bestSolution[i*2+numZeros];
y1 = 1.0;
y2 = 0.0;
}
else
{
y0 = 1.0;
y1 = 0.0;
y2 = 0.0;
}

x0 /= y0;
x1 /= y0;
x2 /= y0;
y1 /= y0;
y2 /= y0;

printf("y[n] = %.10fx[n]", x0);
if (numRemainingZeros > 0)
{
printf(" + %.10fx[n-1]", x1);
}
if (numRemainingZeros > 1)
{
printf(" + %.10fx[n-2]", x2);
}
printf("\n");

if (numRemainingPoles > 0)
{
printf(" + %.10fy[n-1]", -y1);
}
if (numRemainingPoles > 1)
{
printf(" + %.10fy[n-2]", -y2);
}
if (numRemainingPoles > 0)
{
printf("\n");
}

numRemainingZeros -= 2;
numRemainingPoles -= 2;
}

_getch();
printf("Full Expansion:\n");
double* xpolynomial = new double[numRoots + 1];
memset(xpolynomial, 0, sizeof(double) * (numRoots + 1));
xpolynomial[0] = 1.0;
if (numZeros >= 2)
{
xpolynomial[0] = bestSolution[0] * bestSolution[0] +
bestSolution[1] * bestSolution[1];
xpolynomial[1] = -2.0 * bestSolution[0];
xpolynomial[2] = 1.0;
}
else if (numZeros == 1)
{
xpolynomial[0] = bestSolution[0];
xpolynomial[1] = 1.0;
}
else
{
xpolynomial[0] = 1.0;
}

for (i = 2, numRemainingZeros = numZeros; numRemainingZeros >= 2;
i += 2, numRemainingZeros-=2)
{
x2 = 1.0;
x1 = -2.0 * bestSolution[i];
x0 = bestSolution[i] * bestSolution[i] +
bestSolution[i+1] * bestSolution[i+1];
for (j = numRoots; j > 1; j--)
{
xpolynomial[j] = xpolynomial[j-2] + xpolynomial[j-1] * x1 +
xpolynomial[j] * x0;
}
xpolynomial[1] = xpolynomial[0] * x1 + xpolynomial[1] * x0;
xpolynomial[0] *= x0;
}

if (numRemainingZeros > 0)
{
x1 = 1.0;
x0 = bestSolution[numZeros-1];
for (j = numRoots; j > 0; j--)
{
xpolynomial[j] = xpolynomial[j-1] + xpolynomial[j] * x0;
}
xpolynomial[0] *= x0;
}

double* ypolynomial = new double[numRoots + 1];
memset(ypolynomial, 0, sizeof(double) * (numRoots + 1));
ypolynomial[0] = 1.0;
if (numPoles >= 2)
{
ypolynomial[0] = bestSolution[numZeros] * bestSolution[numZeros] +
bestSolution[numZeros+1] * bestSolution[numZeros+1];
ypolynomial[1] = -2.0 * bestSolution[numZeros];
ypolynomial[2] = 1.0;
}
else if (numPoles == 1)
{
ypolynomial[0] = bestSolution[numZeros];
ypolynomial[1] = 1.0;
}
else
{
xpolynomial[0] = 1.0;
}

for (i = 2, numRemainingPoles = numPoles; numRemainingPoles >= 2;
i += 2, numRemainingPoles-=2)
{
y2 = 1.0;
y1 = -2.0 * bestSolution[numZeros+i];
y0 = bestSolution[numZeros+i] * bestSolution[numZeros+i] +
bestSolution[numZeros+i+1] * bestSolution[numZeros+i+1];
for (j = numRoots; j > 1; j--)
{
ypolynomial[j] = ypolynomial[j-2] + ypolynomial[j-1] * y1 +
ypolynomial[j] * y0;
}
ypolynomial[1] = ypolynomial[0] * y1 + ypolynomial[1] * y0;
ypolynomial[0] *= y0;
}

if (numRemainingPoles > 0)
{
y1 = 1.0;
y0 = bestSolution[numZeros+numPoles-1];
for (j = numRoots; j > 0; j--)
{
ypolynomial[j] = ypolynomial[j-1] + ypolynomial[j] * y0;
}
ypolynomial[0] *= y0;
}

y0 = ypolynomial[0];
for (i = 0; i <= numRoots; i++)
{
xpolynomial[i] /= y0;
ypolynomial[i] /= y0;
}

printf("y[n] = %.10fx[n]", xpolynomial[0]*gain);
for (i = 1; i <= numZeros; i++)
{
printf(" + %.10fx[n-%d]", xpolynomial[i]*gain, i);
if ((i % 3) == 2)
{
printf("\n");
}
}

if ((i % 3) != 0)
{
printf("\n");
}

if (numPoles > 0)
{
printf(" ");
}

for (i = 1; i <= numPoles; i++)
{
printf(" + %.10fy[n-%d]", -ypolynomial[i], i);
if ((i % 3) == 2)
{
printf("\n");
}
}

if ((i % 3) != 0)
{
printf("\n");
}

delete[] minimumSolution;
delete[] maximumSolution;
delete[] xpolynomial;
delete[] ypolynomial;
}


bool ComputeSpectrum(double* evenZeros, unsigned numEvenZeros, double* oddZero,
double* evenPoles, unsigned numEvenPoles, double* oddPole,
double gain, ASpectrum* spectrum)
{
unsigned i, j;

// For equally spaced points on the unit circle
unsigned numPoints = spectrum->mNumValues;
double spacingAngle = 3.141592653589793 / (numPoints - 1);
double pointArgument = 0.0;
double pointReal = 0.0;
double pointImag = 0.0;
double rootReal = 0.0;
double rootImag = 0.0;
double differenceReal = 0.0;
double differenceImag = 0.0;
double responseReal = 1.0;
double responseImag = 0.0;
double recipSquareMagnitude = 0.0;
double recipReal = 0.0;
double recipImag = 0.0;
double tempRealReal = 0.0;
double tempRealImag = 0.0;
double tempImagReal = 0.0;
double tempImagImag = 0.0;

for (i = 0; i < numPoints; i++)
{
responseReal = 1.0;
responseImag = 0.0;

// The imaginary component is negated because we're using 1/z, not z
pointReal = cos(pointArgument);
pointImag = -sin(pointArgument);

// For each even zero
for (j = 0; j < numEvenZeros; j+=2)
{
rootReal = evenZeros[j];
rootImag = evenZeros[j + 1];
// Compute distance from that zero to that point
differenceReal = pointReal - rootReal;
differenceImag = pointImag - rootImag;
// Multiply that distance by the accumulating product
tempRealReal = responseReal * differenceReal;
tempRealImag = responseReal * differenceImag;
tempImagReal = responseImag * differenceReal;
tempImagImag = responseImag * differenceImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
// Do the same with the conjugate root
differenceImag = pointImag + rootImag;
tempRealReal = responseReal * differenceReal;
tempRealImag = responseReal * differenceImag;
tempImagReal = responseImag * differenceReal;
tempImagImag = responseImag * differenceImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
// The following way is little faster, if any
// response *= (1/z - r) * (1/z - conj(r))
// *= r*conj(r) - (r + conj(r))/z + 1/(z*z)
// *= real(r)*real(r) + imag(r)*imag(r) - 2*real(r)/z + 1/(z*z)
// *= ... - 2*real(r)*conj(z) + conj(z)*conj(z)
// *= ... - 2*real(r)*real(z) + 2i*real(r)*imag(z) +
// real(z)*real(z) - 2i*real(z)*imag(z) + imag(z)*imag(z)
// *= real(r)*real(r) + imag(r)*imag(r) - 2*real(r)*real(z) +
// real(z)*real(z) + imag(z)*imag(z) +
// 2i * imag(z) * (real(r) - real(z))
// *= (real(r) - real(z))^2 + imag(r)^2 + imag(z)^2 +
// 2i * imag(z) * (real(r) - real(z))
// This ends up being 8 multiplications, 6 additions
}

if (NULL != oddZero)
{
rootReal = *oddZero;
// Compute distance from that zero to that point
differenceReal = pointReal - rootReal;
differenceImag = pointImag;
// Multiply that distance by the accumulating product
tempRealReal = responseReal * differenceReal;
tempRealImag = responseReal * differenceImag;
tempImagReal = responseImag * differenceReal;
tempImagImag = responseImag * differenceImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
}

// For each pole
for (j = 0; j < numEvenPoles; j+=2)
{
rootReal = evenPoles[j];
rootImag = evenPoles[j + 1];
// Compute distance from that pole to that point
differenceReal = pointReal - rootReal;
differenceImag = pointImag - rootImag;
// Multiply the reciprocal of that distance by the accumulating product
recipSquareMagnitude = 1.0 / (differenceReal * differenceReal +
differenceImag * differenceImag);
recipReal = differenceReal * recipSquareMagnitude;
recipImag = -differenceImag * recipSquareMagnitude;
tempRealReal = responseReal * recipReal;
tempRealImag = responseReal * recipImag;
tempImagReal = responseImag * recipReal;
tempImagImag = responseImag * recipImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
// Do the same with the conjugate root
differenceImag = pointImag + rootImag;
recipSquareMagnitude = 1.0 / (differenceReal * differenceReal +
differenceImag * differenceImag);
recipReal = differenceReal * recipSquareMagnitude;
recipImag = -differenceImag * recipSquareMagnitude;
tempRealReal = responseReal * recipReal;
tempRealImag = responseReal * recipImag;
tempImagReal = responseImag * recipReal;
tempImagImag = responseImag * recipImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
}

if (NULL != oddPole)
{
rootReal = *oddPole;
// Compute distance from that point to that zero
differenceReal = pointReal - rootReal;
differenceImag = pointImag;
// Multiply the reciprocal of that distance by the accumulating product
recipSquareMagnitude = 1.0 / (differenceReal * differenceReal +
differenceImag * differenceImag);
recipReal = differenceReal * recipSquareMagnitude;
recipImag = -differenceImag * recipSquareMagnitude;
tempRealReal = responseReal * recipReal;
tempRealImag = responseReal * recipImag;
tempImagReal = responseImag * recipReal;
tempImagImag = responseImag * recipImag;
responseReal = tempRealReal - tempImagImag;
responseImag = tempRealImag + tempImagReal;
}

// Multiply by the gain
responseReal *= gain;
responseImag *= gain;

spectrum->mReals[i] = responseReal;
spectrum->mImags[i] = responseImag;

pointArgument += spacingAngle;
}

return true;
}

// Half-band lowpass
/*
#define kLnTwoToThe127 88.02969193111305
#define kRecipLn10 0.4342944819032518

// Compute square sum of errors for bottom half band
unsigned numLoBandPoints = numPoints >> 1;
double loBandError = 0.0;
double magnitude = 0.0;
double logMagnitude = 0.0;
for (i = 0; i < numLoBandPoints; i++)
{
tempReal = mSpectrum.mReals[i];
tempImag = mSpectrum.mImags[i];
magnitude = tempReal*tempReal + tempImag*tempImag;
if (0.0 == magnitude)
{
logMagnitude = -kLnTwoToThe127;
}
else
{
logMagnitude = log(magnitude) * kRecipLn10;
logMagnitude *= 0.5; // Half the log because it's mag squared
}

loBandError += logMagnitude * logMagnitude;
}

// Compute errors for top half of band
double hiBandError = 0.0;
for ( ; i < numPoints; i++)
{
tempReal = mSpectrum.mReals[i];
tempImag = mSpectrum.mImags[i];
magnitude = tempReal*tempReal + tempImag*tempImag;
hiBandError += magnitude; // Already a squared value
}

// Compute weighted sum of the two subtotals
// Take square root
return sqrt(loBandError + 5000.0 * hiBandError);
*/



Comments

There are no comments on this item

Add your own comment
Comments are displayed in fixed width, no HTML code allowed!
Email:

Comment:

Are you human?



Site created and maintained by Bram
Graphic design by line.out | Server sponsered by fxpansion