From d056668887fde2dd2a784ef3507ec35e98131439 Mon Sep 17 00:00:00 2001 From: "N.N." Date: Wed, 2 Aug 2006 14:02:28 +0000 Subject: no message svn path=/trunk/externals/iem/iem_adaptfilt/; revision=5455 --- src/.DS_Store | Bin 0 -> 6148 bytes src/iem_adaptfilt.c | 46 +++++ src/iemlib.h | 102 +++++++++++ src/makefile.txt | 50 +++++ src/makefile_lin | 51 ++++++ src/makefile_win | 44 +++++ src/makefile_win.txt | 43 +++++ src/sigNLMS.c | 337 ++++++++++++++++++++++++++++++++++ src/sigNLMSCC.c | 390 +++++++++++++++++++++++++++++++++++++++ src/sign_CLNLMS.c | 503 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/sign_CNLMS.c | 482 ++++++++++++++++++++++++++++++++++++++++++++++++ 11 files changed, 2048 insertions(+) create mode 100644 src/.DS_Store create mode 100644 src/iem_adaptfilt.c create mode 100644 src/iemlib.h create mode 100644 src/makefile.txt create mode 100644 src/makefile_lin create mode 100644 src/makefile_win create mode 100644 src/makefile_win.txt create mode 100644 src/sigNLMS.c create mode 100644 src/sigNLMSCC.c create mode 100644 src/sign_CLNLMS.c create mode 100644 src/sign_CNLMS.c (limited to 'src') diff --git a/src/.DS_Store b/src/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/src/.DS_Store differ diff --git a/src/iem_adaptfilt.c b/src/iem_adaptfilt.c new file mode 100644 index 0000000..11381ee --- /dev/null +++ b/src/iem_adaptfilt.c @@ -0,0 +1,46 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +iem_adaptfilt written by Markus Noisternig & Thomas Musil +noisternig_AT_iem.at; musil_AT_iem.at +(c) Institute of Electronic Music and Acoustics, Graz Austria 2005 */ + +#ifdef NT +#pragma warning( disable : 4244 ) +#pragma warning( disable : 4305 ) +#endif + + +#include "m_pd.h" +#include "iemlib.h" + +static t_class *iem_adaptfilt_class; + +static void *iem_adaptfilt_new(void) +{ + t_object *x = (t_object *)pd_new(iem_adaptfilt_class); + + return (x); +} + +void sigNLMS_setup(void); +void sigNLMSCC_setup(void); +void sign_CNLMS_setup(void); +void sign_CLNLMS_setup(void); + +/* ------------------------ setup routine ------------------------- */ + +void iem_adaptfilt_setup(void) +{ + sigNLMS_setup(); + sigNLMSCC_setup(); + sign_CNLMS_setup(); + sign_CLNLMS_setup(); + + post("----------------------------------------------"); + post("iem_adaptfilt (R-1.02) library loaded!"); + post("(c) Markus Noisternig, Thomas Musil"); + post(" {noisternig, musil}_AT_iem.at"); + post(" IEM Graz, Austria"); + post("----------------------------------------------"); +} diff --git a/src/iemlib.h b/src/iemlib.h new file mode 100644 index 0000000..0de2d9d --- /dev/null +++ b/src/iemlib.h @@ -0,0 +1,102 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +iemlib2 written by Thomas Musil, Copyright (c) IEM KUG Graz Austria 2000 - 2004 */ + +#ifndef __IEMLIB_H__ +#define __IEMLIB_H__ + + +#define IS_A_POINTER(atom,index) ((atom+index)->a_type == A_POINTER) +#define IS_A_FLOAT(atom,index) ((atom+index)->a_type == A_FLOAT) +#define IS_A_SYMBOL(atom,index) ((atom+index)->a_type == A_SYMBOL) +#define IS_A_DOLLAR(atom,index) ((atom+index)->a_type == A_DOLLAR) +#define IS_A_DOLLSYM(atom,index) ((atom+index)->a_type == A_DOLLSYM) +#define IS_A_SEMI(atom,index) ((atom+index)->a_type == A_SEMI) +#define IS_A_COMMA(atom,index) ((atom+index)->a_type == A_COMMA) + + +#ifdef NT +int sys_noloadbang; +//t_symbol *iemgui_key_sym=0; +#include +#else +extern int sys_noloadbang; +//extern t_symbol *iemgui_key_sym; +#include +#endif + +#define DEFDELVS 64 +#define XTRASAMPS 4 +#define SAMPBLK 4 + + +#define UNITBIT32 1572864. /* 3*2^19; bit 32 has place value 1 */ + +/* machine-dependent definitions. These ifdefs really +should have been by CPU type and not by operating system! */ +#ifdef IRIX +/* big-endian. Most significant byte is at low address in memory */ +#define HIOFFSET 0 /* word offset to find MSB */ +#define LOWOFFSET 1 /* word offset to find LSB */ +#define int32 long /* a data type that has 32 bits */ +#else +#ifdef MSW +/* little-endian; most significant byte is at highest address */ +#define HIOFFSET 1 +#define LOWOFFSET 0 +#define int32 long +#else +#ifdef __FreeBSD__ +#include +#if BYTE_ORDER == LITTLE_ENDIAN +#define HIOFFSET 1 +#define LOWOFFSET 0 +#else +#define HIOFFSET 0 /* word offset to find MSB */ +#define LOWOFFSET 1 /* word offset to find LSB */ +#endif /* BYTE_ORDER */ +#include +#define int32 int32_t +#endif +#ifdef __linux__ + +#include + +#if !defined(__BYTE_ORDER) || !defined(__LITTLE_ENDIAN) +#error No byte order defined +#endif + +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define HIOFFSET 1 +#define LOWOFFSET 0 +#else +#define HIOFFSET 0 /* word offset to find MSB */ +#define LOWOFFSET 1 /* word offset to find LSB */ +#endif /* __BYTE_ORDER */ + +#include +#define int32 int32_t + +#else +#ifdef __APPLE__ +#define HIOFFSET 0 /* word offset to find MSB */ +#define LOWOFFSET 1 /* word offset to find LSB */ +#define int32 int /* a data type that has 32 bits */ + +#endif /* __APPLE__ */ +#endif /* __linux__ */ +#endif /* MSW */ +#endif /* SGI */ + +union tabfudge +{ + double tf_d; + int32 tf_i[2]; +}; + +#define IEM_DENORMAL(f) ((((*(unsigned int*)&(f))&0x60000000)==0) || \ +(((*(unsigned int*)&(f))&0x60000000)==0x60000000)) +/* more stringent test: anything not between 1e-19 and 1e19 in absolute val */ + +#endif diff --git a/src/makefile.txt b/src/makefile.txt new file mode 100644 index 0000000..6654843 --- /dev/null +++ b/src/makefile.txt @@ -0,0 +1,50 @@ +current: all + +.SUFFIXES: .pd_linux + +INCLUDE = -I. -I/usr/local/src/pd-0.37-1/src + +LDFLAGS = -export-dynamic -shared +LIB = -ldl -lm -lpthread + +#select either the DBG and OPT compiler flags below: + +CFLAGS = -DPD -DUNIX -W -Werror -Wno-unused \ + -Wno-parentheses -Wno-switch -O6 -funroll-loops -fomit-frame-pointer \ + -DDL_OPEN + +SYSTEM = $(shell uname -m) + +# the sources + +SRC = sigNLMS.c \ + sigNLMSCC.c \ + sign_CNLMS.c \ + iem_adaptfilt.c + +TARGET = iem_adaptfilt.pd_linux + + +OBJ = $(SRC:.c=.o) + +# +# ------------------ targets ------------------------------------ +# + +clean: + rm $(TARGET) + rm *.o + +all: $(OBJ) + @echo :: $(OBJ) + ld $(LDFLAGS) -o $(TARGET) *.o $(LIB) + strip --strip-unneeded $(TARGET) + rm *.o + +$(OBJ) : %.o : %.c + touch $*.c + cc $(CFLAGS) $(INCLUDE) -c -o $*.o $*.c + + + + diff --git a/src/makefile_lin b/src/makefile_lin new file mode 100644 index 0000000..1e6a625 --- /dev/null +++ b/src/makefile_lin @@ -0,0 +1,51 @@ +current: all + +.SUFFIXES: .pd_linux + +INCLUDE = -I. -I/usr/local/src/pd-0.37-1/src + +LDFLAGS = -export-dynamic -shared +LIB = -ldl -lm -lpthread + +#select either the DBG and OPT compiler flags below: + +CFLAGS = -DPD -DUNIX -W -Werror -Wno-unused \ + -Wno-parentheses -Wno-switch -O6 -funroll-loops -fomit-frame-pointer \ + -DDL_OPEN + +SYSTEM = $(shell uname -m) + +# the sources + +SRC = sigNLMS.c \ + sigNLMSCC.c \ + sign_CNLMS.c \ + sign_CLNLMS.c \ + iem_adaptfilt.c + +TARGET = iem_adaptfilt.pd_linux + + +OBJ = $(SRC:.c=.o) + +# +# ------------------ targets ------------------------------------ +# + +clean: + rm ../../lib/$(TARGET) + rm *.o + +all: $(OBJ) + @echo :: $(OBJ) + ld $(LDFLAGS) -o $(TARGET) *.o $(LIB) + strip --strip-unneeded $(TARGET) + rm *.o + +$(OBJ) : %.o : %.c + touch $*.c + cc $(CFLAGS) $(INCLUDE) -c -o $*.o $*.c + + + + diff --git a/src/makefile_win b/src/makefile_win new file mode 100644 index 0000000..fe10c99 --- /dev/null +++ b/src/makefile_win @@ -0,0 +1,44 @@ + +all: iem_adaptfilt.dll + +VIS_CPP_PATH = "C:\Programme\Microsoft Visual Studio\Vc98" + +PD_INST_PATH = "C:\Programme\pd-0.37-3" + +PD_WIN_INCLUDE_PATH = /I. /I$(PD_INST_PATH)\src /I$(VIS_CPP_PATH)\include + +PD_WIN_C_FLAGS = /nologo /W3 /WX /DMSW /DNT /DPD /DWIN32 /DWINDOWS /Ox -DPA_LITTLE_ENDIAN + +PD_WIN_L_FLAGS = /nologo + +PD_WIN_LIB = /NODEFAULTLIB:libc /NODEFAULTLIB:oldnames /NODEFAULTLIB:kernel /NODEFAULTLIB:uuid \ + $(VIS_CPP_PATH)\lib\libc.lib \ + $(VIS_CPP_PATH)\lib\oldnames.lib \ + $(VIS_CPP_PATH)\lib\kernel32.lib \ + $(VIS_CPP_PATH)\lib\wsock32.lib \ + $(VIS_CPP_PATH)\lib\winmm.lib \ + $(PD_INST_PATH)\bin\pthreadVC.lib \ + $(PD_INST_PATH)\bin\pd.lib + + +SRC = sigNLMS.c \ + sigNLMSCC.c \ + sign_CNLMS.c \ + sign_CLNLMS.c \ + iem_adaptfilt.c + + +OBJ = $(SRC:.c=.obj) + +.c.obj: + cl $(PD_WIN_C_FLAGS) $(PD_WIN_INCLUDE_PATH) /c $*.c + +iem_adaptfilt.dll: $(OBJ) + link $(PD_WIN_L_FLAGS) /dll /export:iem_adaptfilt_setup \ + /out:iem_adaptfilt.dll $(OBJ) $(PD_WIN_LIB) + + +clean: + del *.obj + + diff --git a/src/makefile_win.txt b/src/makefile_win.txt new file mode 100644 index 0000000..bfbedf8 --- /dev/null +++ b/src/makefile_win.txt @@ -0,0 +1,43 @@ + +all: iem_adaptfilt.dll + +VIS_CPP_PATH = "C:\Programme\Microsoft Visual Studio\Vc98" + +PD_INST_PATH = "C:\Programme\pd-0.37-1" + +PD_WIN_INCLUDE_PATH = /I. /I$(PD_INST_PATH)\src /I$(VIS_CPP_PATH)\include + +PD_WIN_C_FLAGS = /nologo /W3 /WX /DMSW /DNT /DPD /DWIN32 /DWINDOWS /Ox -DPA_LITTLE_ENDIAN + +PD_WIN_L_FLAGS = /nologo + +PD_WIN_LIB = /NODEFAULTLIB:libc /NODEFAULTLIB:oldnames /NODEFAULTLIB:kernel /NODEFAULTLIB:uuid \ + $(VIS_CPP_PATH)\lib\libc.lib \ + $(VIS_CPP_PATH)\lib\oldnames.lib \ + $(VIS_CPP_PATH)\lib\kernel32.lib \ + $(VIS_CPP_PATH)\lib\wsock32.lib \ + $(VIS_CPP_PATH)\lib\winmm.lib \ + $(PD_INST_PATH)\bin\pthreadVC.lib \ + $(PD_INST_PATH)\bin\pd.lib + + +SRC = sigNLMS.c \ + sigNLMSCC.c \ + sign_CNLMS.c \ + sign_CLNLMS.c \ + iem_adaptfilt.c + + +OBJ = $(SRC:.c=.obj) + +.c.obj: + cl $(PD_WIN_C_FLAGS) $(PD_WIN_INCLUDE_PATH) /c $*.c + +iem_adaptfilt.dll: $(OBJ) + link $(PD_WIN_L_FLAGS) /dll /export:iem_adaptfilt_setup \ + /out:iem_adaptfilt.dll $(OBJ) $(PD_WIN_LIB) + copy iem_adaptfilt.dll ..\..\lib\iem_adaptfilt.dll + copy iem_adaptfilt.dll ..\..\..\iem_adaptfilt.dll + +clean: + del *.obj diff --git a/src/sigNLMS.c b/src/sigNLMS.c new file mode 100644 index 0000000..53ab284 --- /dev/null +++ b/src/sigNLMS.c @@ -0,0 +1,337 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +NLMS normalized least mean square (LMS) algorithm +lib iem_adaptfilt written by Markus Noisternig & Thomas Musil +noisternig_AT_iem.at; musil_AT_iem.at +(c) Institute of Electronic Music and Acoustics, Graz Austria 2005 */ + +#ifdef NT +#pragma warning( disable : 4244 ) +#pragma warning( disable : 4305 ) +#endif + + +#include "m_pd.h" +#include "iemlib.h" +#include +#include +#include + + +/* ----------------------- NLMS~ ------------------------------ */ +/* -- Normalized Least Mean Square (linear adaptive FIR-filter) -- */ +/* -- first input: reference signal -- */ +/* -- second input: desired signal -- */ +/* -- -- */ + +/* for further information on adaptive filter design we refer to */ +/* [1] Haykin, "Adaptive Filter Theory", 4th ed, Prentice Hall */ +/* [2] Benesty, "Adaptive Signal Processing", Springer */ + + +typedef struct sigNLMS +{ + t_object x_obj; + t_symbol *x_w_array_sym_name; + t_float *x_w_array_mem_beg; + t_float *x_io_ptr_beg[4];// memory: 2 sig-in and 2 sig-out vectors + t_float *x_in_hist;// start point double buffer for sig-in history + t_int x_rw_index;// read-write-index + t_int x_n_order;// order of filter + t_int x_update;// 2^n rounded value, downsampling of update speed + t_float x_beta;// learn rate [0 .. 2] + t_float x_gamma;// regularization + t_float x_msi; +} t_sigNLMS; + +t_class *sigNLMS_class; + +static t_float *sigNLMS_check_array(t_symbol *array_sym_name, t_int length) +{ + t_int n_points; + t_garray *a; + t_float *vec; + + if(!(a = (t_garray *)pd_findbyclass(array_sym_name, garray_class))) + { + error("%s: no such array for NLMS~", array_sym_name->s_name); + return((t_float *)0); + } + else if(!garray_getfloatarray(a, &n_points, &vec)) + { + error("%s: bad template for NLMS~", array_sym_name->s_name); + return((t_float *)0); + } + else if(n_points < length) + { + error("%s: bad array-size for NLMS~: %d", array_sym_name->s_name, n_points); + return((t_float *)0); + } + else + { + return(vec); + } +} + +static void sigNLMS_beta(t_sigNLMS *x, t_floatarg f) // learn rate +{ + if(f < 0.0f) + f = 0.0f; + if(f > 2.0f) + f = 2.0f; + + x->x_beta = f; +} + +static void sigNLMS_gamma(t_sigNLMS *x, t_floatarg f) // regularization factor (dither) +{ + if(f < 0.0f) + f = 0.0f; + if(f > 1.0f) + f = 1.0f; + + x->x_gamma = f; +} + + +static void sigNLMS_update(t_sigNLMS *x, t_floatarg f) // downsample learn-rate +{ + t_int i=1, u = (t_int)f; + + if(u < 0) + u = 0; + else + { + while(i <= u) // convert u for 2^N + i *= 2; // round downwards + i /= 2; + u = i; + } + x->x_update = u; +} + +/* ============== DSP ======================= */ + +static t_int *sigNLMS_perform_zero(t_int *w) +{ + t_sigNLMS *x = (t_sigNLMS *)(w[1]); + t_int n = (t_int)(w[2]); + + t_float **io = x->x_io_ptr_beg; + t_float *out; + t_int i, j; + + for(j=0; j<2; j++)/* output-vector-row */ + { + out = io[j+2]; + for(i=0; ix_n_order; /* number of filter-order */ + t_int rw_index = x->x_rw_index; + t_float *in = x->x_io_ptr_beg[0];// first sig in + t_float *desired_in = x->x_io_ptr_beg[1], din;// second sig in + t_float *filt_out = x->x_io_ptr_beg[2];// first sig out + t_float *err_out = x->x_io_ptr_beg[3], eout;// second sig out + t_float *write_in_hist1 = x->x_in_hist; + t_float *write_in_hist2 = write_in_hist1+n_order; + t_float *read_in_hist = write_in_hist2; + t_float *w_filt_coeff = x->x_w_array_mem_beg; + t_float my, my_err, sum; + t_float beta = x->x_beta; + t_float gamma = x->x_gamma; + t_int i, j, update_counter; + t_int update = x->x_update; + t_int ord8=n_order&0xfffffff8; + t_int ord_residual=n_order&0x7; + + if(!w_filt_coeff) + goto sigNLMSperfzero;// this is quick&dirty Musil/Miller style + + for(i=0, update_counter=0; ix_w_array_mem_beg; // Musil's special convolution buffer struct + read_in_hist = &write_in_hist2[rw_index]; + for(j=0; j= update) + { + update_counter = 0; + + sum = 0.0f;// calculate energy for last n-order samples in filter + read_in_hist = &write_in_hist2[rw_index]; + for(j=0; jx_w_array_mem_beg; // coefficient constraints + read_in_hist = &write_in_hist2[rw_index]; + for(j=0; j= n_order) + rw_index -= n_order; + } + + x->x_rw_index = rw_index; // back to start + + return(w+3); + +sigNLMSperfzero: + + while(n--) + { + *filt_out++ = 0.0f; + *err_out++ = 0.0f; + } + return(w+3); +} + +static void sigNLMS_dsp(t_sigNLMS *x, t_signal **sp) +{ + t_int i, n = sp[0]->s_n; + + for(i=0; i<4; i++) // store io_vec + x->x_io_ptr_beg[i] = sp[i]->s_vec; + + x->x_w_array_mem_beg = sigNLMS_check_array(x->x_w_array_sym_name, x->x_n_order); + + if(!x->x_w_array_mem_beg) + dsp_add(sigNLMS_perform_zero, 2, x, n); + else + dsp_add(sigNLMS_perform, 2, x, n); +} + + +/* setup/setdown things */ + +static void sigNLMS_free(t_sigNLMS *x) +{ + freebytes(x->x_in_hist, 2*x->x_n_order*sizeof(t_float)); +} + +static void *sigNLMS_new(t_symbol *s, t_int argc, t_atom *argv) +{ + t_sigNLMS *x = (t_sigNLMS *)pd_new(sigNLMS_class); + t_int i, n_order=39; + t_symbol *w_name; + t_float beta=0.1f; + t_float gamma=0.00001f; + + if((argc >= 4) && + IS_A_FLOAT(argv,0) && //IS_A_FLOAT/SYMBOL from iemlib.h + IS_A_FLOAT(argv,1) && + IS_A_FLOAT(argv,2) && + IS_A_SYMBOL(argv,3)) + { + n_order = (t_int)atom_getintarg(0, argc, argv); + beta = (t_float)atom_getfloatarg(1, argc, argv); + gamma = (t_float)atom_getfloatarg(2, argc, argv); + w_name = (t_symbol *)atom_getsymbolarg(3, argc, argv); + + if(beta < 0.0f) + beta = 0.0f; + if(beta > 2.0f) + beta = 2.0f; + + if(gamma < 0.0f) + gamma = 0.0f; + if(gamma > 1.0f) + gamma = 1.0f; + + if(n_order < 2) + n_order = 2; + if(n_order > 11111) + n_order = 11111; + + inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal); + outlet_new(&x->x_obj, &s_signal); + outlet_new(&x->x_obj, &s_signal); + + x->x_msi = 0; + x->x_n_order = n_order; + x->x_update = 0; + x->x_beta = beta; + x->x_gamma = gamma; + // 2 times in and one time desired_in memory allocation (history) + x->x_in_hist = (t_float *)getbytes(2*x->x_n_order*sizeof(t_float)); + + // table-symbols will be linked to their memory in future (dsp_routine) + x->x_w_array_sym_name = gensym(w_name->s_name); + x->x_w_array_mem_beg = (t_float *)0; + + return(x); + } + else + { + post("NLMS~-ERROR: need 3 float- + 1 symbol-arguments:"); + post(" order_of_filter + learnrate_beta + security_value + array_name_taps"); + return(0); + } +} + +void sigNLMS_setup(void) +{ + sigNLMS_class = class_new(gensym("NLMS~"), (t_newmethod)sigNLMS_new, (t_method)sigNLMS_free, + sizeof(t_sigNLMS), 0, A_GIMME, 0); + CLASS_MAINSIGNALIN(sigNLMS_class, t_sigNLMS, x_msi); + class_addmethod(sigNLMS_class, (t_method)sigNLMS_dsp, gensym("dsp"), 0); + class_addmethod(sigNLMS_class, (t_method)sigNLMS_update, gensym("update"), A_FLOAT, 0); // method: downsampling factor of learning (multiple of 2^N) + class_addmethod(sigNLMS_class, (t_method)sigNLMS_beta, gensym("beta"), A_FLOAT, 0); //method: normalized learning rate + class_addmethod(sigNLMS_class, (t_method)sigNLMS_gamma, gensym("gamma"), A_FLOAT, 0); // method: dithering noise related to signal + class_sethelpsymbol(sigNLMS_class, gensym("iemhelp2/NLMS~")); +} diff --git a/src/sigNLMSCC.c b/src/sigNLMSCC.c new file mode 100644 index 0000000..553e595 --- /dev/null +++ b/src/sigNLMSCC.c @@ -0,0 +1,390 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +NLMSCC normalized LMS algorithm with coefficient constraints +lib iem_adaptfilt written by Markus Noisternig & Thomas Musil +noisternig_AT_iem.at; musil_AT_iem.at +(c) Institute of Electronic Music and Acoustics, Graz Austria 2005 */ + +#ifdef NT +#pragma warning( disable : 4244 ) +#pragma warning( disable : 4305 ) +#endif + + +#include "m_pd.h" +#include "iemlib.h" +#include +#include +#include + + +/* ----------------------- NLMSCC~ ------------------------------ */ +/* -- Normalized Least Mean Square (linear adaptive FIR-filter) -- */ +/* -- with Coefficient Constraint +/* -- first input: reference signal -- */ +/* -- second input: desired signal -- */ +/* -- -- */ +/* for further information on adaptive filter design we refer to */ +/* [1] Haykin, "Adaptive Filter Theory", 4th ed, Prentice Hall */ +/* [2] Benesty, "Adaptive Signal Processing", Springer */ +/* */ + + +typedef struct sigNLMSCC +{ + t_object x_obj; + t_symbol *x_w_array_sym_name; + t_float *x_w_array_mem_beg; + t_symbol *x_wmin_array_sym_name; + t_float *x_wmin_array_mem_beg; + t_symbol *x_wmax_array_sym_name; + t_float *x_wmax_array_mem_beg; + t_float *x_io_ptr_beg[4];// memory: 2 sig-in and 2 sig-out vectors + t_float *x_in_hist;// start point double buffer for sig-in history + t_int x_rw_index;// read-write-index + t_int x_n_order;// order of filter + t_int x_update;// 2^n rounded value, downsampling of update speed + t_float x_beta;// learn rate [0 .. 2] + t_float x_gamma;// regularization + t_outlet *x_out_clipping_bang; + t_clock *x_clock; + t_float x_msi; +} t_sigNLMSCC; + +t_class *sigNLMSCC_class; + +static void sigNLMSCC_tick(t_sigNLMSCC *x) +{ + outlet_bang(x->x_out_clipping_bang); +} + +static t_float *sigNLMSCC_check_array(t_symbol *array_sym_name, t_int length) +{ + t_int n_points; + t_garray *a; + t_float *vec; + + if(!(a = (t_garray *)pd_findbyclass(array_sym_name, garray_class))) + { + error("%s: no such array for NLMSCC~", array_sym_name->s_name); + return((t_float *)0); + } + else if(!garray_getfloatarray(a, &n_points, &vec)) + { + error("%s: bad template for NLMSCC~", array_sym_name->s_name); + return((t_float *)0); + } + else if(n_points < length) + { + error("%s: bad array-size for NLMSCC~: %d", array_sym_name->s_name, n_points); + return((t_float *)0); + } + else + { + return(vec); + } +} + +static void sigNLMSCC_beta(t_sigNLMSCC *x, t_floatarg f) // learn rate +{ + if(f < 0.0f) + f = 0.0f; + if(f > 2.0f) + f = 2.0f; + + x->x_beta = f; +} + +static void sigNLMSCC_gamma(t_sigNLMSCC *x, t_floatarg f) // regularization factor (dither) +{ + if(f < 0.0f) + f = 0.0f; + if(f > 1.0f) + f = 1.0f; + + x->x_gamma = f; +} + + +static void sigNLMSCC_update(t_sigNLMSCC *x, t_floatarg f) // downsample of learn-rate +{ + t_int i=1, u = (t_int)f; + + if(u < 0) + u = 0; + else + { + while(i <= u) // convert u for 2^N + i *= 2; // round downwards + i /= 2; + u = i; + } + x->x_update = u; +} + +/* ============== DSP ======================= */ + +static t_int *sigNLMSCC_perform_zero(t_int *w) +{ + t_sigNLMSCC *x = (t_sigNLMSCC *)(w[1]); + t_int n = (t_int)(w[2]); + + t_float **io = x->x_io_ptr_beg; + t_float *out; + t_int i, j; + + for(j=0; j<2; j++)/* output-vector-row */ + { + out = io[j+2]; + for(i=0; ix_n_order; /* filter-order */ + t_int rw_index = x->x_rw_index; + t_float *in = x->x_io_ptr_beg[0];// first sig in + t_float *desired_in = x->x_io_ptr_beg[1], din;// second sig in + t_float *filt_out = x->x_io_ptr_beg[2];// first sig out + t_float *err_out = x->x_io_ptr_beg[3], eout;// second sig out + t_float *write_in_hist1 = x->x_in_hist; + t_float *write_in_hist2 = write_in_hist1+n_order; + t_float *read_in_hist = write_in_hist2; + t_float *w_filt_coeff = x->x_w_array_mem_beg; + t_float *wmin_filt_coeff = x->x_wmin_array_mem_beg; + t_float *wmax_filt_coeff = x->x_wmax_array_mem_beg; + t_float my, my_err, sum; + t_float beta = x->x_beta; + t_float gamma = x->x_gamma; + t_int i, j, update_counter; + t_int update = x->x_update; + t_int ord8=n_order&0xfffffff8; + t_int ord_residual=n_order&0x7; + t_int clipped = 0; + + if(!w_filt_coeff) + goto sigNLMSCCperfzero;// this is Musil/Miller style + if(!wmin_filt_coeff) + goto sigNLMSCCperfzero; + if(!wmax_filt_coeff) + goto sigNLMSCCperfzero;// if not constrained, perform zero + + for(i=0, update_counter=0; ix_w_array_mem_beg; // Musil's special convolution buffer struct + read_in_hist = &write_in_hist2[rw_index]; + for(j=0; j= update) + { + update_counter = 0; + + sum = 0.0f;// calculate energy for last n-order samples in filter + read_in_hist = &write_in_hist2[rw_index]; + for(j=0; jx_w_array_mem_beg; // coefficient constraints + wmin_filt_coeff = x->x_wmin_array_mem_beg; + wmax_filt_coeff = x->x_wmax_array_mem_beg; + read_in_hist = &write_in_hist2[rw_index]; + for(j=0; j wmax_filt_coeff[j]) + { + w_filt_coeff[j] = wmax_filt_coeff[j]; + clipped = 1; + } + else if(w_filt_coeff[j] < wmin_filt_coeff[j]) + { + w_filt_coeff[j] = wmin_filt_coeff[j]; + clipped = 1; + } + } + } + } + rw_index++; + if(rw_index >= n_order) + rw_index -= n_order; + } + + x->x_rw_index = rw_index; // back to start + + if(clipped) + clock_delay(x->x_clock, 0); + return(w+3); + +sigNLMSCCperfzero: + + while(n--) + { + *filt_out++ = 0.0f; + *err_out++ = 0.0f; + } + return(w+3); +} + +static void sigNLMSCC_dsp(t_sigNLMSCC *x, t_signal **sp) +{ + t_int i, n = sp[0]->s_n; + + for(i=0; i<4; i++) // store io_vec + x->x_io_ptr_beg[i] = sp[i]->s_vec; + + x->x_w_array_mem_beg = sigNLMSCC_check_array(x->x_w_array_sym_name, x->x_n_order); + x->x_wmin_array_mem_beg = sigNLMSCC_check_array(x->x_wmin_array_sym_name, x->x_n_order); + x->x_wmax_array_mem_beg = sigNLMSCC_check_array(x->x_wmax_array_sym_name, x->x_n_order); + + if(!(x->x_w_array_mem_beg && x->x_wmin_array_mem_beg && x->x_wmax_array_mem_beg)) + dsp_add(sigNLMSCC_perform_zero, 2, x, n); + else + dsp_add(sigNLMSCC_perform, 2, x, n); +} + + +/* setup/setdown things */ + +static void sigNLMSCC_free(t_sigNLMSCC *x) +{ + + freebytes(x->x_in_hist, 2*x->x_n_order*sizeof(t_float)); + + clock_free(x->x_clock); +} + +static void *sigNLMSCC_new(t_symbol *s, t_int argc, t_atom *argv) +{ + t_sigNLMSCC *x = (t_sigNLMSCC *)pd_new(sigNLMSCC_class); + t_int i, n_order=39; + t_symbol *w_name; + t_symbol *wmin_name; + t_symbol *wmax_name; + t_float beta=0.1f; + t_float gamma=0.00001f; + + if((argc >= 6) && + IS_A_FLOAT(argv,0) && //IS_A_FLOAT/SYMBOL from iemlib.h + IS_A_FLOAT(argv,1) && + IS_A_FLOAT(argv,2) && + IS_A_SYMBOL(argv,3) && + IS_A_SYMBOL(argv,4) && + IS_A_SYMBOL(argv,5)) + { + n_order = (t_int)atom_getintarg(0, argc, argv); + beta = (t_float)atom_getfloatarg(1, argc, argv); + gamma = (t_float)atom_getfloatarg(2, argc, argv); + w_name = (t_symbol *)atom_getsymbolarg(3, argc, argv); + wmin_name = (t_symbol *)atom_getsymbolarg(4, argc, argv); + wmax_name = (t_symbol *)atom_getsymbolarg(5, argc, argv); + + if(beta < 0.0f) + beta = 0.0f; + if(beta > 2.0f) + beta = 2.0f; + + if(gamma < 0.0f) + gamma = 0.0f; + if(gamma > 1.0f) + gamma = 1.0f; + + if(n_order < 2) + n_order = 2; + if(n_order > 11111) + n_order = 11111; + + inlet_new(&x->x_obj, &x->x_obj.ob_pd, &s_signal, &s_signal); + outlet_new(&x->x_obj, &s_signal); + outlet_new(&x->x_obj, &s_signal); + x->x_out_clipping_bang = outlet_new(&x->x_obj, &s_bang); + + x->x_msi = 0; + x->x_n_order = n_order; + x->x_update = 0; + x->x_beta = beta; + x->x_gamma = gamma; + // 2 times in and one time desired_in memory allocation (history) + x->x_in_hist = (t_float *)getbytes(2*x->x_n_order*sizeof(t_float)); + + // table-symbols will be linked to their memory in future (dsp_routine) + x->x_w_array_sym_name = gensym(w_name->s_name); + x->x_w_array_mem_beg = (t_float *)0; + x->x_wmin_array_sym_name = gensym(wmin_name->s_name); + x->x_wmin_array_mem_beg = (t_float *)0; + x->x_wmax_array_sym_name = gensym(wmax_name->s_name); + x->x_wmax_array_mem_beg = (t_float *)0; + + x->x_clock = clock_new(x, (t_method)sigNLMSCC_tick); + + return(x); + } + else + { + post("NLMSCC~-ERROR: need 3 float- + 3 symbol-arguments:"); + post(" order_of_filter + learnrate_beta + security_value + array_name_taps + array_name_tap_min + array_name_tap_max"); + return(0); + } +} + +void sigNLMSCC_setup(void) +{ + sigNLMSCC_class = class_new(gensym("NLMSCC~"), (t_newmethod)sigNLMSCC_new, (t_method)sigNLMSCC_free, + sizeof(t_sigNLMSCC), 0, A_GIMME, 0); + CLASS_MAINSIGNALIN(sigNLMSCC_class, t_sigNLMSCC, x_msi); + class_addmethod(sigNLMSCC_class, (t_method)sigNLMSCC_dsp, gensym("dsp"), 0); + class_addmethod(sigNLMSCC_class, (t_method)sigNLMSCC_update, gensym("update"), A_FLOAT, 0); // method: downsampling factor of learning (multiple of 2^N) + class_addmethod(sigNLMSCC_class, (t_method)sigNLMSCC_beta, gensym("beta"), A_FLOAT, 0); //method: normalized learning rate + class_addmethod(sigNLMSCC_class, (t_method)sigNLMSCC_gamma, gensym("gamma"), A_FLOAT, 0); // method: dithering noise related to signal + class_sethelpsymbol(sigNLMSCC_class, gensym("iemhelp2/NLMSCC~")); +} diff --git a/src/sign_CLNLMS.c b/src/sign_CLNLMS.c new file mode 100644 index 0000000..e1c1c0a --- /dev/null +++ b/src/sign_CLNLMS.c @@ -0,0 +1,503 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +n_CLNLMS multichannel-constrained leaky normalized LMS algorithm +lib iem_adaptfilt written by Markus Noisternig & Thomas Musil +noisternig_AT_iem.at; musil_AT_iem.at +(c) Institute of Electronic Music and Acoustics, Graz Austria 2005 */ + +#ifdef NT +#pragma warning( disable : 4244 ) +#pragma warning( disable : 4305 ) +#endif + + +#include "m_pd.h" +#include "iemlib.h" +#include +#include +#include + + +/* ----------------------- n_CLNLMS~ ------------------------------ */ +/* -- multiple Constraint LEAKY Normalized Least Mean Square (linear adaptive FIR-filter) -- */ + +//* -- first input: reference signal -- */ +/* -- second input: desired signal -- */ +/* -- -- */ + +/* for further information on adaptive filter design we refer to */ +/* [1] Haykin, "Adaptive Filter Theory", 4th ed, Prentice Hall */ +/* [2] Benesty, "Adaptive Signal Processing", Springer */ + +typedef struct sign_CLNLMS_kern +{ + t_symbol *x_w_array_sym_name; + t_float *x_w_array_mem_beg; + t_float *x_in_ptr_beg;// memory: sig-in vector + t_float *x_out_ptr_beg;// memory: sig-out vector + t_float *x_in_hist;// start point double buffer for sig-in history +} t_sign_CLNLMS_kern; + + +typedef struct sign_CLNLMS +{ + t_object x_obj; + t_sign_CLNLMS_kern *x_my_kern; + t_float *x_des_in_ptr_beg;// memory: desired-in vector + t_float *x_err_out_ptr_beg;// memory: error-out vector + t_int x_n_io;// number of in-channels and filtered out-channels + t_int x_rw_index;// read-write-index + t_int x_n_order;// filter order + t_int x_update;// rounded by 2^n, yields downsampling of learn-rate + t_float x_beta;// learn rate [0 .. 2] + t_float x_gamma;// normalization + t_float x_kappa;// constreint: treshold of energy (clipping) + t_float x_leakage;// leakage-Faktor for NLMS + t_outlet *x_out_compressing_bang; + t_clock *x_clock; + t_float x_msi; +} t_sign_CLNLMS; + +t_class *sign_CLNLMS_class; + +static void sign_CLNLMS_tick(t_sign_CLNLMS *x) +{ + outlet_bang(x->x_out_compressing_bang); +} + +static t_float *sign_CLNLMS_check_array(t_symbol *array_sym_name, t_int length) +{ + t_int n_points; + t_garray *a; + t_float *vec; + + if(!(a = (t_garray *)pd_findbyclass(array_sym_name, garray_class))) + { + error("%s: no such array for n_CLNLMS~", array_sym_name->s_name); + return((t_float *)0); + } + else if(!garray_getfloatarray(a, &n_points, &vec)) + { + error("%s: bad template for n_CLNLMS~", array_sym_name->s_name); + return((t_float *)0); + } + else if(n_points < length) + { + error("%s: bad array-size for n_CLNLMS~: %d", array_sym_name->s_name, n_points); + return((t_float *)0); + } + else + { + return(vec); + } +} + +static void sign_CLNLMS_beta(t_sign_CLNLMS *x, t_floatarg f) // learn rate +{ + if(f < 0.0f) + f = 0.0f; + if(f > 2.0f) + f = 2.0f; + + x->x_beta = f; +} + +static void sign_CLNLMS_gamma(t_sign_CLNLMS *x, t_floatarg f) // regularization (dither) +{ + if(f < 0.0f) + f = 0.0f; + if(f > 1.0f) + f = 1.0f; + + x->x_gamma = f; +} + +static void sign_CLNLMS_kappa(t_sign_CLNLMS *x, t_floatarg f) // threshold for w_coeff +{ + if(f < 0.0001f) + f = 0.0001f; + if(f > 10000.0f) + f = 10000.0f; + + x->x_kappa = f; +} + +static void sign_CLNLMS_leakage(t_sign_CLNLMS *x, t_floatarg f) // leakage of NLMS +{ + if(f < 0.0001f) + f = 0.0001f; + if(f > 1.0f) + f = 1.0f; + + x->x_leakage = f; +} + +static void sign_CLNLMS_update(t_sign_CLNLMS *x, t_floatarg f) // downsample learn rate +{ + t_int i=1, u = (t_int)f; + + if(u < 0) + u = 0; + else + { + while(i <= u) // convert u for 2^N + i *= 2; // round down + i /= 2; + u = i; + } + x->x_update = u; +} + +/* ============== DSP ======================= */ + +static t_int *sign_CLNLMS_perform_zero(t_int *w) +{ + t_sign_CLNLMS *x = (t_sign_CLNLMS *)(w[1]); + t_int n = (t_int)(w[2]); + + t_int n_io = x->x_n_io; + t_float *out; + t_int i, j; + + out = x->x_err_out_ptr_beg; + for(i=0; ix_my_kern[j].x_out_ptr_beg; + for(i=0; ix_n_order; /* number of filter-order */ + t_int rw_index2, rw_index = x->x_rw_index; + t_int n_io = x->x_n_io; + t_float *in;// first sig in + t_float din;// second sig in + t_float *filt_out;// first sig out + t_float *err_out, err_sum;// second sig out + t_float *read_in_hist; + t_float *w_filt_coeff; + t_float my, my_err, sum; + t_float beta = x->x_beta; + t_float hgamma, gamma = x->x_gamma; + t_float hkappa, kappa = x->x_kappa; + t_float hleakage, leakage = x->x_leakage; + t_int i, j, k, update_counter; + t_int update = x->x_update; + t_int ord8=n_order&0xfffffff8; + t_int ord_residual=n_order&0x7; + t_int compressed = 0; + + for(k=0; kx_my_kern[k].x_w_array_mem_beg) + goto sign_CLNLMSperfzero;// this is Musil/Miller style + } + + hgamma = gamma * gamma * (float)n_order; + //hkappa = kappa * kappa * (float)n_order; + hkappa = kappa; // kappa regards to energy value, else use line above + + for(i=0, update_counter=0; ix_my_kern[k].x_in_hist[rw_index] = x->x_my_kern[k].x_in_ptr_beg[i]; // save inputs into variabel & history + x->x_my_kern[k].x_in_hist[rw_index+n_order] = x->x_my_kern[k].x_in_ptr_beg[i]; + } + din = x->x_des_in_ptr_beg[i]; + +// begin convolution + err_sum = din; + for(k=0; kx_my_kern[k].x_w_array_mem_beg; // Musil's special convolution buffer struct + read_in_hist = &x->x_my_kern[k].x_in_hist[rw_index2]; + for(j=0; jx_my_kern[k].x_out_ptr_beg[i] = sum; + err_sum -= sum; + } + x->x_err_out_ptr_beg[i] = err_sum; +// end convolution + + if(update) // downsampling of learn rate + { + update_counter++; + if(update_counter >= update) + { + update_counter = 0; + + for(k=0; kx_my_kern[k].x_in_hist[rw_index2]; + for(j=0; jx_my_kern[k].x_w_array_mem_beg; + read_in_hist = &x->x_my_kern[k].x_in_hist[rw_index2]; + sum = 0.0f; + for(j=0; j hkappa) + { + compressed = 1; + my = sqrt(hkappa/sum); + w_filt_coeff = x->x_my_kern[k].x_w_array_mem_beg; + for(j=0; j= n_order) + rw_index -= n_order; + } + + + x->x_rw_index = rw_index; // wieder in die garage stellen + + if(compressed) + clock_delay(x->x_clock, 0); + + return(w+3); + +sign_CLNLMSperfzero: + + err_out = x->x_err_out_ptr_beg; + for(i=0; ix_my_kern[j].x_out_ptr_beg; + for(i=0; is_n; + t_int ok_w = 1; + t_int m = x->x_n_io; + + for(i=0; ix_my_kern[i].x_in_ptr_beg = sp[i]->s_vec; + x->x_des_in_ptr_beg = sp[m]->s_vec; + for(i=0; ix_my_kern[i].x_out_ptr_beg = sp[i+m+1]->s_vec; + x->x_err_out_ptr_beg = sp[2*m+1]->s_vec; + + for(i=0; ix_my_kern[i].x_w_array_mem_beg = sign_CLNLMS_check_array(x->x_my_kern[i].x_w_array_sym_name, x->x_n_order); + if(!x->x_my_kern[i].x_w_array_mem_beg) + ok_w = 0; + } + + if(!ok_w) + dsp_add(sign_CLNLMS_perform_zero, 2, x, n); + else + dsp_add(sign_CLNLMS_perform, 2, x, n); +} + + +/* setup/setdown things */ + +static void sign_CLNLMS_free(t_sign_CLNLMS *x) +{ + t_int i, n_io=x->x_n_io, n_order=x->x_n_order; + + for(i=0; ix_my_kern[i].x_in_hist, 2*x->x_n_order*sizeof(t_float)); + freebytes(x->x_my_kern, n_io*sizeof(t_sign_CLNLMS_kern)); + + clock_free(x->x_clock); +} + +static void *sign_CLNLMS_new(t_symbol *s, t_int argc, t_atom *argv) +{ + t_sign_CLNLMS *x = (t_sign_CLNLMS *)pd_new(sign_CLNLMS_class); + char buffer[400]; + t_int i, n_order=39, n_io=1; + t_symbol *w_name; + t_float beta=0.1f; + t_float gamma=0.00001f; + t_float kappa = 1.0f; + t_float leakage = 0.99f; + + if((argc >= 7) && + IS_A_FLOAT(argv,0) && //IS_A_FLOAT/SYMBOL from iemlib.h + IS_A_FLOAT(argv,1) && + IS_A_FLOAT(argv,2) && + IS_A_FLOAT(argv,3) && + IS_A_FLOAT(argv,4) && + IS_A_FLOAT(argv,5) && + IS_A_SYMBOL(argv,6)) + { + n_io = (t_int)atom_getintarg(0, argc, argv); + n_order = (t_int)atom_getintarg(1, argc, argv); + beta = (t_float)atom_getfloatarg(2, argc, argv); + gamma = (t_float)atom_getfloatarg(3, argc, argv); + kappa = (t_float)atom_getfloatarg(4, argc, argv); + leakage = (t_float)atom_getfloatarg(5, argc, argv); + w_name = (t_symbol *)atom_getsymbolarg(6, argc, argv); + + if(beta < 0.0f) + beta = 0.0f; + if(beta > 2.0f) + beta = 2.0f; + + if(gamma < 0.0f) + gamma = 0.0f; + if(gamma > 1.0f) + gamma = 1.0f; + + if(kappa < 0.0001f) + kappa = 0.0001f; + if(kappa > 10000.0f) + kappa = 10000.0f; + + if(leakage < 0.0001f) + leakage = 0.0001f; + if(leakage > 1.0f) + leakage = 1.0f; + + if(n_order < 2) + n_order = 2; + if(n_order > 11111) + n_order = 11111; + + if(n_io < 1) + n_io = 1; + if(n_io > 60) + n_io = 60; + + for(i=0; ix_obj, &x->x_obj.ob_pd, &s_signal, &s_signal); + for(i=0; i<=n_io; i++) + outlet_new(&x->x_obj, &s_signal); + + x->x_out_compressing_bang = outlet_new(&x->x_obj, &s_bang); + + x->x_msi = 0; + x->x_n_io = n_io; + x->x_n_order = n_order; + x->x_update = 0; + x->x_beta = beta; + x->x_gamma = gamma; + x->x_kappa = kappa; + x->x_leakage = leakage; + x->x_my_kern = (t_sign_CLNLMS_kern *)getbytes(x->x_n_io*sizeof(t_sign_CLNLMS_kern)); + for(i=0; is_name); + x->x_my_kern[i].x_w_array_sym_name = gensym(buffer); + x->x_my_kern[i].x_w_array_mem_beg = (t_float *)0; + x->x_my_kern[i].x_in_hist = (t_float *)getbytes(2*x->x_n_order*sizeof(t_float)); + } + x->x_clock = clock_new(x, (t_method)sign_CLNLMS_tick); + + return(x); + } + else + { + post("n_CLNLMSC~-ERROR: need 6 float- + 1 symbol-arguments:"); + post(" number_of_filters + order_of_filters + learnrate_beta + security_value_gamma + threshold_kappa + leakage_factor_lambda + array_name_taps"); + return(0); + } +} + +void sign_CLNLMS_setup(void) +{ + sign_CLNLMS_class = class_new(gensym("n_CLNLMS~"), (t_newmethod)sign_CLNLMS_new, (t_method)sign_CLNLMS_free, + sizeof(t_sign_CLNLMS), 0, A_GIMME, 0); + CLASS_MAINSIGNALIN(sign_CLNLMS_class, t_sign_CLNLMS, x_msi); + class_addmethod(sign_CLNLMS_class, (t_method)sign_CLNLMS_dsp, gensym("dsp"), 0); + class_addmethod(sign_CLNLMS_class, (t_method)sign_CLNLMS_update, gensym("update"), A_FLOAT, 0); // method: downsampling factor of learning (multiple of 2^N) + class_addmethod(sign_CLNLMS_class, (t_method)sign_CLNLMS_beta, gensym("beta"), A_FLOAT, 0); //method: normalized learning rate + class_addmethod(sign_CLNLMS_class, (t_method)sign_CLNLMS_gamma, gensym("gamma"), A_FLOAT, 0); // method: dithering noise related to signal + class_addmethod(sign_CLNLMS_class, (t_method)sign_CLNLMS_kappa, gensym("kappa"), A_FLOAT, 0); // method: threshold for compressing w_coeff + class_addmethod(sign_CLNLMS_class, (t_method)sign_CLNLMS_leakage, gensym("leakage"), A_FLOAT, 0); // method: leakage factor [0 1] for w update + class_sethelpsymbol(sign_CLNLMS_class, gensym("iemhelp2/n_CLNLMS~")); +} diff --git a/src/sign_CNLMS.c b/src/sign_CNLMS.c new file mode 100644 index 0000000..a97c1ed --- /dev/null +++ b/src/sign_CNLMS.c @@ -0,0 +1,482 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +n_CNLMS multichannel-constrained (non leaky) normalized LMS algorithm +lib iem_adaptfilt written by Markus Noisternig & Thomas Musil +noisternig_AT_iem.at; musil_AT_iem.at +(c) Institute of Electronic Music and Acoustics, Graz Austria 2005 */ + +#ifdef NT +#pragma warning( disable : 4244 ) +#pragma warning( disable : 4305 ) +#endif + + +#include "m_pd.h" +#include "iemlib.h" +#include +#include +#include + + +/* ----------------------- n_CNLMS~ ------------------------------ */ +/* -- multi-channel Constraint Normalized Least Mean Square (linear adaptive FIR-filter) -- */ + +/* -- first input: reference signal -- */ +/* -- second input: desired signal -- */ +/* -- -- */ + +/* for further information on adaptive filter design we refer to */ +/* [1] Haykin, "Adaptive Filter Theory", 4th ed, Prentice Hall */ +/* [2] Benesty, "Adaptive Signal Processing", Springer */ + +typedef struct sign_CNLMS_kern +{ + t_symbol *x_w_array_sym_name; + t_float *x_w_array_mem_beg; + t_float *x_in_ptr_beg;// memory: sig-in vector + t_float *x_out_ptr_beg;// memory: sig-out vector + t_float *x_in_hist;// start point double buffer for sig-in history +} t_sign_CNLMS_kern; + + +typedef struct sign_CNLMS +{ + t_object x_obj; + t_sign_CNLMS_kern *x_my_kern; + t_float *x_des_in_ptr_beg;// memory: desired-in vector + t_float *x_err_out_ptr_beg;// memory: error-out vector + t_int x_n_io;// number of in-channels and filtered out-channels + t_int x_rw_index;// read-write-index + t_int x_n_order;// filter order + t_int x_update;// rounded by 2^n, yields downsampling of update rate + t_float x_beta;// learn rate [0 .. 2] + t_float x_gamma;// normalization + t_float x_kappa;// constraint: threshold of energy (clipping) + t_outlet *x_out_compressing_bang; + t_clock *x_clock; + t_float x_msi; +} t_sign_CNLMS; + +t_class *sign_CNLMS_class; + +static void sign_CNLMS_tick(t_sign_CNLMS *x) +{ + outlet_bang(x->x_out_compressing_bang); +} + +static t_float *sign_CNLMS_check_array(t_symbol *array_sym_name, t_int length) +{ + t_int n_points; + t_garray *a; + t_float *vec; + + if(!(a = (t_garray *)pd_findbyclass(array_sym_name, garray_class))) + { + error("%s: no such array for n_CNLMS~", array_sym_name->s_name); + return((t_float *)0); + } + else if(!garray_getfloatarray(a, &n_points, &vec)) + { + error("%s: bad template for n_CNLMS~", array_sym_name->s_name); + return((t_float *)0); + } + else if(n_points < length) + { + error("%s: bad array-size for n_CNLMS~: %d", array_sym_name->s_name, n_points); + return((t_float *)0); + } + else + { + return(vec); + } +} + +static void sign_CNLMS_beta(t_sign_CNLMS *x, t_floatarg f) // learn rate +{ + if(f < 0.0f) + f = 0.0f; + if(f > 2.0f) + f = 2.0f; + + x->x_beta = f; +} + +static void sign_CNLMS_gamma(t_sign_CNLMS *x, t_floatarg f) // regularization (dither) +{ + if(f < 0.0f) + f = 0.0f; + if(f > 1.0f) + f = 1.0f; + + x->x_gamma = f; +} + +static void sign_CNLMS_kappa(t_sign_CNLMS *x, t_floatarg f) // threshold for w_coeff +{ + if(f < 0.0001f) + f = 0.0001f; + if(f > 10000.0f) + f = 10000.0f; + + x->x_kappa = f; +} + + +static void sign_CNLMS_update(t_sign_CNLMS *x, t_floatarg f) // downsampling of learn rate +{ + t_int i=1, u = (t_int)f; + + if(u < 0) + u = 0; + else + { + while(i <= u) // convert u for 2^N + i *= 2; // round downward + i /= 2; + u = i; + } + x->x_update = u; +} + +/* ============== DSP ======================= */ + +static t_int *sign_CNLMS_perform_zero(t_int *w) +{ + t_sign_CNLMS *x = (t_sign_CNLMS *)(w[1]); + t_int n = (t_int)(w[2]); + + t_int n_io = x->x_n_io; + t_float *out; + t_int i, j; + + out = x->x_err_out_ptr_beg; + for(i=0; ix_my_kern[j].x_out_ptr_beg; + for(i=0; ix_n_order; /* filter-order */ + t_int rw_index2, rw_index = x->x_rw_index; + t_int n_io = x->x_n_io; + t_float *in;// first sig in + t_float din;// second sig in + t_float *filt_out;// first sig out + t_float *err_out, err_sum;// second sig out + t_float *read_in_hist; + t_float *w_filt_coeff; + t_float my, my_err, sum; + t_float beta = x->x_beta; + t_float hgamma, gamma = x->x_gamma; + t_float hkappa, kappa = x->x_kappa; + t_int i, j, k, update_counter; + t_int update = x->x_update; + t_int ord8=n_order&0xfffffff8; + t_int ord_residual=n_order&0x7; + t_int compressed = 0; + + for(k=0; kx_my_kern[k].x_w_array_mem_beg) + goto sign_CNLMSperfzero;// this is Musil/Miller style + } + + hgamma = gamma * gamma * (float)n_order; + //hkappa = kappa * kappa * (float)n_order; + hkappa = kappa;// kappa regards to energy value, else use line above + + for(i=0, update_counter=0; ix_my_kern[k].x_in_hist[rw_index] = x->x_my_kern[k].x_in_ptr_beg[i]; // save inputs into variabel & history + x->x_my_kern[k].x_in_hist[rw_index+n_order] = x->x_my_kern[k].x_in_ptr_beg[i]; + } + din = x->x_des_in_ptr_beg[i]; + +// begin convolution + err_sum = din; + for(k=0; kx_my_kern[k].x_w_array_mem_beg; // Musil's special convolution buffer struct + read_in_hist = &x->x_my_kern[k].x_in_hist[rw_index2]; + for(j=0; jx_my_kern[k].x_out_ptr_beg[i] = sum; + err_sum -= sum; + } + x->x_err_out_ptr_beg[i] = err_sum; +// end convolution + + if(update) // downsampling of learn rate + { + update_counter++; + if(update_counter >= update) + { + update_counter = 0; + + for(k=0; kx_my_kern[k].x_in_hist[rw_index2]; + for(j=0; jx_my_kern[k].x_w_array_mem_beg; + read_in_hist = &x->x_my_kern[k].x_in_hist[rw_index2]; + sum = 0.0f; + for(j=0; j hkappa) + { + compressed = 1; + my = sqrt(hkappa/sum); + w_filt_coeff = x->x_my_kern[k].x_w_array_mem_beg; + for(j=0; j= n_order) + rw_index -= n_order; + } + + + x->x_rw_index = rw_index; // back to start + + if(compressed) + clock_delay(x->x_clock, 0); + + return(w+3); + +sign_CNLMSperfzero: + + err_out = x->x_err_out_ptr_beg; + for(i=0; ix_my_kern[j].x_out_ptr_beg; + for(i=0; is_n; + t_int ok_w = 1; + t_int m = x->x_n_io; + + for(i=0; ix_my_kern[i].x_in_ptr_beg = sp[i]->s_vec; + x->x_des_in_ptr_beg = sp[m]->s_vec; + for(i=0; ix_my_kern[i].x_out_ptr_beg = sp[i+m+1]->s_vec; + x->x_err_out_ptr_beg = sp[2*m+1]->s_vec; + + for(i=0; ix_my_kern[i].x_w_array_mem_beg = sign_CNLMS_check_array(x->x_my_kern[i].x_w_array_sym_name, x->x_n_order); + if(!x->x_my_kern[i].x_w_array_mem_beg) + ok_w = 0; + } + + if(!ok_w) + dsp_add(sign_CNLMS_perform_zero, 2, x, n); + else + dsp_add(sign_CNLMS_perform, 2, x, n); +} + + +/* setup/setdown things */ + +static void sign_CNLMS_free(t_sign_CNLMS *x) +{ + t_int i, n_io=x->x_n_io, n_order=x->x_n_order; + + for(i=0; ix_my_kern[i].x_in_hist, 2*x->x_n_order*sizeof(t_float)); + freebytes(x->x_my_kern, n_io*sizeof(t_sign_CNLMS_kern)); + + clock_free(x->x_clock); +} + +static void *sign_CNLMS_new(t_symbol *s, t_int argc, t_atom *argv) +{ + t_sign_CNLMS *x = (t_sign_CNLMS *)pd_new(sign_CNLMS_class); + char buffer[400]; + t_int i, n_order=39, n_io=1; + t_symbol *w_name; + t_float beta=0.1f; + t_float gamma=0.00001f; + t_float kappa = 1.0f; + + if((argc >= 6) && + IS_A_FLOAT(argv,0) && //IS_A_FLOAT/SYMBOL from iemlib.h + IS_A_FLOAT(argv,1) && + IS_A_FLOAT(argv,2) && + IS_A_FLOAT(argv,3) && + IS_A_FLOAT(argv,4) && + IS_A_SYMBOL(argv,5)) + { + n_io = (t_int)atom_getintarg(0, argc, argv); + n_order = (t_int)atom_getintarg(1, argc, argv); + beta = (t_float)atom_getfloatarg(2, argc, argv); + gamma = (t_float)atom_getfloatarg(3, argc, argv); + kappa = (t_float)atom_getfloatarg(4, argc, argv); + w_name = (t_symbol *)atom_getsymbolarg(5, argc, argv); + + if(beta < 0.0f) + beta = 0.0f; + if(beta > 2.0f) + beta = 2.0f; + + if(gamma < 0.0f) + gamma = 0.0f; + if(gamma > 1.0f) + gamma = 1.0f; + + if(kappa < 0.0001f) + kappa = 0.0001f; + if(kappa > 10000.0f) + kappa = 10000.0f; + + if(n_order < 2) + n_order = 2; + if(n_order > 11111) + n_order = 11111; + + if(n_io < 1) + n_io = 1; + if(n_io > 60) + n_io = 60; + + for(i=0; ix_obj, &x->x_obj.ob_pd, &s_signal, &s_signal); + for(i=0; i<=n_io; i++) + outlet_new(&x->x_obj, &s_signal); + + x->x_out_compressing_bang = outlet_new(&x->x_obj, &s_bang); + + x->x_msi = 0; + x->x_n_io = n_io; + x->x_n_order = n_order; + x->x_update = 0; + x->x_beta = beta; + x->x_gamma = gamma; + x->x_kappa = kappa; + x->x_my_kern = (t_sign_CNLMS_kern *)getbytes(x->x_n_io*sizeof(t_sign_CNLMS_kern)); + for(i=0; is_name); + x->x_my_kern[i].x_w_array_sym_name = gensym(buffer); + x->x_my_kern[i].x_w_array_mem_beg = (t_float *)0; + x->x_my_kern[i].x_in_hist = (t_float *)getbytes(2*x->x_n_order*sizeof(t_float)); + } + x->x_clock = clock_new(x, (t_method)sign_CNLMS_tick); + + return(x); + } + else + { + post("n_CNLMSC~-ERROR: need 5 float- + 1 symbol-arguments:"); + post(" number_of_filters + order_of_filters + learnrate_beta + security_value_gamma + threshold_kappa + array_name_taps"); + return(0); + } +} + +void sign_CNLMS_setup(void) +{ + sign_CNLMS_class = class_new(gensym("n_CNLMS~"), (t_newmethod)sign_CNLMS_new, (t_method)sign_CNLMS_free, + sizeof(t_sign_CNLMS), 0, A_GIMME, 0); + CLASS_MAINSIGNALIN(sign_CNLMS_class, t_sign_CNLMS, x_msi); + class_addmethod(sign_CNLMS_class, (t_method)sign_CNLMS_dsp, gensym("dsp"), 0); + class_addmethod(sign_CNLMS_class, (t_method)sign_CNLMS_update, gensym("update"), A_FLOAT, 0); // method: downsampling factor of learning (multiple of 2^N) + class_addmethod(sign_CNLMS_class, (t_method)sign_CNLMS_beta, gensym("beta"), A_FLOAT, 0); //method: normalized learning rate + class_addmethod(sign_CNLMS_class, (t_method)sign_CNLMS_gamma, gensym("gamma"), A_FLOAT, 0); // method: dithering noise related to signal + class_addmethod(sign_CNLMS_class, (t_method)sign_CNLMS_kappa, gensym("kappa"), A_FLOAT, 0); // method: threshold for compressing w_coeff + class_sethelpsymbol(sign_CNLMS_class, gensym("iemhelp2/n_CNLMS~")); +} -- cgit v1.2.1