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:
Anne de Jong 2020-01-03 21:11:56 +01:00
parent 9afed2c9a9
commit 4cd3f0bf9f
8 changed files with 317 additions and 26 deletions

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
)

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

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

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

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:

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)