Added filterbank and toebehoren
This commit is contained in:
J.A. de Jong - ASCEE
parent
aa3935a82b
commit
ad6c3213cd
|
|
@ -16,7 +16,7 @@ add_library(beamforming_lib
|
|||
mq.c
|
||||
worker.c
|
||||
dfifo.c
|
||||
filter.c
|
||||
filterbank.c
|
||||
)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,60 @@
|
|||
// ascee_python.h
|
||||
//
|
||||
// Author: J.A. de Jong - ASCEE
|
||||
//
|
||||
// Description:
|
||||
// Some routines to generate numpy arrays from matrices and vectors.
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
#pragma once
|
||||
#ifndef ASCEE_PYTHON_H
|
||||
#define ASCEE_PYTHON_H
|
||||
#include <numpy/ndarrayobject.h>
|
||||
#ifdef ASCEE_DOUBLE_PRECISION
|
||||
#define ASCEE_NUMPY_FLOAT_TYPE NPY_FLOAT64
|
||||
#define ASCEE_NUMPY_COMPLEX_TYPE NPY_COMPLEX128
|
||||
#else
|
||||
#define ASCEE_NUMPY_FLOAT_TYPE NPY_FLOAT32
|
||||
#endif
|
||||
|
||||
|
||||
static inline PyObject* dmat_to_ndarray(dmat* mat,bool transfer_ownership) {
|
||||
fsTRACE(15);
|
||||
dbgassert(mat,NULLPTRDEREF);
|
||||
|
||||
import_array();
|
||||
|
||||
// Dimensions given in wrong order, as mat is
|
||||
// Fortran-contiguous. Later on we transpose the result. This is
|
||||
// more easy than using the PyArray_New syntax.
|
||||
npy_intp dims[] = {mat->n_cols,mat->n_rows};
|
||||
PyObject* arr_t = PyArray_SimpleNewFromData(2,dims,
|
||||
ASCEE_NUMPY_FLOAT_TYPE,
|
||||
mat->_data);
|
||||
if(!arr_t) {
|
||||
WARN("Array creation failure");
|
||||
feTRACE(15);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if(transfer_ownership) {
|
||||
mat->_foreign_data = true;
|
||||
PyArray_ENABLEFLAGS(arr_t, NPY_OWNDATA);
|
||||
}
|
||||
|
||||
// Transpose the array
|
||||
PyObject* arr = PyArray_Transpose(arr_t,NULL);
|
||||
if(!arr) {
|
||||
WARN("Array transpose failure");
|
||||
feTRACE(15);
|
||||
return NULL;
|
||||
}
|
||||
Py_DECREF(arr_t);
|
||||
|
||||
|
||||
feTRACE(15);
|
||||
return arr;
|
||||
}
|
||||
|
||||
|
||||
#endif // ASCEE_PYTHON_H
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -0,0 +1,167 @@
|
|||
// filterbank.c
|
||||
//
|
||||
// Author: J.A. de Jong -ASCEE
|
||||
//
|
||||
// Description:
|
||||
// FilterBank implementation.
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
#include "filterbank.h"
|
||||
#include "fft.h"
|
||||
#include "dfifo.h"
|
||||
#include "ascee_tracer.h"
|
||||
#include "ascee_alg.h"
|
||||
#define FIFO_SIZE_MULT 2
|
||||
|
||||
typedef struct FilterBank_s {
|
||||
us nfft;
|
||||
|
||||
us P_m_1; /**< Filter length minus one */
|
||||
|
||||
cmat filters; /* Frequency-domain filter
|
||||
* coefficients */
|
||||
dFifo* output_fifo;
|
||||
dFifo* input_fifo;
|
||||
|
||||
Fft* fft; /* Handle to internal FFT-function */
|
||||
|
||||
} FilterBank;
|
||||
|
||||
FilterBank* FilterBank_create(const dmat* h,
|
||||
const us nfft) {
|
||||
|
||||
fsTRACE(15);
|
||||
dbgassert(h,NULLPTRDEREF);
|
||||
const us P = h->n_rows;
|
||||
const us nfilters = h->n_cols;
|
||||
|
||||
if(P > nfft/2) {
|
||||
WARN("Filter order should be <= nfft/2");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
Fft* fft = Fft_create(nfft);
|
||||
if(!fft) {
|
||||
WARN("Fft allocation failed");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
FilterBank* fb = a_malloc(sizeof(FilterBank));
|
||||
|
||||
fb->nfft = nfft;
|
||||
fb->P_m_1 = P-1;
|
||||
fb->fft = fft;
|
||||
fb->filters = cmat_alloc(nfft/2+1,nfilters);
|
||||
|
||||
fb->output_fifo = dFifo_create(nfilters,FIFO_SIZE_MULT*nfft);
|
||||
fb->input_fifo = dFifo_create(1,FIFO_SIZE_MULT*nfft);
|
||||
|
||||
/* Create a temporary buffer which is going to be FFT'th to
|
||||
* contain the filter transfer functions.
|
||||
*/
|
||||
dmat temp = dmat_alloc(nfft,nfilters);
|
||||
dmat_set(&temp,0);
|
||||
dmat_copy_rows(&temp,h,0,0,h->n_rows);
|
||||
|
||||
/* Fft the FIR impulse responses */
|
||||
Fft_fft(fb->fft,&temp,&fb->filters);
|
||||
|
||||
dmat_free(&temp);
|
||||
|
||||
/* Push initial output of zeros to the output fifo */
|
||||
dmat initial_output = dmat_alloc(fb->P_m_1,nfilters);
|
||||
dmat_set(&initial_output,0);
|
||||
dFifo_push(fb->output_fifo,&initial_output);
|
||||
dmat_free(&initial_output);
|
||||
|
||||
feTRACE(15);
|
||||
return fb;
|
||||
}
|
||||
void FilterBank_free(FilterBank* fb) {
|
||||
fsTRACE(15);
|
||||
dbgassert(fb,NULLPTRDEREF);
|
||||
cmat_free(&fb->filters);
|
||||
dFifo_free(fb->input_fifo);
|
||||
dFifo_free(fb->output_fifo);
|
||||
Fft_free(fb->fft);
|
||||
a_free(fb);
|
||||
feTRACE(15);
|
||||
}
|
||||
dmat FilterBank_filter(FilterBank* fb,
|
||||
const vd* x) {
|
||||
|
||||
fsTRACE(15);
|
||||
dbgassert(fb && x ,NULLPTRDEREF);
|
||||
|
||||
dFifo* input_fifo = fb->input_fifo;
|
||||
dFifo* output_fifo = fb->output_fifo;
|
||||
|
||||
const us nfft = fb->nfft;
|
||||
const us nfilters = fb->filters.n_cols;
|
||||
|
||||
/* Push samples to the input fifo */
|
||||
dmat samples = dmat_foreign_vd((vd*) x);
|
||||
dFifo_push(fb->input_fifo,&samples);
|
||||
dmat_free(&samples);
|
||||
|
||||
dmat input_block = dmat_alloc(nfft,1);
|
||||
|
||||
/* Output is ready to be multiplied with FFt'th filter */
|
||||
cmat input_fft = cmat_alloc(nfft/2+1,1);
|
||||
|
||||
/* Output is ready to be multiplied with FFt'th filter */
|
||||
cmat output_fft = cmat_alloc(nfft/2+1,nfilters);
|
||||
dmat output_block = dmat_alloc(nfft,nfilters);
|
||||
|
||||
while (dFifo_size(input_fifo) >= nfft) {
|
||||
|
||||
us nsamples = dFifo_pop(input_fifo,
|
||||
&input_block,
|
||||
fb->P_m_1 /* save P-1 samples */
|
||||
);
|
||||
dbgassert(nsamples == nfft,"BUG in dFifo");
|
||||
|
||||
Fft_fft(fb->fft,&input_block,&input_fft);
|
||||
|
||||
vc input_fft_col = cmat_column(&input_fft,0);
|
||||
|
||||
for(us col=0;col<nfilters;col++) {
|
||||
vc output_fft_col = cmat_column(&output_fft,col);
|
||||
vc filter_col = cmat_column(&fb->filters,col);
|
||||
vc_hadamard(&output_fft_col,
|
||||
&input_fft_col,
|
||||
&filter_col);
|
||||
|
||||
vc_free(&output_fft_col);
|
||||
vc_free(&filter_col);
|
||||
}
|
||||
vc_free(&input_fft_col);
|
||||
|
||||
Fft_ifft(fb->fft,&output_fft,&output_block);
|
||||
|
||||
dmat valid_samples = dmat_submat(&output_block,
|
||||
0,0, /* startrow, startcol */
|
||||
nfft-fb->P_m_1, /* Number of rows */
|
||||
output_block.n_cols);
|
||||
dFifo_push(fb->output_fifo,&valid_samples);
|
||||
dmat_free(&valid_samples);
|
||||
|
||||
}
|
||||
|
||||
dmat_free(&input_block);
|
||||
cmat_free(&input_fft);
|
||||
cmat_free(&output_fft);
|
||||
dmat_free(&output_block);
|
||||
|
||||
us samples_done = dFifo_size(output_fifo);
|
||||
uVARTRACE(20,samples_done);
|
||||
dmat filtered_result = dmat_alloc(samples_done,nfilters);
|
||||
if(samples_done) {
|
||||
us samples_done2 = dFifo_pop(output_fifo,&filtered_result,0);
|
||||
dbgassert(samples_done2 == samples_done,"BUG in dFifo");
|
||||
}
|
||||
feTRACE(15);
|
||||
return filtered_result;
|
||||
}
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
// filterbank.h
|
||||
//
|
||||
// Author: J.A. de Jong - ASCEE
|
||||
//
|
||||
// Description: Implemententation of a discrete filterbank using fast
|
||||
// convolution and the overlap-save (overlap-scrap method). Multiple
|
||||
// filters can be applied to the same input data (*filterbank*).
|
||||
// Implementation is computationally efficient, as the forward FFT is
|
||||
// performed only over the input data, and the backwards transfer for
|
||||
// each filter in the filterbank.
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
#pragma once
|
||||
#ifndef FILTERBANK_H
|
||||
#define FILTERBANK_H
|
||||
#include "types.h"
|
||||
#include "ascee_math.h"
|
||||
typedef struct FilterBank_s FilterBank;
|
||||
|
||||
/**
|
||||
* Initializes a fast convolution filter bank and returns a FilterBank
|
||||
* handle. The nfft will be chosen to be at least four times the
|
||||
* length of the FIR filters.
|
||||
*
|
||||
* @param h: matrix with filter coefficients of each of the
|
||||
* filters. First axis is the axis of the filter coefficients, second
|
||||
* axis is the filter number. Maximum length of the filter is nfft/2.
|
||||
*
|
||||
* @param nfft: FTT length for fast convolution. For good performance,
|
||||
* nfft should be chosen as the nearest power of 2, approximately four
|
||||
* times the filter lengths. For the lowest possible latency, it is
|
||||
* better to set nfft at twice the filter length.
|
||||
*
|
||||
* @return FilterBank handle, NULL on error.
|
||||
*/
|
||||
FilterBank* FilterBank_create(const dmat* h,const us nfft);
|
||||
|
||||
/**
|
||||
* 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 FilterBank_filter(FilterBank* fb,
|
||||
const vd* x);
|
||||
|
||||
/**
|
||||
* Cleans up an existing filter bank.
|
||||
*
|
||||
* @param f Filter handle
|
||||
*/
|
||||
void FilterBank_free(FilterBank* f);
|
||||
|
||||
|
||||
#endif // FILTERBANK_H
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -28,7 +28,7 @@ cdef extern from "ascee_tracer.h":
|
|||
void TRACE(int,const char*)
|
||||
void fsTRACE(int)
|
||||
void feTRACE(int)
|
||||
|
||||
void clearScreen()
|
||||
cdef extern from "ascee_math.h":
|
||||
ctypedef struct dmat:
|
||||
us n_cols
|
||||
|
|
@ -50,10 +50,16 @@ cdef extern from "ascee_math.h":
|
|||
|
||||
cmat cmat_alloc(us n_rows,us n_cols)
|
||||
dmat dmat_alloc(us n_rows,us n_cols)
|
||||
|
||||
vd vd_foreign(const us size,d* data)
|
||||
void vd_free(vd*)
|
||||
|
||||
void dmat_free(dmat*)
|
||||
void cmat_free(cmat*)
|
||||
void cmat_copy(cmat* to,cmat* from_)
|
||||
|
||||
cdef extern from "ascee_tracer.h":
|
||||
void clearScreen()
|
||||
cdef extern from "numpy/arrayobject.h":
|
||||
void PyArray_ENABLEFLAGS(np.ndarray arr, int flags)
|
||||
|
||||
cdef extern from "ascee_python.h":
|
||||
object dmat_to_ndarray(dmat*,bint transfer_ownership)
|
||||
|
||||
|
|
|
|||
|
|
@ -259,3 +259,37 @@ cdef class AvPowerSpectra:
|
|||
dmat_free(&td)
|
||||
|
||||
return result
|
||||
|
||||
cdef extern from "filterbank.h":
|
||||
ctypedef struct c_FilterBank "FilterBank":
|
||||
pass
|
||||
c_FilterBank* FilterBank_create(const dmat* h,const us nfft) nogil
|
||||
dmat FilterBank_filter(c_FilterBank* fb,const vd* x) nogil
|
||||
void FilterBank_free(c_FilterBank* fb) nogil
|
||||
|
||||
|
||||
cdef class FilterBank:
|
||||
cdef:
|
||||
c_FilterBank* fb
|
||||
def __cinit__(self,d[::1,:] h, us nfft):
|
||||
cdef dmat hmat = dmat_foreign(h.shape[0],h.shape[1],&h[0,0])
|
||||
self.fb = FilterBank_create(&hmat,nfft)
|
||||
dmat_free(&hmat)
|
||||
if not self.fb:
|
||||
raise RuntimeError('Error creating FilberBank')
|
||||
|
||||
def __dealloc__(self):
|
||||
if self.fb:
|
||||
FilterBank_free(self.fb)
|
||||
|
||||
def filter_(self,d[:] input_):
|
||||
cdef vd input_vd = vd_foreign(input_.size,&input_[0])
|
||||
cdef dmat output = FilterBank_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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,36 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Mon Jan 15 19:45:33 2018
|
||||
|
||||
@author: anne
|
||||
"""
|
||||
import numpy as np
|
||||
from beamforming import FilterBank, cls
|
||||
cls()
|
||||
|
||||
nfft=50
|
||||
P = 4
|
||||
L = nfft-P+2
|
||||
nfilters = 1
|
||||
print('nfft:',nfft)
|
||||
|
||||
h = np.zeros((P,nfilters),order='F')
|
||||
h[P-1,0] = 1.
|
||||
#h[0,1] = 1.
|
||||
|
||||
fb = FilterBank(h,nfft)
|
||||
# %%
|
||||
#data = np.zeros(nfft//2,order = 'F')
|
||||
data = np.zeros(L,order = 'F')
|
||||
data[:] = 1
|
||||
#data[:] = 1.
|
||||
#data[1] = 1.
|
||||
#data[2] = 1.
|
||||
|
||||
res = fb.filter_(data)
|
||||
print('res shape %i:\n' %res.shape[0])
|
||||
print(res)
|
||||
#for i in range(4):
|
||||
# res = fb.filter_(data)
|
||||
# print('res:\n',res)
|
||||
Loading…
Reference in New Issue