initial ci of NLMS object without feedback subtraction

svn path=/trunk/externals/iem/iem_adaptfilt/; revision=14738

2 files changed, 545 insertions, 0 deletions

diff --git a/help/NLMSerr_in~-help.pd b/help/NLMSerr_in~-help.pd
new file mode 100644
index 0000000..328a888
--- /dev/null
+++ b/help/NLMSerr_in~-help.pd
@@ -0,0 +1,214 @@
+#N canvas 378 66 925 637 10;
+#N canvas 0 22 450 300 (subpatch) 0;
+#X array W 40 float 0;
+#X coords 0 1 39 -1 200 140 1;
+#X restore 545 340 graph;
+#X msg 25 245 update \$1;
+#X msg 102 255 beta \$1;
+#X obj 272 24 tgl 15 0 empty empty empty 0 -6 0 8 -225280 -1 -1 0 1
+;
+#X obj 272 45 dsp;
+#X floatatom 272 91 5 0 0 0 - - -;
+#X floatatom 285 70 5 0 0 0 - - -;
+#X obj 276 563 bng 15 150 20 0 empty empty empty 0 -6 0 8 -262144 -1
+-1;
+#X text 453 230 2.arg: <float> learn-rate = beta;
+#X obj 53 275 noise~;
+#X obj 25 58 vradio 15 1 0 8 empty empty empty 0 -6 0 8 -225280 -1
+-1 0;
+#N canvas 165 115 464 314 (subpatch) 0;
+#X obj 76 61 inlet;
+#X msg 32 163 0;
+#X msg 63 159 1;
+#X msg 97 158 2;
+#X msg 132 156 4;
+#X msg 159 157 8;
+#X msg 191 162 16;
+#X msg 219 164 32;
+#X msg 248 165 64;
+#X obj 76 84 sel 0 1 2 3 4 5 6 7;
+#X obj 32 217 outlet;
+#X connect 0 0 9 0;
+#X connect 1 0 10 0;
+#X connect 2 0 10 0;
+#X connect 3 0 10 0;
+#X connect 4 0 10 0;
+#X connect 5 0 10 0;
+#X connect 6 0 10 0;
+#X connect 7 0 10 0;
+#X connect 8 0 10 0;
+#X connect 9 0 1 0;
+#X connect 9 1 2 0;
+#X connect 9 2 3 0;
+#X connect 9 3 4 0;
+#X connect 9 4 5 0;
+#X connect 9 5 6 0;
+#X connect 9 6 7 0;
+#X connect 9 7 8 0;
+#X restore 25 205 pd;
+#X text 34 37 internal downsampling of update;
+#X msg 96 337 gamma \$1;
+#X text 75 363 input signal;
+#X text 190 362 desired signal;
+#N canvas 0 22 450 300 (subpatch) 0;
+#X array IR 40 float 0;
+#X coords 0 1 39 -1 200 140 1;
+#X restore 367 11 graph;
+#X obj 54 540 unsig~;
+#X floatatom 54 566 9 0 0 0 - - -;
+#X obj 174 539 unsig~;
+#X floatatom 174 565 9 0 0 0 - - -;
+#X text 26 363 x(n);
+#X text 243 377 d(n);
+#X text 29 508 y(n) = W * x(n);
+#X obj 276 291 FIR~ IR 32;
+#X obj 618 73 loadbang;
+#X text 503 220 (array-sizes have to be >= then FIR_size);
+#X text 453 210 1.arg: <float> number of order of FIR-filter;
+#X text 42 71 update every sample;
+#X text 42 56 stop \, no update;
+#X text 42 86 update every 2nd sample;
+#X text 42 101 update every 4th sample;
+#X text 42 116 update every 8th sample;
+#X text 42 131 update every 16th sample;
+#X text 42 146 update every 32nd sample;
+#X text 42 161 update every 64th sample;
+#N canvas 0 22 474 324 (subpatch) 0;
+#X obj 48 46 inlet;
+#X obj 205 47 inlet;
+#X msg 205 71 \; IR const 0;
+#X msg 48 120 \; IR 0 0 0 0 0.314287 0.8 0.75 0 0 0 0 -0.7 -0.65 0
+0 0 0.157143 0.128572 0 0 -0.128572 -0.1 0 0 0 0 0 0 0 0 0 0 0 0 0
+0 0 0 0 0 0;
+#X obj 92 84 loadbang;
+#X connect 0 0 3 0;
+#X connect 1 0 2 0;
+#X connect 4 0 3 0;
+#X restore 619 48 pd;
+#X obj 632 30 bng 15 250 50 0 empty empty empty 0 -6 0 8 -262144 -1
+-1;
+#X text 649 27 clear;
+#X obj 619 11 bng 15 250 50 0 empty empty empty 0 -6 0 8 -262144 -1
+-1;
+#X text 636 8 load;
+#X text 644 50 IR;
+#X text 4 495 filtered output signal;
+#X text 178 495 error signal;
+#X text 152 507 e(n) = d(n) - W * x(n);
+#X text 293 557 constrain;
+#X text 293 567 coefficients;
+#X msg 276 522 0;
+#X obj 276 542 speedlim 100;
+#X obj 83 512 cnv 8 1 1 empty empty * 0 7 0 14 -262144 -1 0;
+#X obj 247 511 cnv 8 1 1 empty empty * 0 7 0 14 -262144 -1 0;
+#N canvas 0 22 470 320 (subpatch) 0;
+#X obj 137 73 tgl 15 0 empty empty empty 0 -6 0 8 -262144 -1 -1 1 1
+;
+#X msg 137 115 39;
+#X msg 137 156 \$1 39;
+#X msg 136 53 1;
+#X obj 137 92 metro 200;
+#X obj 136 24 inlet;
+#X obj 137 135 tabread W;
+#X obj 137 180 tabwrite W;
+#X connect 0 0 4 0;
+#X connect 1 0 6 0;
+#X connect 2 0 7 0;
+#X connect 3 0 0 0;
+#X connect 4 0 1 0;
+#X connect 5 0 3 0;
+#X connect 6 0 2 0;
+#X restore 618 94 pd;
+#X text 643 100 update of W;
+#X text 642 89 graphical;
+#X text 453 250 4.arg: <symbol> table-name of W;
+#X text 453 240 3.arg: <float> minimum input value gamma;
+#X text 132 317 minimum input value;
+#X msg 102 218 0.1;
+#X msg 96 299 1e-05;
+#X text 155 256 beta [0 .. 2];
+#X text 152 336 gamma [0 .. 1];
+#X obj 455 187 cnv 15 68 17 empty empty empty 20 12 0 14 -225280 -66577
+0;
+#N canvas 0 22 499 295 FORMULAS 0;
+#X obj 167 52 cnv 15 150 40 empty empty empty 20 12 0 14 -225280 -66577
+0;
+#X obj 167 123 cnv 15 270 50 empty empty empty 20 12 0 14 -225280 -66577
+0;
+#X obj 167 205 cnv 15 260 30 empty empty empty 20 12 0 14 -225280 -66577
+0;
+#X text 280 129 beta;
+#X text 231 146 >;
+#X text 232 144 _;
+#X text 232 138 _;
+#X text 283 141 2;
+#X text 346 141 2;
+#X text 311 146 gamma * blocksize);
+#X text 300 146 +;
+#X text 243 146 x[n-i];
+#X text 174 135 my(n) =;
+#X text 223 130 _________________________________;
+#X text 16 135 normalized learn rate:;
+#X text 173 56 y(n) = W * x(n);
+#X obj 237 54 cnv 11 1 1 empty empty * 0 9 0 14 -225280 -1 0;
+#X text 173 72 e(n) = d(n) - W * x(n);
+#X obj 286 71 cnv 11 1 1 empty empty * 0 9 0 14 -225280 -1 0;
+#X text 119 73 error:;
+#X text 116 55 output:;
+#X text 26 210 coefficient iteration:;
+#X text 188 211 W(k+1 \, i) = W(k \, i) + my(n)* e(n)* x(n);
+#X restore 455 187 pd FORMULAS;
+#X obj 102 238 cnv 14 31 14 empty empty empty 20 12 0 14 -225280 -66577
+0;
+#X floatatom 102 238 5 0 2 0 - - -;
+#X text 138 237 learn-rate;
+#X obj 96 319 cnv 14 31 14 empty empty empty 20 12 0 14 -225280 -66577
+0;
+#X floatatom 96 319 5 0 2 0 - - -;
+#X floatatom 25 226 5 0 0 0 - - -;
+#X text 455 169 Normalized Least Mean Square (linear adaptive FIR-filter)
+;
+#X text 451 268 (C) 2005 \, m.noisternig & t.musil \, IEM \, Graz \,
+Austria;
+#X text 479 281 [noisternig \, musil]_AT_iem.at;
+#X obj 53 450 NLMSerr_in~ 32 0.1 1e-05 W;
+#X obj 20 390 delwrite~ causal_del 2;
+#X msg 756 387 64 44.1;
+#X obj 760 415 /;
+#X obj 760 442 print;
+#X obj 20 411 delread~ causal_del 1.45126;
+#X obj 226 475 delwrite~ feedback_del 2;
+#X obj 351 358 delread~ feedback_del 0;
+#X obj 248 415 -~;
+#X text 249 437 err;
+#X text 351 376 filt out;
+#X connect 1 0 72 0;
+#X connect 2 0 72 0;
+#X connect 3 0 4 0;
+#X connect 4 0 5 0;
+#X connect 4 1 6 0;
+#X connect 9 0 24 0;
+#X connect 9 0 73 0;
+#X connect 10 0 11 0;
+#X connect 11 0 68 0;
+#X connect 13 0 72 0;
+#X connect 17 0 18 0;
+#X connect 19 0 20 0;
+#X connect 24 0 80 0;
+#X connect 25 0 51 0;
+#X connect 37 0 36 1;
+#X connect 39 0 36 0;
+#X connect 47 0 48 0;
+#X connect 48 0 7 0;
+#X connect 57 0 64 0;
+#X connect 58 0 67 0;
+#X connect 64 0 2 0;
+#X connect 67 0 13 0;
+#X connect 68 0 1 0;
+#X connect 72 0 17 0;
+#X connect 72 0 78 0;
+#X connect 74 0 75 0;
+#X connect 75 0 76 0;
+#X connect 77 0 72 0;
+#X connect 79 0 80 1;
+#X connect 80 0 72 1;
diff --git a/src/NLMSerr_in~.c b/src/NLMSerr_in~.c
new file mode 100644
index 0000000..a94e6ad
--- /dev/null
+++ b/src/NLMSerr_in~.c
@@ -0,0 +1,331 @@
+/* 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 <math.h>
+#include <stdio.h>
+#include <string.h>
+
+
+/* ----------------------- NLMSerr_in~ ------------------------------ */
+/* -- Normalized Least Mean Square (linear adaptive FIR-filter) -- */
+/* -- first input:  reference signal -- */
+/* -- second input: desired signal -- */
+/* -- the difference to NLMS~ is: we have only one ERROR input instead of desired in minus filter out  -- */
+/* -- that means there is no feedback  -- */
+
+/* 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 NLMSerr_in_tilde
+{
+    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_NLMSerr_in_tilde;
+
+t_class *NLMSerr_in_tilde_class;
+
+static t_float *NLMSerr_in_tilde_check_array(t_symbol *array_sym_name, t_int length)
+{
+    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 NLMSerr_in~", array_sym_name->s_name);
+        return((t_float *)0);
+    }
+    else if(!garray_getfloatarray(a, &n_points, &vec))
+    {
+        error("%s: bad template for NLMSerr_in~", array_sym_name->s_name);
+        return((t_float *)0);
+    }
+    else if(n_points < length)
+    {
+        error("%s: bad array-size for NLMSerr_in~: %d", array_sym_name->s_name, n_points);
+        return((t_float *)0);
+    }
+    else
+    {
+        return(vec);
+    }
+}
+
+static void NLMSerr_in_tilde_beta(t_NLMSerr_in_tilde *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 NLMSerr_in_tilde_gamma(t_NLMSerr_in_tilde *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 NLMSerr_in_tilde_update(t_NLMSerr_in_tilde *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 *NLMSerr_in_tilde_perform_zero(t_int *w)
+{
+  t_NLMSerr_in_tilde *x = (t_NLMSerr_in_tilde *)(w[1]);
+  t_int n = (t_int)(w[2]);
+  
+  t_float **io = x->x_io_ptr_beg;
+  t_float *out;
+  t_int i;
+  
+  
+  out = io[2];
+  for(i=0; i<n; i++)
+  {
+    *out++ = 0.0f;
+  }
+  return (w+3);
+}
+
+static t_int *NLMSerr_in_tilde_perform(t_int *w)
+{
+    t_NLMSerr_in_tilde *x = (t_NLMSerr_in_tilde *)(w[1]);
+    t_int n = (t_int)(w[2]);
+    t_int n_order = x->x_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 *err_in = x->x_io_ptr_beg[1], errin;// second sig in
+    t_float *filt_out = x->x_io_ptr_beg[2];// first 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 NLMSerr_in_tildeperfzero;// this is quick&dirty Musil/Miller style
+    
+    for(i=0, update_counter=0; i<n; i++)// store history and convolve
+    {
+        write_in_hist1[rw_index] = in[i]; // save inputs to variable & history
+        write_in_hist2[rw_index] = in[i];
+        errin = err_in[i];
+        
+		// begin convolution
+        sum = 0.0f;
+        w_filt_coeff = x->x_w_array_mem_beg; // Musil's special convolution buffer struct
+        read_in_hist = &write_in_hist2[rw_index];
+        for(j=0; j<ord8; j+=8)	// loop unroll 8 taps
+        {
+            sum += w_filt_coeff[0] * read_in_hist[0];
+            sum += w_filt_coeff[1] * read_in_hist[-1];
+            sum += w_filt_coeff[2] * read_in_hist[-2];
+            sum += w_filt_coeff[3] * read_in_hist[-3];
+            sum += w_filt_coeff[4] * read_in_hist[-4];
+            sum += w_filt_coeff[5] * read_in_hist[-5];
+            sum += w_filt_coeff[6] * read_in_hist[-6];
+            sum += w_filt_coeff[7] * read_in_hist[-7];
+            w_filt_coeff += 8;
+            read_in_hist -= 8;
+        }
+        for(j=0; j<ord_residual; j++)	// for filter order < 2^N
+            sum += w_filt_coeff[j] * read_in_hist[-j];
+        
+        filt_out[i] = sum;
+        
+        
+        if(update)	// downsampling for learn rate
+        {
+            update_counter++;
+            if(update_counter >= 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; j<ord8; j+=8)	// unrolling quadrature calc
+                {
+                    sum += read_in_hist[0] * read_in_hist[0];
+                    sum += read_in_hist[-1] * read_in_hist[-1];
+                    sum += read_in_hist[-2] * read_in_hist[-2];
+                    sum += read_in_hist[-3] * read_in_hist[-3];
+                    sum += read_in_hist[-4] * read_in_hist[-4];
+                    sum += read_in_hist[-5] * read_in_hist[-5];
+                    sum += read_in_hist[-6] * read_in_hist[-6];
+                    sum += read_in_hist[-7] * read_in_hist[-7];
+                    read_in_hist -= 8;
+                }
+                for(j=0; j<ord_residual; j++)	// residual
+                    sum += read_in_hist[-j] * read_in_hist[-j]; // [-j] only valid for Musil's double buffer structure
+                sum += gamma * gamma * (float)n_order; // convert gamma corresponding to filter order
+                my = beta / sum;// calculate mue
+                
+                
+                my_err = my * errin;
+                w_filt_coeff = x->x_w_array_mem_beg; // coefficient constraints
+                read_in_hist = &write_in_hist2[rw_index];
+                for(j=0; j<n_order; j++) // without unroll
+                    w_filt_coeff[j] += read_in_hist[-j] * my_err;
+            }
+        }
+        rw_index++;
+        if(rw_index >= n_order)
+            rw_index -= n_order;
+    }
+
+    x->x_rw_index = rw_index; // back to start
+
+    return(w+3);
+    
+NLMSerr_in_tildeperfzero:
+    
+    while(n--)
+    {
+        *filt_out++ = 0.0f;
+    }
+    return(w+3);
+}
+
+static void NLMSerr_in_tilde_dsp(t_NLMSerr_in_tilde *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 = NLMSerr_in_tilde_check_array(x->x_w_array_sym_name, x->x_n_order);
+
+    if(!x->x_w_array_mem_beg)
+        dsp_add(NLMSerr_in_tilde_perform_zero, 2, x, n);
+    else
+        dsp_add(NLMSerr_in_tilde_perform, 2, x, n);
+}
+
+
+/* setup/setdown things */
+
+static void NLMSerr_in_tilde_free(t_NLMSerr_in_tilde *x)
+{
+    freebytes(x->x_in_hist, 2*x->x_n_order*sizeof(t_float));
+}
+
+static void *NLMSerr_in_tilde_new(t_symbol *s, t_int argc, t_atom *argv)
+{
+    t_NLMSerr_in_tilde *x = (t_NLMSerr_in_tilde *)pd_new(NLMSerr_in_tilde_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);
+        
+        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 err_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("NLMSerr_in~-ERROR: need 3 float- + 1 symbol-arguments:");
+        post("  order_of_filter + learnrate_beta + security_value + array_name_taps");
+        return(0);
+    }
+}
+
+void NLMSerr_in_tilde_setup(void)
+{
+    NLMSerr_in_tilde_class = class_new(gensym("NLMSerr_in~"), (t_newmethod)NLMSerr_in_tilde_new, (t_method)NLMSerr_in_tilde_free,
+        sizeof(t_NLMSerr_in_tilde), 0, A_GIMME, 0);
+    CLASS_MAINSIGNALIN(NLMSerr_in_tilde_class, t_NLMSerr_in_tilde, x_msi);
+    class_addmethod(NLMSerr_in_tilde_class, (t_method)NLMSerr_in_tilde_dsp, gensym("dsp"), 0);
+    class_addmethod(NLMSerr_in_tilde_class, (t_method)NLMSerr_in_tilde_update, gensym("update"), A_FLOAT, 0); // method: downsampling factor of learning (multiple of 2^N)
+    class_addmethod(NLMSerr_in_tilde_class, (t_method)NLMSerr_in_tilde_beta, gensym("beta"), A_FLOAT, 0); //method: normalized learning rate
+    class_addmethod(NLMSerr_in_tilde_class, (t_method)NLMSerr_in_tilde_gamma, gensym("gamma"), A_FLOAT, 0);   // method: dithering noise related to signal
+}