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

lasp/c/CMakeLists.txt

@@ -1,6 +1,5 @@
 if(!LASP_DEBUG)
-# SET_SOURCE_FILES_PROPERTIES(y.c PROPERTIES COMPILE_FLAGS -O3)
+SET_SOURCE_FILES_PROPERTIES(lasp_sosfilterbank.c PROPERTIES COMPILE_FLAGS -O3)
-# SET_SOURCE_FILES_PROPERTIES(x.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
   )

lasp/c/lasp_mat.h

@@ -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) row) <= (mat)->n_rows),OUTOFBOUNDSMATR);     \
-    dbgassert((((us) col) <= mat->n_cols),,OUTOFBOUNDSMATC);    \
+    dbgassert((((us) col) <= (mat)->n_cols),OUTOFBOUNDSMATC);    \
-    (mat)->data[(col)*(mat)->stride+(row)] = val;
+    (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;
 }
 

lasp/c/lasp_python.h

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

lasp/c/lasp_sosfilterbank.c

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

lasp/c/lasp_sosfilterbank.h

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

lasp/wrappers.pyx

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

test/filterbank_test.py

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

test/sosfilterbank_test.py

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