Added Second Order Sections filterbank. Works much better that Fir filterbank. Does not need decimation. Easy generation of filters with scipy.signal.butter.
This commit is contained in:
parent
9afed2c9a9
commit
4cd3f0bf9f
|
|
@ -1,6 +1,5 @@
|
|||
if(!LASP_DEBUG)
|
||||
# SET_SOURCE_FILES_PROPERTIES(y.c PROPERTIES COMPILE_FLAGS -O3)
|
||||
# SET_SOURCE_FILES_PROPERTIES(x.c PROPERTIES COMPILE_FLAGS -O3)
|
||||
SET_SOURCE_FILES_PROPERTIES(lasp_sosfilterbank.c PROPERTIES COMPILE_FLAGS -O3)
|
||||
endif(!LASP_DEBUG)
|
||||
|
||||
add_library(lasp_lib
|
||||
|
|
@ -19,7 +18,6 @@ add_library(lasp_lib
|
|||
lasp_dfifo.c
|
||||
lasp_firfilterbank.c
|
||||
lasp_sosfilterbank.c
|
||||
# lasp_octave_fir.c
|
||||
lasp_decimation.c
|
||||
lasp_sp_lowpass.c
|
||||
)
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
#define LASP_MAT_H
|
||||
#include "lasp_math_raw.h"
|
||||
#include "lasp_alloc.h"
|
||||
#include "lasp_assert.h"
|
||||
#include "lasp_tracer.h"
|
||||
#include "lasp_assert.h"
|
||||
|
||||
|
|
@ -18,7 +19,7 @@ typedef struct {
|
|||
us n_rows;
|
||||
us n_cols;
|
||||
bool _foreign_data;
|
||||
us stride;
|
||||
us colstride;
|
||||
d* _data;
|
||||
} dmat;
|
||||
|
||||
|
|
@ -27,7 +28,7 @@ typedef struct {
|
|||
us n_rows;
|
||||
us n_cols;
|
||||
bool _foreign_data;
|
||||
us stride;
|
||||
us colstride;
|
||||
c* _data;
|
||||
} cmat;
|
||||
|
||||
|
|
@ -47,9 +48,9 @@ typedef cmat vc;
|
|||
(vec)->_data[index] = val;
|
||||
|
||||
#define setmatval(mat,row,col,val) \
|
||||
dbgassert((((us) row) <= mat->n_rows),OUTOFBOUNDSMATR); \
|
||||
dbgassert((((us) col) <= mat->n_cols),,OUTOFBOUNDSMATC); \
|
||||
(mat)->data[(col)*(mat)->stride+(row)] = val;
|
||||
dbgassert((((us) row) <= (mat)->n_rows),OUTOFBOUNDSMATR); \
|
||||
dbgassert((((us) col) <= (mat)->n_cols),OUTOFBOUNDSMATC); \
|
||||
(mat)->_data[(col)*(mat)->colstride+(row)] = val;
|
||||
|
||||
/**
|
||||
* Return pointer to a value from a vector
|
||||
|
|
@ -68,7 +69,7 @@ static inline d* getdmatval(const dmat* mat,us row,us col){
|
|||
dbgassert(mat,NULLPTRDEREF);
|
||||
dbgassert(row < mat->n_rows,OUTOFBOUNDSMATR);
|
||||
dbgassert(col < mat->n_cols,OUTOFBOUNDSMATC);
|
||||
return &mat->_data[col*mat->stride+row];
|
||||
return &mat->_data[col*mat->colstride+row];
|
||||
}
|
||||
/**
|
||||
* Return a value from a matrix of complex floating points
|
||||
|
|
@ -81,7 +82,7 @@ static inline c* getcmatval(const cmat* mat,const us row,const us col){
|
|||
dbgassert(mat,NULLPTRDEREF);
|
||||
dbgassert(row < mat->n_rows,OUTOFBOUNDSMATR);
|
||||
dbgassert(col < mat->n_cols,OUTOFBOUNDSMATC);
|
||||
return &mat->_data[col*mat->stride+row];
|
||||
return &mat->_data[col*mat->colstride+row];
|
||||
}
|
||||
|
||||
static inline d* getvdval(const vd* vec,us row){
|
||||
|
|
@ -110,7 +111,7 @@ static inline c* getvcval(const vc* vec,us row){
|
|||
#define check_overflow_xmat(xmat) \
|
||||
TRACE(15,"Checking overflow " #xmat); \
|
||||
if(!(xmat)._foreign_data) { \
|
||||
dbgassert((xmat)._data[((xmat).n_cols-1)*(xmat).stride+(xmat).n_rows] \
|
||||
dbgassert((xmat)._data[((xmat).n_cols-1)*(xmat).colstride+(xmat).n_rows] \
|
||||
== OVERFLOW_MAGIC_NUMBER, \
|
||||
"Buffer overflow detected on" #xmat ); \
|
||||
} \
|
||||
|
|
@ -165,7 +166,10 @@ static inline void cmat_set(cmat* mat,const c value){
|
|||
*/
|
||||
static inline dmat dmat_alloc(us n_rows,
|
||||
us n_cols) {
|
||||
dmat result = { n_rows, n_cols, false, n_rows, NULL};
|
||||
dmat result = { n_rows, n_cols,
|
||||
false,
|
||||
n_rows, // The column stride
|
||||
NULL};
|
||||
|
||||
#ifdef LASP_DEBUG
|
||||
result._data = (d*) a_malloc((n_rows*n_cols+1)*sizeof(d));
|
||||
|
|
@ -234,7 +238,7 @@ static inline vc vc_alloc(us size) {
|
|||
* Creates a dmat from foreign data. Does not copy the data, but only
|
||||
* initializes the row pointers. Assumes column-major ordering for the
|
||||
* data. Please do not keep this one alive after the data has been
|
||||
* destroyed. Assumes the column stride equals to n_rows.
|
||||
* destroyed. Assumes the column colstride equals to n_rows.
|
||||
*
|
||||
* @param n_rows Number of rows
|
||||
* @param n_cols Number of columns
|
||||
|
|
@ -247,12 +251,12 @@ static inline dmat dmat_foreign(dmat* other) {
|
|||
dmat result = {other->n_rows,
|
||||
other->n_cols,
|
||||
true,
|
||||
other->stride,
|
||||
other->colstride,
|
||||
other->_data};
|
||||
return result;
|
||||
}
|
||||
/**
|
||||
* Create a dmat from foreign data. Assumes the stride of the data is
|
||||
* Create a dmat from foreign data. Assumes the colstride of the data is
|
||||
* n_rows.
|
||||
*
|
||||
* @param n_rows Number of rows
|
||||
|
|
@ -275,7 +279,7 @@ static inline dmat dmat_foreign_data(us n_rows,
|
|||
return result;
|
||||
}
|
||||
/**
|
||||
* Create a cmat from foreign data. Assumes the stride of the data is
|
||||
* Create a cmat from foreign data. Assumes the colstride of the data is
|
||||
* n_rows.
|
||||
*
|
||||
* @param n_rows Number of rows
|
||||
|
|
@ -302,7 +306,7 @@ static inline cmat cmat_foreign_data(us n_rows,
|
|||
* Creates a cmat from foreign data. Does not copy the data, but only
|
||||
* initializes the row pointers. Assumes column-major ordering for the
|
||||
* data. Please do not keep this one alive after the data has been
|
||||
* destroyed. Assumes the column stride equals to n_rows.
|
||||
* destroyed. Assumes the column colstride equals to n_rows.
|
||||
*
|
||||
* @param n_rows
|
||||
* @param n_cols
|
||||
|
|
@ -315,7 +319,7 @@ static inline cmat cmat_foreign(cmat* other) {
|
|||
cmat result = {other->n_rows,
|
||||
other->n_cols,
|
||||
true,
|
||||
other->stride,
|
||||
other->colstride,
|
||||
other->_data};
|
||||
return result;
|
||||
}
|
||||
|
|
@ -392,7 +396,7 @@ static inline dmat dmat_submat(const dmat* parent,
|
|||
|
||||
dmat result = { n_rows,n_cols,
|
||||
true, // Foreign data = true
|
||||
parent->n_rows, // This is the stride to get to
|
||||
parent->n_rows, // This is the colstride to get to
|
||||
// the next column.
|
||||
getdmatval(parent,startrow,startcol)};
|
||||
|
||||
|
|
@ -422,7 +426,7 @@ static inline cmat cmat_submat(cmat* parent,
|
|||
|
||||
cmat result = { n_rows,n_cols,
|
||||
true, // Foreign data = true
|
||||
parent->n_rows, // This is the stride to get to
|
||||
parent->n_rows, // This is the colstride to get to
|
||||
// the next column.
|
||||
getcmatval(parent,startrow,startcol)};
|
||||
|
||||
|
|
@ -497,7 +501,7 @@ static inline void vc_copy(vc* to,const vc* from) {
|
|||
* @return vector with reference to column
|
||||
*/
|
||||
static inline vd dmat_column(dmat* x,us col) {
|
||||
vd res = {x->n_rows,1,true,x->stride,getdmatval(x,0,col)};
|
||||
vd res = {x->n_rows,1,true,x->colstride,getdmatval(x,0,col)};
|
||||
return res;
|
||||
}
|
||||
|
||||
|
|
@ -510,7 +514,7 @@ static inline vd dmat_column(dmat* x,us col) {
|
|||
* @return vector with reference to column
|
||||
*/
|
||||
static inline vc cmat_column(cmat* x,us col) {
|
||||
vc res = {x->n_rows,1,true,x->stride,getcmatval(x,0,col)};
|
||||
vc res = {x->n_rows,1,true,x->colstride,getcmatval(x,0,col)};
|
||||
return res;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -58,8 +58,7 @@ static inline PyObject* dmat_to_ndarray(dmat* mat,bool transfer_ownership) {
|
|||
return NULL;
|
||||
}
|
||||
Py_DECREF(arr_t);
|
||||
|
||||
|
||||
|
||||
feTRACE(15);
|
||||
return arr;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,150 @@
|
|||
#define TRACERPLUS 10
|
||||
#include "lasp_sosfilterbank.h"
|
||||
|
||||
|
||||
typedef struct Sosfilterbank {
|
||||
/// The filter_coefs matrix contains filter coefficients for a SOS filter.
|
||||
us filterbank_size;
|
||||
us nsections;
|
||||
|
||||
/// The filter coefficients for each of the filters in the Filterbank
|
||||
/// The *first* axis is the filter no, the second axis contains the
|
||||
/// filter coefficients, in the order, b_0, b_1, b_2, a_0, a_1, a_2, which
|
||||
/// corresponds to the transfer function
|
||||
/// b_0 + b_1 z^-1 + b_2 z^-2
|
||||
/// H[z] = -------------------------
|
||||
/// a_0 + a_1 z^-1 + a_2 z^-2
|
||||
dmat sos; /// sos[filter_no, coeff]
|
||||
|
||||
/// Storage for the current state of the output, first axis correspond to
|
||||
/// the filter number axis, the second axis contains state coefficients
|
||||
dmat state;
|
||||
} Sosfilterbank;
|
||||
|
||||
|
||||
Sosfilterbank* Sosfilterbank_create(const us filterbank_size,
|
||||
const us nsections) {
|
||||
fsTRACE(15);
|
||||
dbgassert(filterbank_size <= MAX_SOS_FILTER_BANK_SIZE,
|
||||
"Illegal filterbank size. Max size is "
|
||||
annestr(MAX_SOS_FILTER_BANK_SIZE));
|
||||
|
||||
Sosfilterbank* fb = (Sosfilterbank*) a_malloc(sizeof(Sosfilterbank));
|
||||
|
||||
fb->filterbank_size = filterbank_size;
|
||||
dbgassert(nsections < MAX_SOS_FILTER_BANK_NSECTIONS,"Illegal number of sections");
|
||||
fb->nsections = nsections;
|
||||
|
||||
/// Allocate filter coefficients matrix
|
||||
fb->sos = dmat_alloc(filterbank_size, nsections*6);
|
||||
fb->state = dmat_alloc(filterbank_size, nsections*2);
|
||||
dmat_set(&(fb->state), 0);
|
||||
|
||||
/// Set all filter coefficients to unit impulse response
|
||||
vd imp_response = vd_alloc(6*nsections);
|
||||
vd_set(&imp_response,0);
|
||||
for(us section = 0;section < nsections; section++) {
|
||||
// Set b0 coefficient to 1
|
||||
setvecval(&imp_response, 0 + 6*section, 1);
|
||||
// Set a0 coefficient to 1
|
||||
setvecval(&imp_response, 3 + 6*section, 1);
|
||||
}
|
||||
|
||||
// Initialize all filters with a simple impulse response, single pass
|
||||
for(us filter_no = 0; filter_no < filterbank_size; filter_no++) {
|
||||
Sosfilterbank_setFilter(fb,filter_no,imp_response);
|
||||
}
|
||||
// Check if coefficients are properly initialized
|
||||
// print_dmat(&(fb->sos));
|
||||
vd_free(&imp_response);
|
||||
feTRACE(15);
|
||||
return fb;
|
||||
}
|
||||
|
||||
void Sosfilterbank_setFilter(Sosfilterbank* fb,const us filter_no,
|
||||
const vd filter_coefs) {
|
||||
fsTRACE(15);
|
||||
assertvalidptr(fb);
|
||||
assert_vx(&filter_coefs);
|
||||
dbgassert(filter_no < fb->filterbank_size, "Illegal filter number");
|
||||
dbgassert(filter_coefs.n_rows == fb->nsections * 6,
|
||||
"Illegal filter coefficient length");
|
||||
|
||||
dmat *sos = &fb->sos;
|
||||
dmat *state = &fb->state;
|
||||
us nsections = fb->nsections;
|
||||
|
||||
for(us index=0;index<nsections*6;index++){
|
||||
// Copy contents to position in sos matrix
|
||||
*getdmatval(sos,filter_no,index) = *getvdval(&filter_coefs,index);
|
||||
}
|
||||
|
||||
feTRACE(15);
|
||||
}
|
||||
|
||||
void Sosfilterbank_free(Sosfilterbank* fb) {
|
||||
fsTRACE(15);
|
||||
assertvalidptr(fb);
|
||||
|
||||
dmat_free(&(fb->sos));
|
||||
dmat_free(&(fb->state));
|
||||
|
||||
a_free(fb);
|
||||
feTRACE(15);
|
||||
|
||||
}
|
||||
|
||||
dmat Sosfilterbank_filter(Sosfilterbank* fb,const vd* xs) {
|
||||
|
||||
fsTRACE(15);
|
||||
assertvalidptr(fb);
|
||||
assert_vx(xs);
|
||||
dmat state = fb->state;
|
||||
dmat sos = fb->sos;
|
||||
|
||||
us nsections = fb->nsections;
|
||||
us filterbank_size = fb->filterbank_size;
|
||||
us nsamples = xs->n_rows;
|
||||
|
||||
dmat ys = dmat_alloc(nsamples, filterbank_size);
|
||||
/// Copy input signal to output array
|
||||
for(us filter=0;filter<filterbank_size;filter++) {
|
||||
d_copy(getdmatval(&ys,0,filter),getvdval(xs,0),nsamples,1,1);
|
||||
}
|
||||
|
||||
/// Implementation is based on Proakis & Manolakis - Digital Signal
|
||||
/// Processing, Fourth Edition, p. 550
|
||||
for(us section=0;section<nsections;section++) {
|
||||
/// Obtain state information for current section, and all filters
|
||||
|
||||
for(us filter=0;filter<filterbank_size;filter++) {
|
||||
d w1 = *getdmatval(&state,filter,section*2);
|
||||
d w2 = *getdmatval(&state,filter,section*2+1);
|
||||
|
||||
d b0 = *getdmatval(&sos,filter,section*6+0);
|
||||
d b1 = *getdmatval(&sos,filter,section*6+1);
|
||||
d b2 = *getdmatval(&sos,filter,section*6+2);
|
||||
d a0 = *getdmatval(&sos,filter,section*6+3);
|
||||
d a1 = *getdmatval(&sos,filter,section*6+4);
|
||||
d a2 = *getdmatval(&sos,filter,section*6+5);
|
||||
|
||||
d* y = getdmatval(&ys, 0, filter);
|
||||
|
||||
for(us sample=0;sample<nsamples;sample++){
|
||||
d w0 = *y - a1*w1 - a2*w2;
|
||||
d yn = b0*w0 + b1*w1 + b2*w2;
|
||||
w2 = w1;
|
||||
w1 = w0;
|
||||
*y++ = yn;
|
||||
}
|
||||
*getdmatval(&state,filter,section*2) = w1;
|
||||
*getdmatval(&state,filter,section*2+1) = w2;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
feTRACE(15);
|
||||
return ys;
|
||||
}
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
// lasp_sosfilterbank.h
|
||||
//
|
||||
// Author: J.A. de Jong - ASCEE
|
||||
//
|
||||
// Description: Implemententation of a discrete filterbank using cascaded
|
||||
// second order sections (sos), also called BiQuads.
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
#pragma once
|
||||
#ifndef LASP_FILTERBANK_H
|
||||
#define LASP_FILTERBANK_H
|
||||
#include "lasp_types.h"
|
||||
#include "lasp_mat.h"
|
||||
|
||||
#define MAX_SOS_FILTER_BANK_SIZE 40
|
||||
#define MAX_SOS_FILTER_BANK_NSECTIONS 6
|
||||
|
||||
typedef struct Sosfilterbank Sosfilterbank;
|
||||
|
||||
/**
|
||||
* Initializes a Sosfilterbank. Sets all coefficients in such a way that the
|
||||
* filter effectively does nothing (unit impulse response).
|
||||
*/
|
||||
Sosfilterbank* Sosfilterbank_create(const us filterbank_size,
|
||||
const us nsections);
|
||||
|
||||
/**
|
||||
* Initialize the filter coeficients in the filterbank
|
||||
*
|
||||
* @param fb: Filterbank handle
|
||||
* @param filter_no: Filter number in the bank
|
||||
* @param coefss: Array of filter coefficients. Should have a length of
|
||||
* nsections x 6, for each of the sections, it contains (b0, b1, b2, a0,
|
||||
* a1, a2), where a are the numerator coefficients and b are the denominator
|
||||
* coefficients.
|
||||
*
|
||||
*/
|
||||
void Sosfilterbank_setFilter(Sosfilterbank* fb,const us filter_no,
|
||||
const vd coefs);
|
||||
|
||||
/**
|
||||
* Filters x using h, returns y
|
||||
*
|
||||
* @param x Input time sequence block. Should have at least one sample.
|
||||
|
||||
* @return Filtered output in an allocated array. The number of
|
||||
* columns in this array equals the number of filters in the
|
||||
* filterbank. The number of output samples is equal to the number of
|
||||
* input samples in x.
|
||||
*/
|
||||
dmat Sosfilterbank_filter(Sosfilterbank* fb,
|
||||
const vd* x);
|
||||
|
||||
/**
|
||||
* Cleans up an existing filter bank.
|
||||
*
|
||||
* @param f Filterbank handle
|
||||
*/
|
||||
void Sosfilterbank_free(Sosfilterbank* f);
|
||||
|
||||
|
||||
#endif // LASP_FILTERBANK_H
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -65,7 +65,7 @@ cdef extern from "numpy/arrayobject.h":
|
|||
cdef extern from "lasp_python.h":
|
||||
object dmat_to_ndarray(dmat*,bint transfer_ownership)
|
||||
|
||||
__all__ = ['AvPowerSpectra']
|
||||
__all__ = ['AvPowerSpectra', 'SosFilterBank', 'FilterBank']
|
||||
|
||||
setTracerLevel(15)
|
||||
cdef extern from "cblas.h":
|
||||
|
|
@ -392,6 +392,58 @@ cdef class FilterBank:
|
|||
|
||||
return result
|
||||
|
||||
cdef extern from "lasp_sosfilterbank.h":
|
||||
ctypedef struct c_Sosfilterbank "Sosfilterbank"
|
||||
c_Sosfilterbank* Sosfilterbank_create(const us filterbank_size, const us nsections) nogil
|
||||
void Sosfilterbank_setFilter(c_Sosfilterbank* fb,const us filter_no, const vd filter_coefs)
|
||||
dmat Sosfilterbank_filter(c_Sosfilterbank* fb,const vd* x) nogil
|
||||
void Sosfilterbank_free(c_Sosfilterbank* fb) nogil
|
||||
|
||||
|
||||
cdef class SosFilterBank:
|
||||
cdef:
|
||||
c_Sosfilterbank* fb
|
||||
def __cinit__(self,const us filterbank_size, const us nsections):
|
||||
|
||||
self.fb = Sosfilterbank_create(filterbank_size,nsections)
|
||||
|
||||
def setFilter(self,us filter_no, d[:, ::1] sos):
|
||||
"""
|
||||
|
||||
Args:
|
||||
filter_no: Filter number of the filterbank to set the
|
||||
filter for
|
||||
sos: Second axis are the filter coefficients, first axis
|
||||
is the section
|
||||
|
||||
"""
|
||||
cdef dmat coefs = dmat_foreign_data(sos.size,1,
|
||||
&sos[0, 0],False)
|
||||
Sosfilterbank_setFilter(self.fb,filter_no, coefs)
|
||||
|
||||
def __dealloc__(self):
|
||||
if self.fb:
|
||||
Sosfilterbank_free(self.fb)
|
||||
|
||||
def filter_(self,d[::1, :] input_):
|
||||
# Only single channel input
|
||||
assert input_.shape[1] == 1
|
||||
cdef dmat input_vd = dmat_foreign_data(input_.shape[0],1,
|
||||
&input_[0, 0],False)
|
||||
|
||||
|
||||
cdef dmat output
|
||||
with nogil:
|
||||
output = Sosfilterbank_filter(self.fb,&input_vd)
|
||||
|
||||
# Steal the pointer from output
|
||||
result = dmat_to_ndarray(&output,True)
|
||||
|
||||
dmat_free(&output)
|
||||
vd_free(&input_vd)
|
||||
|
||||
return result
|
||||
|
||||
cdef extern from "lasp_decimation.h":
|
||||
ctypedef struct c_Decimator "Decimator"
|
||||
ctypedef enum DEC_FAC:
|
||||
|
|
|
|||
|
|
@ -33,4 +33,4 @@ print('res shape %i:\n' %res.shape[0])
|
|||
print(res)
|
||||
#for i in range(4):
|
||||
# res = fb.filter_(data)
|
||||
# print('res:\n',res)
|
||||
# print('res:\n',res)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,26 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Mon Jan 15 19:45:33 2018
|
||||
|
||||
@author: anne
|
||||
"""
|
||||
import numpy as np
|
||||
from lasp.wrappers import SosFilterBank
|
||||
|
||||
fb = SosFilterBank(1, 2)
|
||||
|
||||
newfilter1 = np.array([0,1.,0.,1.,0.,0.])[:,np.newaxis] # Unit sample delay
|
||||
newfilter2 = np.array([0,1.,0.,1.,0.,0.])[:,np.newaxis] # Unit sample delay
|
||||
newfilter = np.vstack((newfilter1, newfilter2))
|
||||
|
||||
fb.setFilter(0, newfilter)
|
||||
x = np.zeros(10)
|
||||
x[5]=1
|
||||
x[8] = 3
|
||||
x = x[:,np.newaxis]
|
||||
y = fb.filter_(x)
|
||||
print(x)
|
||||
print(y)
|
||||
y = fb.filter_(x)
|
||||
print(y)
|
||||
Loading…
Reference in New Issue