2018-05-28 10:07:53 +00:00
|
|
|
|
/******************************************************************************
|
|
|
|
|
|
|
|
|
|
Calcs.cpp
|
|
|
|
|
|
|
|
|
|
Copyright (c) Roger Burghall 2014..2017
|
|
|
|
|
|
|
|
|
|
******************************************************************************/
|
|
|
|
|
|
|
|
|
|
|
2018-05-27 14:10:28 +00:00
|
|
|
|
/** \file
|
|
|
|
|
'Calcs.cpp' is one of the files intended to hold the functions for
|
|
|
|
|
calculation purposes.
|
|
|
|
|
|
|
|
|
|
It is not involved with GUIs and so it is expected that it will not (or
|
|
|
|
|
barely) need editting for porting to a different OS.
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
// ***************************
|
|
|
|
|
// #include <vcl\vcl.h>
|
|
|
|
|
// #pragma hdrstop
|
|
|
|
|
#include <cmath>
|
|
|
|
|
#include "math.h"
|
|
|
|
|
#include "Calcs.h"
|
|
|
|
|
#include <iostream>
|
|
|
|
|
#include <complex>
|
|
|
|
|
#include <stdlib.h>
|
|
|
|
|
#include <sstream>
|
|
|
|
|
// #include "filter.h"
|
|
|
|
|
// #include "control.h"
|
|
|
|
|
// ***************************
|
|
|
|
|
|
|
|
|
|
#if SHOW_FILE_COMPILING
|
|
|
|
|
#pragma message "Compiling Calcs.cpp"
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
TFilter filter;
|
|
|
|
|
|
|
|
|
|
TFilter::TFilter( )
|
|
|
|
|
{
|
|
|
|
|
fclass = Lowpass;
|
|
|
|
|
sclass = Bessel;
|
|
|
|
|
cclass = SallKey;
|
|
|
|
|
frequency = 1.0;
|
|
|
|
|
bandwidth = 0.25;
|
|
|
|
|
gain = 1.0;
|
|
|
|
|
poles = 3;
|
|
|
|
|
zeroes = 0;
|
|
|
|
|
fmax = tmax = 2.0;
|
|
|
|
|
ripple = 0.1;
|
|
|
|
|
a0 = 0.70794578;
|
|
|
|
|
type = '0'; // 'Prototype' filter.
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double isinh(double x)
|
|
|
|
|
{
|
|
|
|
|
double y=1.0;
|
|
|
|
|
do {
|
|
|
|
|
y+=(x-sinh(y))/cosh(y);
|
|
|
|
|
} while(((sinh(y)-x)/x) >=0.001);
|
|
|
|
|
return(y);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double n_to_freq(int n, double f1, int nmax=FSTEPS, int decades=2)
|
|
|
|
|
{
|
2022-08-12 12:32:15 +00:00
|
|
|
|
return(f1 * exp10(double(n) * decades / nmax));
|
2018-05-27 14:10:28 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
using namespace std;
|
|
|
|
|
|
|
|
|
|
// void bessel(TFilter& filter);
|
|
|
|
|
// void butterworth(TFilter& filter);
|
|
|
|
|
// void chebyshev(TFilter& filter);
|
|
|
|
|
// void lowpass(TFilter& filter, double f);
|
|
|
|
|
// void highpass(TFilter& filter);
|
|
|
|
|
|
|
|
|
|
void geffe(double sigma, double omega, double w0, double bw, double &a1, double &b1, double &a2, double &b2);
|
|
|
|
|
|
|
|
|
|
double atof(string s)
|
|
|
|
|
{
|
|
|
|
|
double d;
|
|
|
|
|
sscanf(s.c_str(), "%lf", &d);
|
|
|
|
|
return(d);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
TFilter::_pole(double a, double w)
|
|
|
|
|
{
|
|
|
|
|
// Draw pole at (a,w).
|
|
|
|
|
int aa, ww;
|
|
|
|
|
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "Pole at " << a << ", " << w << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::show_filter(void)
|
|
|
|
|
{
|
|
|
|
|
int i;
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "No of poles:" << poles << "\n";
|
|
|
|
|
cout << "No of zeroes:" << zeroes << "\n";
|
|
|
|
|
|
|
|
|
|
for(i=0; i<poles; i++) cout << "i: " << st[i].pole << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::show_filter(string title)
|
|
|
|
|
{
|
|
|
|
|
int i;
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << title << "\n";
|
|
|
|
|
cout << "No of poles:" << poles << "\n";
|
|
|
|
|
cout << "No of zeroes:" << zeroes << "\n";
|
|
|
|
|
|
|
|
|
|
for(i=0; i<poles; i++) cout << "i: " << st[i].pole << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// Assignment operator; don't actually need to copy all data for this application.
|
|
|
|
|
stage& stage::operator= (stage& f1)
|
|
|
|
|
{
|
|
|
|
|
this->fclass = f1.fclass;
|
|
|
|
|
this->cclass = f1.cclass;
|
|
|
|
|
this->T = f1.T;
|
|
|
|
|
this->q = f1.q;
|
|
|
|
|
this->pole = f1.pole;
|
|
|
|
|
this->zero = f1.zero;
|
2022-08-12 12:32:15 +00:00
|
|
|
|
return(*this);
|
2018-05-27 14:10:28 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// Put the poles into falling order of imaginary part. As only a few objects are to be sorted, use a bubble sort.
|
|
|
|
|
void TFilter::sort(void)
|
|
|
|
|
{
|
|
|
|
|
stage s0;
|
|
|
|
|
int i, j;
|
|
|
|
|
for(j=0; j < poles-1; j++) for(i=0; i < poles-1; i++) {
|
|
|
|
|
if(st[i].pole.imag( ) < st[i+1].pole.imag( )) {
|
|
|
|
|
// poles in wrong order, swap them.
|
|
|
|
|
s0.pole = st[i].pole;
|
|
|
|
|
st[i].pole = st[i+1].pole;
|
|
|
|
|
st[i+1].pole = s0.pole;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double TFilter::fgain(double f)
|
|
|
|
|
{
|
|
|
|
|
unsigned i, k;
|
|
|
|
|
double t, q;
|
|
|
|
|
complex<double> j = std::complex<double>(0.0, 1.0);
|
|
|
|
|
complex<double> s, r;
|
|
|
|
|
double rf;
|
|
|
|
|
rf = 1.0;
|
|
|
|
|
for(k=0; k<(poles/2); k++) {
|
|
|
|
|
t = st[k].T, q = st[k].q;
|
|
|
|
|
s = std::complex<double>(0.0, (2.0*M_PI)*f);
|
|
|
|
|
r = 1.0/(1.0+s*t/q+s*s*t*t);
|
|
|
|
|
rf *= abs(r);
|
|
|
|
|
}
|
|
|
|
|
if(k < (poles+1)/2) {
|
|
|
|
|
s = std::complex<double>(0.0, (2.0*M_PI)*f);
|
|
|
|
|
r = 1.0/(1.0+s*tau);
|
|
|
|
|
rf *= abs(r);
|
|
|
|
|
}
|
|
|
|
|
return(rf);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// We want to normalise the filter so the cut-off is at 1.0 radians per second.
|
|
|
|
|
void TFilter::Proto_normalise(void)
|
|
|
|
|
{
|
|
|
|
|
int k;
|
|
|
|
|
double f, f1;
|
|
|
|
|
double g, g1;
|
|
|
|
|
char logstr[256];
|
|
|
|
|
log("\n<Normalise prototype>");
|
|
|
|
|
for(f = 0.1; f < 10.0; f *= 1.01) {
|
|
|
|
|
g = fgain(f);
|
|
|
|
|
if(g < a0) break;
|
|
|
|
|
g1 = g;
|
|
|
|
|
f1 = f;
|
|
|
|
|
}
|
|
|
|
|
// std::cout << "Gain is " << g1 << " at " << f1 << ", and " << g << " at " << f << "\n";
|
|
|
|
|
sprintf(logstr, "Prototype gain is %f at %f, and %f at %f", g1, f1, g, f);
|
|
|
|
|
log(logstr);
|
|
|
|
|
f = 2.0 * M_PI * (f * (-a0 + g1) + f1 * (-g + a0)) / (-g + g1);
|
|
|
|
|
for(k=0; k<(poles); k++) {
|
|
|
|
|
st[k].pole /= f;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
std::cout << "f = " << f << "\n";
|
|
|
|
|
show_filter("Normalised prototype");
|
|
|
|
|
log("</Normalise prototype>\n");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
Calculate the t and q values for each stage from the poles with
|
|
|
|
|
positive imaginary parts (i.e. one of each pair), and if the
|
|
|
|
|
number of poles is odd, calculate the tau value from the remaining
|
|
|
|
|
pole.
|
|
|
|
|
*/
|
|
|
|
|
void TFilter::ts_and_qs(void)
|
|
|
|
|
{
|
|
|
|
|
unsigned i, j, k;
|
|
|
|
|
double theta;
|
|
|
|
|
double tt, qq;
|
|
|
|
|
|
|
|
|
|
log("<Determine Ts and qs>");
|
|
|
|
|
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "ts_and_qs\n";
|
|
|
|
|
#endif
|
|
|
|
|
switch(fclass) {
|
|
|
|
|
case Lowpass: k = poles/2; break;
|
|
|
|
|
case Highpass: k = poles/2; break;
|
|
|
|
|
case Bandpass: k = poles; break;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
for(i=0, j=0; i<k; i++) {
|
|
|
|
|
if(st[i].pole.imag( ) < 0.0) continue;
|
|
|
|
|
tt=1.0/abs(st[i].pole),
|
|
|
|
|
qq=-1.0/(2.0*tt*real(st[i].pole));
|
|
|
|
|
st[j].T = tt;
|
|
|
|
|
st[j].q = qq;
|
|
|
|
|
log("T=", tt);
|
|
|
|
|
log("q=", qq);
|
|
|
|
|
++j;
|
|
|
|
|
}
|
|
|
|
|
if(poles%2) {
|
|
|
|
|
tau = 1.0 / st[poles/2].pole.real();
|
|
|
|
|
log("Tau=", -1.0/tau);
|
|
|
|
|
st[j].T = tau;
|
|
|
|
|
}
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "end ts_and_qs\n";
|
|
|
|
|
#endif
|
|
|
|
|
log("</Determine Ts and qs>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::butterworth(void)
|
|
|
|
|
{
|
|
|
|
|
unsigned i;
|
|
|
|
|
double theta;
|
|
|
|
|
double tt, qq;
|
|
|
|
|
log("<Butterworth>");
|
|
|
|
|
theta=M_PI/(2.0*poles);
|
|
|
|
|
for(i=0; i<poles; i++) {
|
|
|
|
|
if((poles%2) && (i == poles/2)) {
|
|
|
|
|
// pole[i] = std::complex <double>(-sin(theta)*2.0*M_PI, 0.0);
|
|
|
|
|
st[i].pole = std::complex <double>(-sin(theta), 0.0);
|
|
|
|
|
} else {
|
|
|
|
|
// pole[i] = std::complex <double>(-sin(theta)*2.0*M_PI, cos(theta)*2.0*M_PI);
|
|
|
|
|
st[i].pole = std::complex <double>(-sin(theta), cos(theta));
|
|
|
|
|
}
|
|
|
|
|
log("pole at ", st[i].pole);
|
|
|
|
|
theta += M_PI/poles;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
type = 'P';
|
|
|
|
|
|
|
|
|
|
log("/Butterworth");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
TFilter::bessel(void)
|
|
|
|
|
{
|
|
|
|
|
int i, j, k, nn;
|
|
|
|
|
double a[20], b[20], bb[20], bbb[20], aa[10], ww[10], limit, p, q,
|
|
|
|
|
qq, tt, dd, t, w;
|
|
|
|
|
std::ofstream file;
|
|
|
|
|
int n;
|
|
|
|
|
|
|
|
|
|
log("<Bessel>");
|
|
|
|
|
n = poles;
|
|
|
|
|
/// Bessel-Thomson filter design.
|
|
|
|
|
if((n<3)||(n>19)) {
|
|
|
|
|
cout << "Unreasonable order.\n";
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
nn=n;
|
|
|
|
|
limit=exp(std::log(3.5)*n)*0.00001;
|
|
|
|
|
/// Evaluate the terms of the polynomial.
|
|
|
|
|
for(i=0; i<=19; i++) a[i]=b[i]=bb[i]=bbb[i]=0;
|
|
|
|
|
bbb[0]=bbb[1]=1.0;
|
|
|
|
|
bb[0]=bb[1]=3.0, bb[2]=1.0;
|
|
|
|
|
for(i=3; i<=n; i++) {
|
|
|
|
|
for(j=0; j<=n; j++)b[j]=(2*i-1)*bb[j];
|
|
|
|
|
for(j=2; j<=n; j++) b[j]+=bbb[j-2];
|
|
|
|
|
for(j=0; j<=n; j++) {
|
|
|
|
|
bbb[j]=bb[j];
|
|
|
|
|
bb[j]=b[j];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "Bessel polynomial:\n";
|
|
|
|
|
for(i=0; i<=n; i++) cout << "a[" << i << "]=" << b[i] << " "; cout << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
for(i=0; i<=n; i++) a[n-i]=b[i];
|
|
|
|
|
k=0;
|
|
|
|
|
file.open("pfile.z");
|
|
|
|
|
if(!file) {
|
|
|
|
|
cout << "\nUnable to open 'pfile.z'\n";
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
file << "Bessel: P " << n << " " << 0;
|
|
|
|
|
// 440
|
|
|
|
|
int pp = 0;
|
|
|
|
|
do {
|
|
|
|
|
p=q=0.01;
|
|
|
|
|
do {
|
|
|
|
|
b[0]=a[0];
|
|
|
|
|
b[1]=a[1]-b[0]*p;
|
|
|
|
|
for(i=2; i<=n;i++) b[i]=a[i]-b[i-2]*q-b[i-1]*p;
|
|
|
|
|
if(b[n-2]==0) b[n-2]=0.000001;
|
|
|
|
|
q=a[n]/b[n-2]; //530
|
|
|
|
|
p=(a[n-1]-b[n-3]*q)/b[n-2];
|
|
|
|
|
} while((abs(b[n-1]) >= limit) || (abs(b[n]) >= limit));
|
|
|
|
|
cout << "Solution at " << -p/2.0 << " +/- j*" << sqrt(q-(p*p/4.0)) << "\n";
|
|
|
|
|
file << " " << -p/2.0 << " " << sqrt(q-(p*p/4.0)) << "\n";
|
|
|
|
|
st[pp++].pole = complex<double>(-p/2.0, sqrt(q-(p*p/4.0)));
|
|
|
|
|
st[pp++].pole = complex<double>(-p/2.0, -sqrt(q-(p*p/4.0)));
|
|
|
|
|
aa[k]=-p/2.0, ww[k]=sqrt(q-(p*p/4.0));
|
|
|
|
|
k++;
|
|
|
|
|
for(i=0; i<=n; i++) a[i]=b[i];
|
|
|
|
|
n-=2;
|
|
|
|
|
} while(n>2);
|
|
|
|
|
if(n==1) {
|
|
|
|
|
// 720
|
|
|
|
|
cout << "Solution at " << -b[1]/b[0] << "\n";
|
|
|
|
|
file << " " << -b[1]/b[0];
|
|
|
|
|
st[pp++].pole = complex<double>(-b[1]/b[0], 0.0);
|
|
|
|
|
} else {
|
|
|
|
|
//660
|
|
|
|
|
cout << "Solution' at " << -a[1]/(2.0*a[0]);
|
|
|
|
|
cout << " +/-j*" << (sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]));
|
|
|
|
|
file << " " << -a[1]/(2.0*a[0]) << " " << sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]);
|
|
|
|
|
st[pp++].pole = complex<double>(-a[1]/(2.0*a[0]), sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]));
|
|
|
|
|
st[pp++].pole = complex<double>(-a[1]/(2.0*a[0]), -sqrt(-a[1]*a[1]+4.0*a[0]*a[2])/(2.0*a[0]));
|
|
|
|
|
k++;
|
|
|
|
|
}
|
|
|
|
|
file.close();
|
|
|
|
|
|
|
|
|
|
for(i=0; i<=nn/2; i++) {
|
|
|
|
|
_pole(aa[i]/3.0, ww[i]/3.0);
|
|
|
|
|
_pole(aa[i]/3.0, -ww[i]/3.0);
|
|
|
|
|
}
|
|
|
|
|
if(nn%2) {
|
|
|
|
|
_pole(-b[1]/(3.0*b[0]), 0);
|
|
|
|
|
// pole[nn - 1] = complex<double>(-b[1]/(3.0*b[0], 0.0));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "\nEndBessel\n";
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// 940
|
|
|
|
|
if(dd!='D') ;
|
|
|
|
|
type = 'P';
|
|
|
|
|
|
|
|
|
|
log("</Bessel>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
TFilter::chebyshev(void)
|
|
|
|
|
{
|
|
|
|
|
char dump;
|
|
|
|
|
int i;
|
|
|
|
|
double aa[20], ww[20], g, a, w, tt, qq;
|
|
|
|
|
|
|
|
|
|
// Chebyshev filter design.
|
|
|
|
|
log("<Chebyshev>");
|
|
|
|
|
unsigned n = poles;
|
2022-08-12 12:32:15 +00:00
|
|
|
|
// Version 2.11: correct ripple factor from dB.
|
|
|
|
|
// g = ripple;
|
|
|
|
|
g = sqrt(pow(10.0, ripple / 10.0) - 1.0);
|
2018-05-27 14:10:28 +00:00
|
|
|
|
|
|
|
|
|
for(i=1; i<=n; i++) {
|
|
|
|
|
a=-sin((2*i-1)*M_PI/(2*n))*sinh(isinh(1.0/g)/n);
|
|
|
|
|
|
|
|
|
|
w=cos((2*i-1)*M_PI/(2*n))*cosh(isinh(1.0/g)/n);
|
|
|
|
|
|
|
|
|
|
if(fabs(w)<1e-9) w=0;
|
|
|
|
|
|
|
|
|
|
st[i-1].pole = complex<double>(a, w);
|
|
|
|
|
|
|
|
|
|
cout << "\n" << a << ", " << w;
|
|
|
|
|
aa[i]=-a, ww[i]=w;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
type = 'P';
|
|
|
|
|
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "EndChebyshev\n";
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
log("</Chebyshev>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
TFilter::lowpass(void)
|
|
|
|
|
{
|
|
|
|
|
unsigned i;
|
|
|
|
|
log("<lowpass>");
|
|
|
|
|
// Mod 2016-09-01-RB
|
|
|
|
|
// for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI * frequency;
|
|
|
|
|
for(i=0; i<poles; i++) st[i].pole *= frequency;
|
|
|
|
|
fclass = Lowpass;
|
|
|
|
|
zeroes = 0;
|
|
|
|
|
type = 'L';
|
|
|
|
|
log("</lowpass>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
Convert the continuous filter to discrete form via the bilinear transformation
|
|
|
|
|
*/
|
|
|
|
|
void TFilter::bilinear(void)
|
|
|
|
|
{
|
|
|
|
|
std::complex<double> _bilinear(std::complex<double> s, double Ts);
|
|
|
|
|
double normalise(double fc, double fs);
|
|
|
|
|
unsigned i;
|
|
|
|
|
double fc, fs, omega;
|
|
|
|
|
|
|
|
|
|
log("<bilinear transformation>");
|
|
|
|
|
|
|
|
|
|
for(i=0; i<poles/2; i++) {
|
|
|
|
|
fc = 1.0 / (2.0*M_PI*st[i].T);
|
|
|
|
|
fs = samplingfreq;
|
|
|
|
|
if(fc >= fs/2.0) {
|
|
|
|
|
std::cout << "That doesn't make sense! Cut-off must be less than sampling freq / 2.\n" <<
|
|
|
|
|
"Here fc = " << fc << " and fs = " << fs << "\n";
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
omega = normalise(fc, fs);
|
|
|
|
|
std::cout << "\nStage " << i << "\nomega =" << omega << "\n";
|
|
|
|
|
double omega_ac = tan(omega/2.0);
|
|
|
|
|
std::cout << "omega_ac =" << omega_ac << "\n\n";
|
|
|
|
|
st[i].iir1.transform(fclass, omega_ac, st[i].q);
|
|
|
|
|
st[i].iir1.write(i);
|
|
|
|
|
/* st[i].iir1.step(0.0);
|
|
|
|
|
std::cout << "\n";
|
|
|
|
|
std::cout << "Direct Form 1 realisation.\n--------------------------\n";
|
|
|
|
|
for(i=0; i<200; i++) std::cout << st[i].iir1.step(1.0) << "\n";
|
|
|
|
|
*/
|
|
|
|
|
// st[i].z = _bilinear(st[i].pole, 1000.0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
log("</bilinear transformation>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
Calculate the frequency response of the filter.
|
|
|
|
|
*/
|
|
|
|
|
void
|
|
|
|
|
bode_calc(TFilter& filter)
|
|
|
|
|
{
|
|
|
|
|
// extern TControl_window *Control_window;
|
|
|
|
|
unsigned i, k;
|
|
|
|
|
double f, w;
|
|
|
|
|
double t, q;
|
|
|
|
|
std::complex<double> s, r, j;
|
|
|
|
|
double temp;
|
|
|
|
|
double max_gain = 0.0;
|
|
|
|
|
ofstream bode_file;
|
|
|
|
|
|
|
|
|
|
filter.log("<bode_calc>");
|
|
|
|
|
j = std::complex<double>(0.0, 1.0);
|
|
|
|
|
|
|
|
|
|
// filter.fmax = 1.0 * filter.frequency * M_PI / 2.0;
|
2022-08-12 12:32:15 +00:00
|
|
|
|
filter.fmax = T_MULTIPLE_HP * filter.frequency;
|
2018-05-27 14:10:28 +00:00
|
|
|
|
for(i=0; i<FSTEPS; i++) filter.freq_resp[i] = 1.0;
|
|
|
|
|
for(k=0; k<(filter.poles/2); k++) {
|
|
|
|
|
// t = 1.0 / abs(filter.pole[k]);
|
|
|
|
|
// q = 1.0 / (2.0*t*-real(filter.pole[k]));
|
|
|
|
|
t = filter.st[k].T, q = filter.st[k].q;
|
|
|
|
|
for(i=0; i<FSTEPS; i++) {
|
|
|
|
|
#if LINEAR_FREQ
|
|
|
|
|
f = i*filter.fmax/FSTEPS;
|
|
|
|
|
#else
|
|
|
|
|
f = n_to_freq(i, filter.fmax);
|
|
|
|
|
#endif
|
|
|
|
|
s = std::complex<double>(0.0, (2.0*M_PI)*f);
|
|
|
|
|
r = 1.0/(1.0+s*t/q+s*s*t*t);
|
|
|
|
|
if(filter.fclass == Bandpass) r *= s * t;
|
|
|
|
|
if(filter.fclass == Highpass) r *= s * s * t * t;
|
|
|
|
|
filter.freq_resp[i] *= abs(r);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
if(k < (filter.poles+1)/2) {
|
|
|
|
|
// t = 1.0 / abs(filter.pole[k]);
|
|
|
|
|
for(i=0; i<FSTEPS; i++) {
|
|
|
|
|
#if LINEAR_FREQ
|
|
|
|
|
f = i*filter.fmax/FSTEPS;
|
|
|
|
|
#else
|
|
|
|
|
f = n_to_freq(i, filter.fmax);
|
|
|
|
|
#endif
|
|
|
|
|
s = std::complex<double>(0.0, (2.0*M_PI)*f);
|
|
|
|
|
r = 1.0/(1.0+s*filter.tau);
|
|
|
|
|
if(filter.fclass == Bandpass) { cout << "How do we have a bandpass filter with an odd pole?\n"; r *= s * filter.tau; }
|
|
|
|
|
if(filter.fclass == Highpass) r *= s * filter.tau;
|
|
|
|
|
filter.freq_resp[i] *= abs(r);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Normalise gain
|
|
|
|
|
if(filter.fclass == Bandpass) {
|
|
|
|
|
for(i=0, max_gain=0; i<FSTEPS; i++) {
|
|
|
|
|
if(filter.freq_resp[i] > max_gain) max_gain = filter.freq_resp[i];
|
|
|
|
|
}
|
|
|
|
|
for(i=0; i<FSTEPS; i++) {
|
|
|
|
|
filter.freq_resp[i] /= max_gain;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bode_file.open("./bode.csv");
|
|
|
|
|
if(bode_file.is_open( )) {
|
|
|
|
|
bode_file << "Frequency, Gain\n";
|
|
|
|
|
for(i=0; i<FSTEPS; i++) {
|
|
|
|
|
#if LINEAR_FREQ
|
|
|
|
|
f = i*filter.fmax/FSTEPS;
|
|
|
|
|
#else
|
|
|
|
|
f = n_to_freq(i, filter.frequency);
|
|
|
|
|
#endif
|
|
|
|
|
bode_file << f << ", " << filter.freq_resp[i] << "\n";
|
|
|
|
|
}
|
|
|
|
|
bode_file.close( );
|
|
|
|
|
}
|
|
|
|
|
filter.log("</bode_calc>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
'step_calc' convolves the impulse response of each stage with a step function
|
|
|
|
|
so as to derive the step response of the entire filter.
|
|
|
|
|
*/
|
|
|
|
|
void
|
|
|
|
|
TFilter::step_calc(void)
|
|
|
|
|
{
|
|
|
|
|
// extern TControl_window *Control_window;
|
|
|
|
|
char logstr[256];
|
|
|
|
|
unsigned i, k, u;
|
|
|
|
|
double ti, w, w0;
|
|
|
|
|
double tt, qq, zeta;
|
|
|
|
|
double a, b;
|
|
|
|
|
double input[TSTEPS], output[TSTEPS], stage_n[TSTEPS];
|
|
|
|
|
double integral, max;
|
|
|
|
|
double k1, dt;
|
|
|
|
|
std::complex<double> s, r, j;
|
|
|
|
|
ofstream step_file;
|
|
|
|
|
|
|
|
|
|
log("<step_calc>");
|
|
|
|
|
j = std::complex<double>(0.0, 1.0);
|
|
|
|
|
|
|
|
|
|
switch(fclass) {
|
2022-08-12 12:32:15 +00:00
|
|
|
|
case Lowpass: tmax = T_MULTIPLE_LP/frequency; break;
|
|
|
|
|
case Bandpass: tmax = T_MULTIPLE_BP/bandwidth; break;
|
|
|
|
|
case Highpass: tmax = T_MULTIPLE_HP/frequency; break;
|
2018-05-27 14:10:28 +00:00
|
|
|
|
default: cout << "Unknown fclass\n"; exit(-1);
|
|
|
|
|
}
|
|
|
|
|
dt = tmax/TSTEPS;
|
|
|
|
|
|
|
|
|
|
sprintf(logstr, "tmax = %f seconds", tmax);
|
|
|
|
|
log(logstr);
|
|
|
|
|
|
|
|
|
|
// c = tmax / 5.0;
|
|
|
|
|
/// Start with step function
|
|
|
|
|
for(i=0; i<TSTEPS; i++) input[i] = 1.0, output[i] = 0.0;
|
|
|
|
|
for(k=0; k<(poles/2); k++) {
|
|
|
|
|
/// Calculate constants for stage 'k'
|
|
|
|
|
tt = st[k].T;
|
|
|
|
|
qq = st[k].q;
|
|
|
|
|
a = -1.0 / (2 * qq * tt);
|
|
|
|
|
b = sqrt(1.0 / (tt * tt) - (a * a));
|
|
|
|
|
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "T = " << tt << ", q = " << qq << "\n";
|
|
|
|
|
cout << "a = " << a << ", b = " << b << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
w0 = 1.0 / tt;
|
|
|
|
|
log("'step_calc' tt = ", tt);
|
|
|
|
|
log("'step_calc' q = ", qq);
|
|
|
|
|
// log("'step_calc' w0 = ", w0);
|
|
|
|
|
k1 = 1.0 / (b*tt);
|
|
|
|
|
// k1 = 1.0 / (b*tt*tt);
|
|
|
|
|
for(i=0; i<TSTEPS; i++) {
|
|
|
|
|
/// Calculate time for point 'i'
|
|
|
|
|
ti = i*dt; // CHECK THIS
|
|
|
|
|
/// Calculate the impulse response of stage 'k'
|
|
|
|
|
switch(fclass) {
|
|
|
|
|
case Lowpass: stage_n[i] = k1*exp(a * ti) * sin(b * ti) * b; break;
|
|
|
|
|
case Highpass: stage_n[i] = -exp(a * ti) * sin(b * ti) * k1;
|
|
|
|
|
if(i == 0) stage_n[0] += tt*TSTEPS/tmax; break;
|
|
|
|
|
case Bandpass: stage_n[i] = (sin(b * ti) * a * exp(a * ti) + exp(a * ti) * b * cos(b * ti)) / b; break;
|
|
|
|
|
default: cout << "\nUnknown 'fclass'!\n"; exit(-1);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// initialise the output array...
|
|
|
|
|
for(i=0; i<TSTEPS; i++) output[i] = 0.0;
|
|
|
|
|
|
|
|
|
|
/// Convolve with input to stage 'k'
|
|
|
|
|
for(i=0; i<TSTEPS; i++) {
|
|
|
|
|
// Calculate time for point 'i'
|
|
|
|
|
ti = i*tmax/TSTEPS; // CHECK THIS
|
|
|
|
|
for(u=0; u<TSTEPS; u++) {
|
|
|
|
|
if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u]*tmax/TSTEPS; // CHECK THIS
|
|
|
|
|
else break;
|
|
|
|
|
// if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u]; // CHECK THIS
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#ifdef SHOW_STAGES
|
|
|
|
|
log("'step calc' stage[] = ", stage_n, TSTEPS);
|
|
|
|
|
log("'step-calc' input[] = ", input, TSTEPS);
|
|
|
|
|
log("’stepicalc' output[] = ", output, TSTEPS);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/// Copy output of stage 'k' to input for stage 'k+1'
|
|
|
|
|
for(i=0; i<TSTEPS; i++) {
|
|
|
|
|
input[i] = output[i];
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// If order is odd, process first order stage.
|
|
|
|
|
if(poles%2) {
|
|
|
|
|
if(fclass == Bandpass) {
|
|
|
|
|
log("Impossible! Bandpass can't have odd order!");
|
|
|
|
|
cout << "Impossible! Bandpass can't have odd order!\n";
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
tt = -st[k].T;
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << "Tau = " << tt << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
w0 = 1.0 / tt;
|
|
|
|
|
log("'step_calc' tt = ", tt);
|
|
|
|
|
for(i=0; i<TSTEPS; i++) {
|
|
|
|
|
/// Calculate time for point 'i'
|
|
|
|
|
ti = i*tmax/TSTEPS; // CHECK THIS
|
|
|
|
|
/// Calculate the impulse response of 1st order stage 'k'
|
|
|
|
|
switch(fclass) {
|
|
|
|
|
case Lowpass: stage_n[i] = exp(-ti/tt); break;
|
|
|
|
|
// case Highpass: stage_n[i] = -exp(-ti/tt)/tt;
|
|
|
|
|
case Highpass: stage_n[i] = -exp(-ti/tt);
|
|
|
|
|
if(i == 0) stage_n[0] += tt*TSTEPS/tmax; break;
|
|
|
|
|
case Bandpass: std::cout << "*** ERROR! Band-pass filter cannot have odd order! ***\n"; break;
|
|
|
|
|
default: cout << "\nUnknown 'fclass'!\n"; exit(-1);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// initialise the output array...
|
|
|
|
|
for(i=0; i<TSTEPS; i++) output[i] = 0.0;
|
|
|
|
|
|
|
|
|
|
/// Convolve with input to stage 'k'
|
|
|
|
|
for(i=0; i<TSTEPS; i++) {
|
|
|
|
|
// Calculate time for point 'i'
|
|
|
|
|
ti = i*tmax/TSTEPS; // CHECK THIS
|
|
|
|
|
for(u=0; u<TSTEPS; u++) {
|
|
|
|
|
if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u]*tmax/TSTEPS; // CHECK THIS
|
|
|
|
|
else break;
|
|
|
|
|
// if((i+u) < TSTEPS) output[i+u]+=input[i]*stage_n[u]; // CHECK THIS
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#ifdef SHOW_STAGES
|
|
|
|
|
log("'step calc' stage[] = ", stage_n, TSTEPS);
|
|
|
|
|
log("'step-calc' input[] = ", input, TSTEPS);
|
|
|
|
|
log("’stepicalc' output[] = ", output, TSTEPS);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
/// Copy output of stage 'k' to input for stage 'k+1'
|
|
|
|
|
for(i=0; i<TSTEPS; i++) {
|
|
|
|
|
input[i] = output[i];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/// Adjust gain - I hope this is a temporary frig!
|
|
|
|
|
switch(fclass) {
|
|
|
|
|
case Highpass: for(i=1, max = 0.0; i<TSTEPS; i++) if(fabs(output[i]) > max) max = fabs(output[i]); break;
|
|
|
|
|
default: for(i=0, max = 0.0; i<TSTEPS; i++) if(output[i] > max) max = output[i]; break;
|
|
|
|
|
}
|
|
|
|
|
for(i=0; i<TSTEPS; i++) output[i] /= max;
|
|
|
|
|
|
|
|
|
|
/// Write to './step.csv' time and step response
|
|
|
|
|
step_file.open("./step.csv");
|
|
|
|
|
for(i=0, integral=0.0; i<TSTEPS; i++) {
|
|
|
|
|
step_resp[i] = output[i];
|
|
|
|
|
integral += output[i];
|
|
|
|
|
step_file << i*tmax/TSTEPS << ", " << step_resp[i] /* << ", " << integral */ << "\n";
|
|
|
|
|
}
|
|
|
|
|
step_file.close( );
|
|
|
|
|
log("</step_calc>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double scale(TFilter filter)
|
|
|
|
|
{
|
|
|
|
|
double max = 0;
|
|
|
|
|
int i;
|
|
|
|
|
for(i = 0; i < filter.poles; i++) {
|
|
|
|
|
if(-(double)filter.st[i].pole.real() > max) max = -(double)filter.st[i].pole.real();
|
|
|
|
|
if((double)filter.st[i].pole.imag() > max) max = (double)filter.st[i].pole.imag();
|
|
|
|
|
}
|
|
|
|
|
return((max * 4.0) / 3.0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::Calculate(void)
|
|
|
|
|
{
|
|
|
|
|
char str[256];
|
|
|
|
|
int i;
|
|
|
|
|
log("<Calculate>");
|
|
|
|
|
|
|
|
|
|
/// Create prototype filter.
|
|
|
|
|
switch(sclass) {
|
|
|
|
|
case Butterworth: butterworth( ); break;
|
|
|
|
|
case Bessel: bessel( ); sort( ); break;
|
|
|
|
|
case Chebyshev: chebyshev( ); break;
|
|
|
|
|
default: cout << "Unknown filter!" << "\n"; return;
|
|
|
|
|
}
|
|
|
|
|
/// Adjust prototype filter so attenuation is x dB at 1 Hertz.
|
|
|
|
|
std::cout << "***** Prototype filter *****\n";
|
|
|
|
|
for(i=0; i<poles; i++) {
|
|
|
|
|
std::cout << "Pole: " << st[i].pole << "\n";
|
|
|
|
|
sprintf(str, "Pole: %f + i.%f", st[i].pole.real( ), st[i].pole.imag( ));
|
|
|
|
|
log(str);
|
|
|
|
|
}
|
|
|
|
|
std::cout << "****************************\n";
|
|
|
|
|
/// Calculate T and q values from poles.
|
|
|
|
|
ts_and_qs( );
|
|
|
|
|
#ifdef NORMALISE
|
|
|
|
|
Proto_normalise( );
|
|
|
|
|
#endif
|
|
|
|
|
/// Transform prototype to desired filter class (e.g. high-pass etc.).
|
|
|
|
|
transform( );
|
|
|
|
|
ts_and_qs( );
|
|
|
|
|
show_filter( );
|
|
|
|
|
log("</Calculate>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::log(string s)
|
|
|
|
|
{
|
|
|
|
|
#if LOG
|
|
|
|
|
ofstream logfile;
|
|
|
|
|
logfile.open("./rbfilter.log", ios::app);
|
|
|
|
|
logfile << s << "\n";
|
|
|
|
|
logfile.close( );
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::log(string s, double d)
|
|
|
|
|
{
|
|
|
|
|
ostringstream s1;
|
|
|
|
|
std::string s2;
|
|
|
|
|
s1 << s << d;
|
|
|
|
|
s2 = s1.str( );
|
|
|
|
|
log(s2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::log(string s, double *pd)
|
|
|
|
|
{
|
|
|
|
|
ostringstream s1;
|
|
|
|
|
std::string s2;
|
|
|
|
|
s1 << s << *pd;
|
|
|
|
|
s2 = s1.str( );
|
|
|
|
|
log(s2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// Log the array pd but no more than 100 elements of it.
|
|
|
|
|
void TFilter::log(string s, double *pd, int n)
|
|
|
|
|
{
|
|
|
|
|
int i;
|
|
|
|
|
#if LOG
|
|
|
|
|
char s1[128];
|
|
|
|
|
n = n>100? 100: n;
|
|
|
|
|
for(i=0; i<(n-1); i++) {
|
|
|
|
|
sprintf(s1, "%s %f", s.c_str( ), *(pd+i));
|
|
|
|
|
log(s1);
|
|
|
|
|
}
|
|
|
|
|
log("");
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::log(string s, std::complex<double> c)
|
|
|
|
|
{
|
|
|
|
|
ostringstream s1;
|
|
|
|
|
std::string s2;
|
|
|
|
|
s1 << s << c;
|
|
|
|
|
s2 = s1.str( );
|
|
|
|
|
log(s2);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
void TFilter::print_T_q(void)
|
|
|
|
|
{
|
|
|
|
|
// cout << s << "\n";
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// 'transform( )' transforms the prototype filter to the desired type (e.g. bandpass) and frequency.
|
|
|
|
|
void TFilter::transform(void)
|
|
|
|
|
{
|
|
|
|
|
char t, type;
|
|
|
|
|
int i;
|
|
|
|
|
double p[10], z[10];
|
|
|
|
|
ifstream ifile;
|
|
|
|
|
ofstream ofile;
|
|
|
|
|
|
|
|
|
|
type = TFilter::type;
|
|
|
|
|
log("<transform>");
|
|
|
|
|
if(type != 'P') {
|
|
|
|
|
cout << "\nFile has already been transformed.\n";
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
/// 220; Transform to the chosen type.
|
|
|
|
|
do {
|
|
|
|
|
switch(fclass) {
|
|
|
|
|
case Lowpass:
|
|
|
|
|
lowpass( );
|
|
|
|
|
type = 'L';
|
|
|
|
|
for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI;
|
|
|
|
|
break;
|
|
|
|
|
case Highpass:
|
|
|
|
|
highpass( );
|
|
|
|
|
type = 'H';
|
|
|
|
|
// for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI;
|
|
|
|
|
break;
|
|
|
|
|
case Bandpass:
|
|
|
|
|
bandpass( );
|
|
|
|
|
type = 'B';
|
|
|
|
|
for(i=0; i<poles; i++) st[i].pole *= 2.0 * M_PI;
|
|
|
|
|
break;
|
|
|
|
|
default: cout << "Unknown filter class\n"; continue;
|
|
|
|
|
}
|
|
|
|
|
break;
|
|
|
|
|
} while(true);
|
|
|
|
|
/// 330; Rewrite the parameter file.
|
|
|
|
|
ofile.open("pfile.z");
|
|
|
|
|
if(!ofile) {
|
|
|
|
|
cout << "\nUnable to open 'pfile.z'\n";
|
|
|
|
|
// exit(0);
|
|
|
|
|
}
|
|
|
|
|
ofile << type << " " << poles << " " << zeroes << "\n";
|
|
|
|
|
for(i=0; i<poles; i++) ofile << st[i].pole << " ";
|
|
|
|
|
if(zeroes) {
|
|
|
|
|
for(i=0; i<zeroes; i++) ofile << st[i].zero << "\n";
|
|
|
|
|
}
|
|
|
|
|
ofile.close();
|
|
|
|
|
|
|
|
|
|
// ts_and_qs(filter);
|
|
|
|
|
|
|
|
|
|
log("</transform>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/** Transform to highpass
|
|
|
|
|
Keep the q values unaltered but take the reciprocal of the magnitude; adjust the magnitudes for
|
|
|
|
|
frequency; add zeroes at the origin.
|
|
|
|
|
*/
|
|
|
|
|
void
|
|
|
|
|
TFilter::highpass(void)
|
|
|
|
|
{
|
|
|
|
|
int i, type;
|
|
|
|
|
double dsq, d, ratio;
|
|
|
|
|
double a1, w1;
|
|
|
|
|
|
|
|
|
|
log("<highpass>");
|
|
|
|
|
for(i=0; i<poles; i++) {
|
|
|
|
|
std::cout << "From " << st[i].pole;
|
|
|
|
|
dsq = st[i].pole.real( ) * st[i].pole.real( ) + st[i].pole.imag( ) *st[i].pole.imag( );
|
|
|
|
|
d = sqrt(dsq);
|
|
|
|
|
std::cout << " (Magnitude " << d << ") ";
|
|
|
|
|
ratio = st[i].pole.imag( ) / st[i].pole.real( );
|
|
|
|
|
if(st[i].pole.imag( ) == 0.0) {
|
|
|
|
|
w1 = 0.0;
|
|
|
|
|
a1 = 1.0 / st[i].pole.real( );
|
|
|
|
|
} else {
|
|
|
|
|
d = 1.0 / d;
|
|
|
|
|
dsq = 1.0 / dsq;
|
|
|
|
|
w1 = st[i].pole.imag( ) * dsq;
|
|
|
|
|
a1 = st[i].pole.real( ) * dsq;
|
|
|
|
|
}
|
|
|
|
|
st[i].pole = complex<double>(a1, w1);
|
|
|
|
|
st[i].zero = complex<double>(0.0, 0.0);
|
|
|
|
|
st[i].pole *= 2.0 * M_PI * frequency;
|
|
|
|
|
// st[i].zero *= 2.0 * M_PI * frequency;
|
|
|
|
|
std::cout << " to " << st[i].pole << "\n";
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
fclass = Highpass;
|
|
|
|
|
zeroes = poles;
|
|
|
|
|
type='H';
|
|
|
|
|
|
|
|
|
|
log("</highpass>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/// Transform the filter to bandpass using the Geffe algotithm.
|
|
|
|
|
void TFilter::bandpass(void)
|
|
|
|
|
{
|
|
|
|
|
int i, j;
|
|
|
|
|
double f, w1, a1, d, bw, w0, b1, a2, b2;
|
|
|
|
|
ofstream ofile;
|
|
|
|
|
TFilter filter1;
|
|
|
|
|
|
|
|
|
|
log("<bandpass>");
|
|
|
|
|
/// Transformation to bandpass doubles the number of poles!
|
|
|
|
|
// if(poles % 2) log("Odd order! Impossible");
|
|
|
|
|
|
|
|
|
|
for(i=0, j=0; i<poles; i++) {
|
|
|
|
|
if(st[i].pole.imag( ) < -0.0001) continue;
|
|
|
|
|
geffe(st[i].pole.real( ), st[i].pole.imag( ), frequency, bandwidth, a1, b1, a2, b2);
|
|
|
|
|
#if VERBOSE
|
|
|
|
|
cout << i << ": " << a1 << ", +-" << b1 << " \\ " << a2 << ", +/-" << b2 << "\n";
|
|
|
|
|
#endif
|
|
|
|
|
filter1.st[j++].pole = complex<double>(a1, b1);
|
|
|
|
|
filter1.st[j++].pole = complex<double>(a1, -b1);
|
|
|
|
|
if(i != poles - 1) {
|
|
|
|
|
filter1.st[j++].pole = complex<double>(a2, b2);
|
|
|
|
|
filter1.st[j++].pole = complex<double>(a2, -b2);
|
|
|
|
|
ofile << -a1 << " " << b1 << " " << -a2 << " " << b2 << "\n";
|
|
|
|
|
} else ofile << -a1 << " " << b1 << "\n";
|
|
|
|
|
}
|
|
|
|
|
zeroes = poles;
|
|
|
|
|
filter1.poles = 2 * poles;
|
|
|
|
|
type = 'B';
|
|
|
|
|
filter1.type = type;
|
|
|
|
|
/* if(n%2)
|
|
|
|
|
ofile << p[n]*bw/2.0 << " " << sqrt(w0*w0-p[n]*p[n]*bw*bw/4.0) << "\n";
|
|
|
|
|
ofile.close();
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
for(i=0; i<filter1.poles; i++) st[i].pole = filter1.st[i].pole;
|
|
|
|
|
poles = filter1.poles;
|
|
|
|
|
|
|
|
|
|
fclass = Bandpass;
|
|
|
|
|
|
|
|
|
|
log("</bandpass>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
double arccos(double x)
|
|
|
|
|
{
|
|
|
|
|
return(2.0 * atan(sqrt(1.0 - x*x) / (1.0 + x)));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
'geffe' is an implementation of the "Geffe" algorithm which transforms a low pass prototype filter into a bandpass filter. The "Geffe" algorithm is described in "Analogue Filter Design" by M. E. van Valkenburg.
|
|
|
|
|
*/
|
|
|
|
|
void
|
|
|
|
|
geffe(double sigma, double omega, double w0, double bw,
|
|
|
|
|
double &a1, double &b1, double &a2, double &b2)
|
|
|
|
|
{
|
|
|
|
|
double c, d, e, g, q, k, w, w01, w02, qc, a, b;
|
|
|
|
|
double q1, q2; // Used for checking.
|
|
|
|
|
|
|
|
|
|
filter.log("<geffe>");
|
|
|
|
|
|
|
|
|
|
qc=w0/bw;
|
|
|
|
|
c=sigma*sigma+omega*omega;
|
|
|
|
|
d=-2.0*sigma/qc;
|
|
|
|
|
e=4.0+c/(qc*qc);
|
|
|
|
|
g=sqrt(e*e-4.0*d*d);
|
|
|
|
|
q=sqrt((e+g)/2.0)/d;
|
|
|
|
|
k=sigma*q/qc;
|
|
|
|
|
// As originally written, 'w' would often be evaluated as 'nan'. This is intended to avoid that problem.
|
|
|
|
|
if(fabs((k*k-1.0) < 0.0001)) w = k;
|
|
|
|
|
else w=k+sqrt(k*k-1.0);
|
|
|
|
|
w01=w*w0;
|
|
|
|
|
w02=w0/w;
|
|
|
|
|
a1=w01/(2.0*q);
|
|
|
|
|
a2=w02/(2.0*q);
|
|
|
|
|
a=1.0/(2.0*q);
|
|
|
|
|
#if 1
|
|
|
|
|
b=sin(atan(sqrt((1.0-a*a)/(a*a))));
|
|
|
|
|
b1=w01*b;
|
|
|
|
|
b2=w02*b;
|
|
|
|
|
#else
|
|
|
|
|
b1 = -w01*sin(arccos(1.0 / (2.0 * q)));
|
|
|
|
|
b2 = -w02*sin(arccos(1.0 / (2.0 * q)));
|
|
|
|
|
#endif
|
|
|
|
|
// Check:
|
|
|
|
|
q1 = (a1*a1+b1*b1)/(2.0*a1);
|
|
|
|
|
q2 = (a2*a2+b2*b2)/(2.0*a2);
|
2022-08-12 12:32:15 +00:00
|
|
|
|
// cout << "q=" << q1 << " & " << q2 << "\n"; TODO: WHAT has gone wrong here?!!!
|
2018-05-27 14:10:28 +00:00
|
|
|
|
|
|
|
|
|
filter.log("</geffe>");
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|