You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
386 lines
11 KiB
C++
386 lines
11 KiB
C++
/*=============================================================================
|
|
//
|
|
// This software has been released under the terms of the GNU Public
|
|
// license. See http://www.gnu.org/copyleft/gpl.html for details.
|
|
//
|
|
// Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
|
|
//
|
|
//=============================================================================
|
|
*/
|
|
|
|
/* Design and implementation of different types of digital filters
|
|
|
|
*/
|
|
#include "firfilter.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
|
|
#if 0
|
|
/* Add new data to circular queue designed to be used with a FIR
|
|
filter. xq is the circular queue, in pointing at the new sample, xi
|
|
current index for xq and n the length of the filter. xq must be n*2
|
|
long.
|
|
*/
|
|
#define updateq(n,xi,xq,in)\
|
|
xq[xi]=(xq)[(xi)+(n)]=*(in);\
|
|
xi=(++(xi))&((n)-1);
|
|
#endif
|
|
|
|
|
|
/******************************************************************************
|
|
* FIR filter implementations
|
|
******************************************************************************/
|
|
|
|
/* C implementation of FIR filter y=w*x
|
|
|
|
n number of filter taps, where mod(n,4)==0
|
|
w filter taps
|
|
x input signal must be a circular buffer which is indexed backwards
|
|
*/
|
|
/*
|
|
TfirFilter::_ftype_t TfirFilter::fir(register unsigned int n, _ftype_t* w, _ftype_t* x)
|
|
{
|
|
register _ftype_t y; // Output
|
|
y = 0.0;
|
|
do{
|
|
n--;
|
|
y+=w[n]*x[n];
|
|
}while(n != 0);
|
|
return y;
|
|
}
|
|
*/
|
|
/*
|
|
// Boxcar
|
|
//
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
*/
|
|
void TfirFilter::boxcar(int n, _ftype_t* w)
|
|
{
|
|
int i;
|
|
// Calculate window coefficients
|
|
for (i=0 ; i<n ; i++)
|
|
w[i] = 1.0;
|
|
}
|
|
|
|
|
|
/*
|
|
// Triang a.k.a Bartlett
|
|
//
|
|
// | (N-1)|
|
|
// 2 * |k - -----|
|
|
// | 2 |
|
|
// w = 1.0 - ---------------
|
|
// N+1
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
*/
|
|
void TfirFilter::triang(int n, _ftype_t* w)
|
|
{
|
|
_ftype_t k1 = (_ftype_t)(n & 1);
|
|
_ftype_t k2 = 1/((_ftype_t)n + k1);
|
|
int end = (n + 1) >> 1;
|
|
int i;
|
|
|
|
// Calculate window coefficients
|
|
for (i=0 ; i<end ; i++)
|
|
w[i] = w[n-i-1] = _ftype_t((2.0*((_ftype_t)(i+1))-(1.0-k1))*k2);
|
|
}
|
|
|
|
|
|
/*
|
|
// Hanning
|
|
// 2*pi*k
|
|
// w = 0.5 - 0.5*cos(------), where 0 < k <= N
|
|
// N+1
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
*/
|
|
void TfirFilter::hanning(int n, _ftype_t* w)
|
|
{
|
|
int i;
|
|
_ftype_t k = _ftype_t(2*M_PI/((_ftype_t)(n+1))); // 2*pi/(N+1)
|
|
|
|
// Calculate window coefficients
|
|
for (i=0; i<n; i++)
|
|
*w++ = _ftype_t(0.5*(1.0 - cos(k*(_ftype_t)(i+1))));
|
|
}
|
|
|
|
/*
|
|
// Hamming
|
|
// 2*pi*k
|
|
// w(k) = 0.54 - 0.46*cos(------), where 0 <= k < N
|
|
// N-1
|
|
//
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
*/
|
|
void TfirFilter::hamming(int n,_ftype_t* w)
|
|
{
|
|
int i;
|
|
_ftype_t k = _ftype_t(2*M_PI/((_ftype_t)(n-1))); // 2*pi/(N-1)
|
|
|
|
// Calculate window coefficients
|
|
for (i=0; i<n; i++)
|
|
*w++ = _ftype_t(0.54 - 0.46*cos(k*(_ftype_t)i));
|
|
}
|
|
|
|
/*
|
|
// Blackman
|
|
// 2*pi*k 4*pi*k
|
|
// w(k) = 0.42 - 0.5*cos(------) + 0.08*cos(------), where 0 <= k < N
|
|
// N-1 N-1
|
|
//
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
*/
|
|
void TfirFilter::blackman(int n,_ftype_t* w)
|
|
{
|
|
int i;
|
|
_ftype_t k1 = _ftype_t(2*M_PI/((_ftype_t)(n-1))); // 2*pi/(N-1)
|
|
_ftype_t k2 = 2*k1; // 4*pi/(N-1)
|
|
|
|
// Calculate window coefficients
|
|
for (i=0; i<n; i++)
|
|
*w++ = _ftype_t(0.42 - 0.50*cos(k1*(_ftype_t)i) + 0.08*cos(k2*(_ftype_t)i));
|
|
}
|
|
|
|
/*
|
|
// Flattop
|
|
// 2*pi*k 4*pi*k
|
|
// w(k) = 0.2810638602 - 0.5208971735*cos(------) + 0.1980389663*cos(------), where 0 <= k < N
|
|
// N-1 N-1
|
|
//
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
*/
|
|
void TfirFilter::flattop(int n,_ftype_t* w)
|
|
{
|
|
int i;
|
|
_ftype_t k1 = _ftype_t(2*M_PI/((_ftype_t)(n-1))); // 2*pi/(N-1)
|
|
_ftype_t k2 = 2*k1; // 4*pi/(N-1)
|
|
|
|
// Calculate window coefficients
|
|
for (i=0; i<n; i++)
|
|
*w++ = _ftype_t(0.2810638602 - 0.5208971735*cos(k1*(_ftype_t)i) + 0.1980389663*cos(k2*(_ftype_t)i));
|
|
}
|
|
|
|
/* Computes the 0th order modified Bessel function of the first kind.
|
|
// (Needed to compute Kaiser window)
|
|
//
|
|
// y = sum( (x/(2*n))^2 )
|
|
// n
|
|
*/
|
|
|
|
TfirFilter::_ftype_t TfirFilter::besselizero(_ftype_t x)
|
|
{
|
|
static const _ftype_t BIZ_EPSILON=_ftype_t(1E-21); // Max error acceptable
|
|
_ftype_t temp;
|
|
_ftype_t sum = 1.0f;
|
|
_ftype_t u = 1.0f;
|
|
_ftype_t halfx = x/2.0f;
|
|
int n = 1;
|
|
|
|
do {
|
|
temp = halfx/(_ftype_t)n;
|
|
u *=temp * temp;
|
|
sum += u;
|
|
n++;
|
|
} while (u >= BIZ_EPSILON * sum);
|
|
return(sum);
|
|
}
|
|
|
|
/*
|
|
// Kaiser
|
|
//
|
|
// n window length
|
|
// w buffer for the window parameters
|
|
// b beta parameter of Kaiser window, Beta >= 1
|
|
//
|
|
// Beta trades the rejection of the low pass filter against the
|
|
// transition width from passband to stop band. Larger Beta means a
|
|
// slower transition and greater stop band rejection. See Rabiner and
|
|
// Gold (Theory and Application of DSP) under Kaiser windows for more
|
|
// about Beta. The following table from Rabiner and Gold gives some
|
|
// feel for the effect of Beta:
|
|
//
|
|
// All ripples in dB, width of transition band = D*N where N = window
|
|
// length
|
|
//
|
|
// BETA D PB RIP SB RIP
|
|
// 2.120 1.50 +-0.27 -30
|
|
// 3.384 2.23 0.0864 -40
|
|
// 4.538 2.93 0.0274 -50
|
|
// 5.658 3.62 0.00868 -60
|
|
// 6.764 4.32 0.00275 -70
|
|
// 7.865 5.0 0.000868 -80
|
|
// 8.960 5.7 0.000275 -90
|
|
// 10.056 6.4 0.000087 -100
|
|
*/
|
|
void TfirFilter::kaiser(int n, _ftype_t* w, _ftype_t b)
|
|
{
|
|
_ftype_t tmp;
|
|
_ftype_t k1 = 1.0f/besselizero(b);
|
|
int k2 = 1 - (n & 1);
|
|
int end = (n + 1) >> 1;
|
|
int i;
|
|
|
|
// Calculate window coefficients
|
|
for (i=0 ; i<end ; i++){
|
|
tmp = (_ftype_t)(2*i + k2) / ((_ftype_t)n - 1.0f);
|
|
w[end-(1&(!k2))+i] = w[end-1-i] = k1 * besselizero(_ftype_t(b*sqrt(1.0 - tmp*tmp)));
|
|
}
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* FIR filter design
|
|
******************************************************************************/
|
|
|
|
/* Design FIR filter using the Window method
|
|
|
|
n filter length must be odd for HP and BS filters
|
|
w buffer for the filter taps (must be n long)
|
|
fc cutoff frequencies (1 for LP and HP, 2 for BP and BS)
|
|
0 < fc < 1 where 1 <=> Fs/2
|
|
flags window and filter type as defined in filter.h
|
|
variables are ored together: i.e. LP|HAMMING will give a
|
|
low pass filter designed using a hamming window
|
|
opt beta constant used only when designing using kaiser windows
|
|
|
|
returns 0 if OK, -1 if fail
|
|
*/
|
|
TfirFilter::_ftype_t* TfirFilter::design_fir(unsigned int *n, _ftype_t* fc, Type type, Window window, _ftype_t opt)
|
|
{
|
|
unsigned int o = *n & 1; // Indicator for odd filter length
|
|
unsigned int end = ((*n + 1) >> 1) - o; // Loop end
|
|
unsigned int i; // Loop index
|
|
|
|
_ftype_t k1 = 2 * _ftype_t(M_PI); // 2*pi*fc1
|
|
_ftype_t k2 = 0.5f * (_ftype_t)(1 - o);// Constant used if the filter has even length
|
|
_ftype_t k3; // 2*pi*fc2 Constant used in BP and BS design
|
|
_ftype_t g = 0.0f; // Gain
|
|
_ftype_t t1,t2,t3; // Temporary variables
|
|
_ftype_t fc1,fc2; // Cutoff frequencies
|
|
|
|
// Sanity check
|
|
if(*n==0) return NULL;
|
|
fc[0]=std::clamp(fc[0],_ftype_t(0.001),_ftype_t(1));
|
|
|
|
if (!o && (type==Type::BANDSTOP || type==Type::HIGHPASS))
|
|
(*n)++;
|
|
auto w = new _ftype_t[*n];
|
|
|
|
// Get window coefficients
|
|
switch(window){
|
|
case(Window::BOX):
|
|
boxcar(*n,w); break;
|
|
case(Window::TRIANGLE):
|
|
triang(*n,w); break;
|
|
case(Window::HAMMING):
|
|
hamming(*n,w); break;
|
|
case(Window::HANNING):
|
|
hanning(*n,w); break;
|
|
case(Window::BLACKMAN):
|
|
blackman(*n,w); break;
|
|
case(Window::FLATTOP):
|
|
flattop(*n,w); break;
|
|
case(Window::KAISER):
|
|
kaiser(*n,w,opt); break;
|
|
default:
|
|
{
|
|
delete []w;
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
if(type==Type::LOWPASS || type==Type::HIGHPASS){
|
|
fc1=*fc;
|
|
// Cutoff frequency must be < 0.5 where 0.5 <=> Fs/2
|
|
fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25f;
|
|
k1 *= fc1;
|
|
|
|
if(type==Type::LOWPASS){ // Low pass filter
|
|
|
|
// If the filter length is odd, there is one point which is exactly
|
|
// in the middle. The value at this point is 2*fCutoff*sin(x)/x,
|
|
// where x is zero. To make sure nothing strange happens, we set this
|
|
// value separately.
|
|
if (o){
|
|
w[end] = fc1 * w[end] * 2.0f;
|
|
g=w[end];
|
|
}
|
|
|
|
// Create filter
|
|
for (i=0 ; i<end ; i++){
|
|
t1 = (_ftype_t)(i+1) - k2;
|
|
w[end-i-1] = w[*n-end+i] = _ftype_t(w[end-i-1] * sin(k1 * t1)/(M_PI * t1)); // Sinc
|
|
g += 2*w[end-i-1]; // Total gain in filter
|
|
}
|
|
}
|
|
else{ // High pass filter
|
|
//if (!o) // High pass filters must have odd length
|
|
// return -1;
|
|
w[end] = _ftype_t(1.0 - (fc1 * w[end] * 2.0));
|
|
g= w[end];
|
|
|
|
// Create filter
|
|
for (i=0 ; i<end ; i++){
|
|
t1 = (_ftype_t)(i+1);
|
|
w[end-i-1] = w[*n-end+i] = _ftype_t(-1 * w[end-i-1] * sin(k1 * t1)/(M_PI * t1)); // Sinc
|
|
g += ((i&1) ? (2*w[end-i-1]) : (-2*w[end-i-1])); // Total gain in filter
|
|
}
|
|
}
|
|
}
|
|
|
|
if(type==Type::BANDPASS || type==Type::BANDSTOP){
|
|
fc1=fc[0];
|
|
fc2=std::clamp(fc[1],_ftype_t(0.001),_ftype_t(1));
|
|
// Cutoff frequencies must be < 1.0 where 1.0 <=> Fs/2
|
|
fc1 = ((fc1 <= 1.0) && (fc1 > 0.0)) ? fc1/2 : 0.25f;
|
|
fc2 = ((fc2 <= 1.0) && (fc2 > 0.0)) ? fc2/2 : 0.25f;
|
|
k3 = k1 * fc2; // 2*pi*fc2
|
|
k1 *= fc1; // 2*pi*fc1
|
|
|
|
if(type==Type::BANDPASS){ // Band pass
|
|
// Calculate center tap
|
|
if (o){
|
|
g=w[end]*(fc1+fc2);
|
|
w[end] = (fc2 - fc1) * w[end] * 2.0f;
|
|
}
|
|
|
|
// Create filter
|
|
for (i=0 ; i<end ; i++){
|
|
t1 = (_ftype_t)(i+1) - k2;
|
|
t2 = _ftype_t(sin(k3 * t1)/(M_PI * t1)); // Sinc fc2
|
|
t3 = _ftype_t(sin(k1 * t1)/(M_PI * t1)); // Sinc fc1
|
|
g += w[end-i-1] * (t3 + t2); // Total gain in filter
|
|
w[end-i-1] = w[*n-end+i] = w[end-i-1] * (t2 - t3);
|
|
}
|
|
}
|
|
else{ // Band stop
|
|
//if (!o) // Band stop filters must have odd length
|
|
// return -1;
|
|
w[end] = _ftype_t(1.0 - (fc2 - fc1) * w[end] * 2.0);
|
|
g= w[end];
|
|
|
|
// Create filter
|
|
for (i=0 ; i<end ; i++){
|
|
t1 = (_ftype_t)(i+1);
|
|
t2 = _ftype_t(sin(k1 * t1)/(M_PI * t1)); // Sinc fc1
|
|
t3 = _ftype_t(sin(k3 * t1)/(M_PI * t1)); // Sinc fc2
|
|
w[end-i-1] = w[*n-end+i] = w[end-i-1] * (t2 - t3);
|
|
g += 2*w[end-i-1]; // Total gain in filter
|
|
}
|
|
}
|
|
}
|
|
|
|
// Normalize gain
|
|
g=1/g;
|
|
for (i=0; i<*n; i++)
|
|
w[i] *= g;
|
|
|
|
return w;
|
|
}
|