ASCEE/lasp
ASCEE/lasp
5
1
Fork 0
Code Issues 5 Pull Requests Actions Releases 14 Activity

Added Second Order Sections filterbank. Works much better that Fir filterbank. Does not need decimation. Easy generation of filters with scipy.signal.butter.

Browse Source
This commit is contained in:
Anne de Jong 2020-01-03 21:11:56 +01:00
parent 9afed2c9a9
commit 4cd3f0bf9f
8 changed files with 317 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
lasp/c/CMakeLists.txt
View File

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

42
lasp/c/lasp_mat.h
View File

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

1
lasp/c/lasp_python.h
View File

@ -59,7 +59,6 @@ static inline PyObject* dmat_to_ndarray(dmat* mat,bool transfer_ownership) {
}
Py_DECREF(arr_t);
feTRACE(15);
return arr;
}

150
lasp/c/lasp_sosfilterbank.c Normal file
View File

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

62
lasp/c/lasp_sosfilterbank.h Normal file
View File

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

54
lasp/wrappers.pyx
View File

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

26
test/sosfilterbank_test.py Normal file
View File

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