jDttSP/lmadf.c

   1 /* lmadf.c
   2
   3 This file is part of a program that implements a Software-Defined Radio.
   4
   5 Copyright (C) 2004 by Frank Brickle, AB2KT and Bob McGwier, N4HY
   6
   7 This program is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License as published by
   9 the Free Software Foundation; either version 2 of the License, or
  10 (at your option) any later version.
  11
  12 This program is distributed in the hope that it will be useful,
  13 but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with this program; if not, write to the Free Software
  19 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  20
  21 The authors can be reached by email at
  22
  23 ab2kt@arrl.net
  24 or
  25 rwmcgwier@comcast.net
  26
  27 or by paper mail at
  28
  29 The DTTS Microwave Society
  30 6 Kathleen Place
  31 Bridgewater, NJ 08807
  32 */
  33
  34 #include <lmadf.h>
  35
  36 #ifdef REALLMS
  37
  38 LMSR
  39 new_lmsr(CXB signal,
  40          int delay,
  41          REAL adaptation_rate,
  42          REAL leakage,
  43          int adaptive_filter_size,
  44          int filter_type) {
  45   LMSR lms = (LMSR) safealloc(1, sizeof(_lmsstate), "new_lmsr state");
  46
  47   lms->signal = signal;
  48   CXBsize(lms->signal);
  49   lms->delay = delay;
  50   lms->size = 512;
  51   lms->mask = lms->size - 1;
  52   lms->delay_line = newvec_REAL(lms->size, "lmsr delay");
  53   lms->adaptation_rate = adaptation_rate;
  54   lms->leakage = leakage;
  55   lms->adaptive_filter_size = adaptive_filter_size;
  56   lms->adaptive_filter = newvec_REAL(128, "lmsr filter");
  57   lms->filter_type = filter_type;
  58   lms->delay_line_ptr = 0;
  59
  60   return lms;
  61 }
  62
  63 void
  64 del_lmsr(LMSR lms) {
  65   if (lms) {
  66     delvec_REAL(lms->delay_line);
  67     delvec_REAL(lms->adaptive_filter);
  68     safefree((char *) lms);
  69   }
  70 }
  71
  72 // just to make the algorithm itself a little clearer,
  73 // get the admin stuff out of the way
  74
  75 #define ssiz (lms->signal_size)
  76 #define asiz (lms->adaptive_filter_size)
  77 #define dptr (lms->delay_line_ptr)
  78 #define rate (lms->adaptation_rate)
  79 #define leak (lms->leakage)
  80
  81 #define ssig(n) (CXBreal(lms->signal,(n)))
  82
  83 #define dlay(n) (lms->delay_line[(n)])
  84
  85 #define afil(n) (lms->adaptive_filter[(n)])
  86 #define wrap(n) (((n) + (lms->delay) + (lms->delay_line_ptr)) & (lms->mask))
  87 #define bump(n) (((n) + (lms->mask)) & (lms->mask))
  88
  89 static void
  90 lmsr_adapt_i(LMSR lms) {
  91   int i, j, k;
  92   REAL sum_sq, scl1, scl2;
  93   REAL accum, error;
  94
  95   scl1 = 1.0 - rate * leak;
  96
  97   for (i = 0; i < ssiz; i++) {
  98
  99     dlay(dptr) = ssig(i);
 100     accum = 0.0;
 101     sum_sq = 0.0;
 102
 103     for (j = 0; j < asiz; j++) {
 104       k = wrap(j);
 105       sum_sq += sqr(dlay(k));
 106       accum += afil(j) * dlay(k);
 107     }
 108
 109     error = ssig(i) - accum;
 110     ssig(i) = error;
 111
 112     scl2 = rate / (sum_sq + 1e-10);
 113     error *= scl2;
 114     for (j = 0; j < asiz; j++) {
 115       k = wrap(j);
 116       afil(j) = afil(j) * scl1 + error * dlay(k);
 117     }
 118
 119     dptr = bump(dptr);
 120   }
 121 }
 122
 123 static void
 124 lmsr_adapt_n(LMSR lms) {
 125   int i, j, k;
 126   REAL sum_sq, scl1, scl2;
 127   REAL accum, error;
 128
 129   scl1 = 1.0 - rate * leak;
 130
 131   for (i = 0; i < ssiz; i++) {
 132
 133     dlay(dptr) = ssig(i);
 134     accum = 0.0;
 135     sum_sq = 0.0;
 136
 137     for (j = 0; j < asiz; j++) {
 138       k = wrap(j);
 139       sum_sq += sqr(dlay(k));
 140       accum += afil(j) * dlay(k);
 141     }
 142
 143     error = ssig(i) - accum;
 144     ssig(i) = accum;
 145
 146     scl2 = rate / (sum_sq + 1e-10);
 147     error *= scl2;
 148     for (j = 0; j < asiz; j++) {
 149       k = wrap(j);
 150       afil(j) = afil(j) * scl1 + error * dlay(k);
 151     }
 152
 153     dptr = bump(dptr);
 154   }
 155 }
 156
 157 #else
 158
 159 LMSR
 160 new_lmsr(CXB signal,
 161          int delay,
 162          REAL adaptation_rate,
 163          REAL leakage,
 164          int adaptive_filter_size,
 165          int filter_type) {
 166   LMSR lms = (LMSR) safealloc(1, sizeof(_lmsstate), "new_lmsr state");
 167
 168   lms->signal = signal;
 169   CXBsize(lms->signal);
 170   lms->delay = delay;
 171   lms->size = 512;
 172   lms->mask = lms->size - 1;
 173   lms->delay_line = newvec_COMPLEX(lms->size, "lmsr delay");
 174   lms->adaptation_rate = adaptation_rate;
 175   lms->leakage = leakage;
 176   lms->adaptive_filter_size = adaptive_filter_size;
 177   lms->adaptive_filter = newvec_COMPLEX(128, "lmsr filter");
 178   lms->filter_type = filter_type;
 179   lms->delay_line_ptr = 0;
 180
 181   return lms;
 182 }
 183
 184 void
 185 del_lmsr(LMSR lms) {
 186   if (lms) {
 187     delvec_COMPLEX(lms->delay_line);
 188     delvec_COMPLEX(lms->adaptive_filter);
 189     safefree((char *) lms);
 190   }
 191 }
 192
 193 // just to make the algorithm itself a little clearer,
 194 // get the admin stuff out of the way
 195
 196 #define ssiz (lms->signal_size)
 197 #define asiz (lms->adaptive_filter_size)
 198 #define dptr (lms->delay_line_ptr)
 199 #define rate (lms->adaptation_rate)
 200 #define leak (lms->leakage)
 201
 202 #define ssig(n) (CXBdata(lms->signal,(n)))
 203
 204 #define dlay(n) (lms->delay_line[(n)])
 205
 206 #define afil(n) (lms->adaptive_filter[(n)])
 207 #define wrap(n) (((n) + (lms->delay) + (lms->delay_line_ptr)) & (lms->mask))
 208 #define bump(n) (((n) + (lms->mask)) & (lms->mask))
 209
 210 static void
 211 lmsr_adapt_i(LMSR lms) {
 212   int i, j, k;
 213   REAL sum_sq, scl1, scl2;
 214   COMPLEX accum, error;
 215   scl1 = 1.0 - rate * leak;
 216   for (i = 0; i < ssiz; i++) {
 217
 218     dlay(dptr) = ssig(i);
 219     accum = cxzero;
 220     sum_sq = 0.0;
 221
 222     for (j = 0; j < asiz; j++) {
 223       k = wrap(j);
 224       sum_sq += Csqrmag(dlay(k));
 225       accum = Cadd(accum, Cmul(Conjg(afil(j)), dlay(k)));
 226     }
 227
 228     error = Csub(ssig(i), accum);
 229     ssig(i) = error;
 230
 231     scl2 = rate / (sum_sq + 1e-10);
 232         error = Cscl(Conjg(error),scl2);
 233     for (j = 0; j < asiz; j++) {
 234       k = wrap(j);
 235       afil(j) = Cadd(Cscl(afil(j), scl1),Cmul(error, dlay(k)));
 236     }
 237
 238     dptr = bump(dptr);
 239   }
 240 }
 241
 242 static void
 243 lmsr_adapt_n(LMSR lms) {
 244   int i, j, k;
 245   REAL sum_sq, scl1, scl2;
 246   COMPLEX accum, error;
 247   scl1 = 1.0 - rate * leak;
 248   for (i = 0; i < ssiz; i++) {
 249
 250     dlay(dptr) = ssig(i);
 251     accum = cxzero;
 252     sum_sq = 0.0;
 253
 254     for (j = 0; j < asiz; j++) {
 255       k = wrap(j);
 256       sum_sq += Csqrmag(dlay(k));
 257       accum = Cadd(accum, Cmul(Conjg(afil(j)), dlay(k)));
 258     }
 259
 260     error = Csub(ssig(i), accum);
 261     ssig(i) = accum;
 262
 263     scl2 = rate / (sum_sq + 1e-10);
 264         error = Cscl(Conjg(error),scl2);
 265     for (j = 0; j < asiz; j++) {
 266       k = wrap(j);
 267       afil(j) = Cadd(Cscl(afil(j), scl1),Cmul(error, dlay(k)));
 268     }
 269
 270     dptr = bump(dptr);
 271   }
 272 }
 273
 274 #endif
 275
 276 extern void
 277 lmsr_adapt(LMSR lms) {
 278   switch(lms->filter_type) {
 279   case LMADF_NOISE:
 280     lmsr_adapt_n(lms);
 281     break;
 282   case LMADF_INTERFERENCE:
 283    lmsr_adapt_i(lms);
 284     break;
 285   }
 286 }