Rev to new LMSDenoiseNotch

AudioLMSDenoiseNotch_F32.cpp

@@ -61,7 +61,15 @@ void AudioLMSDenoiseNotch_F32::update(void)
 
         // Circular delay line to find correlated components
         dataD[kNextD] = blockDataIn;     // Get a new data point from block
-        // Rotate to next higher k index
+
+#ifdef LMS_NORMALIZE
+        powerNorm[i] = blockDataIn*blockDataIn;
+        pNorm += powerNorm[i];
+        if(i==127)
+           pNorm -= powerNorm[0];
+        else
+           pNorm -= powerNorm[i+1];
+#endif
 
         if(++kNextD >= lengthDataD)     // Next spot in delay line
            kNextD = 0;
@@ -80,10 +88,15 @@ void AudioLMSDenoiseNotch_F32::update(void)
         error = blockDataIn - firOut;
 
         // Update the coefficients
+#ifdef LMS_NORMALIZE
+        float32_t kcf = error*beta/pNorm;
+#else
+        float32_t kcf = error*beta;
+#endif
         for(j=0; j<lengthDataF; j++)
             {
             k = (j + kOffsetF) & kMask;
-            coeff[j] = coeff[j] + beta*error*dataF[k];
+            coeff[j] = coeff[j] + kcf*dataF[k];
             }
 
         // Move to next positions in circular data buffer via kOffsetF
@@ -94,7 +107,7 @@ void AudioLMSDenoiseNotch_F32::update(void)
         if(what == DENOISE)
           blockOut->data[i] = firOut;
         else
-          blockOut->data[i] = blockDataIn;    //  error;           // Auto-Notch
+          blockOut->data[i] = error;           // Auto-Notch
         }
         //transmit the block and be done
         AudioStream_F32::transmit(blockOut);

AudioLMSDenoiseNotch_F32.h

@@ -2,7 +2,8 @@
  * AudioLMSDenoiseNotch_F32.h
  *
  * Created: Bob Larkin, January 2022
- * Purpose; LMS DeNoise for audio.  Assumes floating-point data.
+ * Purpose; LMS DeNoise and Auto-notch for audio.
+ * Assumes floating-point data.
  *
  * 22 January 2022   copyright (c)Robert Larkin 2022
  *
@@ -33,7 +34,7 @@
   * The auto-notch is very effective at removing annoying tones when they are
   * reasonably strong.  Again for radio systems, this can be quite useful.
   * The initialization selects whether DeNoise or AutoNotch is used.  It makes
-  * no sense to use both at once as, in a perfect world, would remove everything.
+  * no sense to use both at once as, in a perfect world, that would remove everything.
   *
   * The LMS algorithm for optimization was first proposed by
   * Widrow and Hoff in 1960.
@@ -44,9 +45,13 @@
   * subtracted from the FIR filter output to remove coherent signals,
   * producing the so called "auto-notch."
   *
-  *       Johan, dsp-10  <<<<<<<<<<<<<<<<<<<<
+  * This particular write of the denoise and auto-notch traces back to
+  * Johan Forrer, KC7WW, per September 1994 QEX.  From there it was used
+  * in the DSP-10 project, http://www.janbob.com/electron/dsp10/dsp10.htm
   *
-  *   beta and decay
+  * The normalized version of coefficient update is generally best.  If it
+  * is not desired, it can be removed at compile time by commenting out
+  * "#define LMS_NORMALIZE" below.
   *
   * Initialization also sets the size of the FIR  buffer used to filter signal
   * and noise.  Small buffers respond to change quickly.  Large buffers can work
@@ -62,6 +67,14 @@
   *
   * This block behaves as a pass-through filter with one input and one output.
   *
+  * There are two parameters that are set in the .ino via the function
+  * setParameters(float32_t beta, float32_t decay)
+  * The first, beta determines the rate of convergence of the coefficients.
+  * This changes the "sound" of the audio and normally is one of a radio's
+  * front panel adjustments.  The second parameter, decay, slowly turns
+  * the algorithm off when signals are absent.  It is normally very
+  * slightly less than 1.0.  This can also change the "sound."
+  *
   * The Teensy 3.6 needs 690 microseconds per 128 block update using a FIR
   * buffer size of 32.  It needs 1335 microseconds using 64 FIR Buffer.
   * Note that the ARM library LMS routines might improve these
@@ -74,7 +87,8 @@
   * 270 for 64, and 529 microseconds for 128.
   *
   * All timing was done with a delay buffer of 4, but this size has
-  * very little effect, anyway.
+  * very little effect, anyway. Normalization was off, also, but
+  * again, this has a minor effect.
   */
 
 #ifndef _AudioLMSDenoiseNotch_F32_h
@@ -83,6 +97,9 @@
 #include <AudioStream_F32.h>
 #include "arm_math.h"
 
+// Default is to use the normalized form of coefficient update
+#define LMS_NORMALIZE
+
 #define MAX_FIR  256
 #define MAX_DELAY 16
 #define DENOISE  1
@@ -106,6 +123,10 @@ class AudioLMSDenoiseNotch_F32 : public AudioStream_F32
         kMask = lengthDataF - 1;
         lengthDataD = _lengthDataD;
         lengthDataD = (lengthDataD>MAX_DELAY ? MAX_DELAY : lengthDataD);  // Limit length
+#ifdef LMS_NORMALIZE
+        for(int i=0; i<128; i++) powerNorm[i] = 0.01f;
+        pNorm = 0.01f * 128.0f;
+#endif
         return lengthDataF;
         }
 
@@ -138,6 +159,10 @@ class AudioLMSDenoiseNotch_F32 : public AudioStream_F32
     uint16_t lengthDataD = 4;      // Any value, 2 to MAX_DELAY
 
     float32_t coeff[128];
+#ifdef LMS_NORMALIZE
+    float32_t powerNorm[128];
+    float32_t pNorm = 0.0f;
+#endif
 
     // dataF[] is arranged, by added variables kOffset and
     // lengthDataF, to be circular. A power-of-2 mask makes it circular.
@@ -147,8 +172,8 @@ class AudioLMSDenoiseNotch_F32 : public AudioStream_F32
     uint16_t lengthDataF = 64;
     uint16_t kMask = 63;
 
-    float32_t beta = 0.001;  //0.03f;
+    float32_t beta = 0.03f;
-    float32_t decay = 0.9952f;
+    float32_t decay = 0.995f;
     uint16_t numLeak = 0;
 };
 #endif

examples/LMS1DenoiseNotch/LMS1DenoiseNotch.ino

@@ -0,0 +1,121 @@
+/* Test AudioLMSDenoiseNotch_F32 from OpenAudio_ArduinoLibrary
+ *  Just a simpe sine wave plus noise input.  Select
+ *  Deoise or AutoNotch Filter functions by #defines below.
+ *  Output is either a sample of the time series or
+ *  the spectrum fro the FFT.
+ *
+ * For notes, see: AudioLMSDenoiseNotch_F32.h
+ * 
+ * Bob Larkin 29 Jan 2022
+ * Public Domain
+ */
+#include "AudioStream_F32.h"
+#include "Arduino.h"
+#include "Audio.h"
+#include "OpenAudio_ArduinoLibrary.h"
+
+// ***** UN-COMMENT ONE:
+#define DO_DENOISE
+//#define DO_AUTONOTCH
+
+// ***** UN-COMMENT ONE:
+#define OUTPUT_QUEUE
+// #define OUTPUT_FFT
+
+// T3.x supported sample rates: 2000, 8000, 11025, 16000, 22050, 24000, 32000, 44100, 44117, 48000,
+//                             88200, 88235 (44117*2), 95680, 96000, 176400, 176470, 192000
+// T4.x supports any sample rate the codec will handle.
+
+const float sample_rate_Hz = 44117.0f;
+const int   audio_block_samples = 128;
+AudioSettings_F32 audio_settings(sample_rate_Hz, audio_block_samples);
+
+#include "OpenAudio_ArduinoLibrary.h"
+#include "AudioStream_F32.h"
+#include <Audio.h>
+
+AudioSynthSineCosine_F32 sine1;      //xy=60,219
+AudioSynthGaussian_F32   GaussianWhiteNoise1; //xy=106,263
+AudioMixer4_F32          mixer4_1;       //xy=154,334
+AudioLMSDenoiseNotch_F32 LMS1;           //xy=206,411
+AudioOutputI2S_F32       audioOutI2S1;   //xy=301,542
+AudioAnalyzeFFT1024_F32  FFT1024_1;      //xy=362,495
+AudioRecordQueue_F32     recordQueue1;   //xy=378,446
+AudioConnection_F32      patchCord1(sine1, 0, mixer4_1, 0);
+AudioConnection_F32      patchCord2(GaussianWhiteNoise1, 0, mixer4_1, 1);
+AudioConnection_F32      patchCord3(mixer4_1, LMS1);
+AudioConnection_F32      patchCord4(LMS1, recordQueue1);
+AudioConnection_F32      patchCord5(LMS1, FFT1024_1);
+AudioConnection_F32      patchCord6(LMS1, 0, audioOutI2S1, 0);
+AudioConnection_F32      patchCord7(LMS1, 0, audioOutI2S1, 1);
+AudioControlSGTL5000     sgtl5000_1;     //xy=97,571
+
+float saveDat[512];
+
+void setup() {
+  Serial.begin(300);  // Any value
+  delay(1000);
+  Serial.println("OpenAudio_ArduinoLibrary  - Test LMS DeNoise & AutoNotch");
+  
+  // AudioMemory(5);
+  AudioMemory_F32(50, audio_settings);
+  sgtl5000_1.enable();
+  // Change the next 3 to suit your experiment
+  sine1.frequency(1292.49f);
+  sine1.amplitude(0.5f);
+  GaussianWhiteNoise1.amplitude(0.1f);  // Standard Deviation
+
+  Serial.print("Achieved FIR length = ");
+#ifdef DO_DENOISE
+  // Change the last two parameters for array sizes.  See .h file.
+  Serial.println(LMS1.initializeLMS(DENOISE, 64, 4));  // <== Modify to suit
+#endif
+#ifdef DO_AUTONOTCH
+  Serial.println(LMS1.initializeLMS(NOTCH, 32, 4));  // <== Modify to suit
+#endif
+  LMS1.setParameters(0.05f, 0.999f);      // (float _beta, float _decay);
+  LMS1.enable(true);
+  FFT1024_1.windowFunction(NULL);     // (AudioWindowHanning1024);
+  
+#ifdef OUTPUT_QUEUE
+  recordQueue1.begin();
+#endif
+  }
+
+void loop()
+  {
+  float *pq;
+  int nQ;
+  static uint32_t nTimes = 0;
+#ifdef OUTPUT_FFT
+  if ( FFT1024_1.available() )
+     {
+     // When new FFT data is available
+     // print it all to the Arduino Serial Monitor
+     float* pin = FFT1024_1.getData();
+     for (int  kk=0; kk<512; kk++)
+         saveDat[kk]= *(pin + kk);
+     if(++nTimes>4 && nTimes<6)
+         {
+         Serial.println("Freq, Hz   Power, dB");
+         for (int i=0; i<512; i++)
+             {
+             Serial.print(43.083f*(float32_t)i, 2);
+             Serial.print(",  ");
+             Serial.println(20.0f*log10f(0.0078125f*saveDat[i]), 3);
+             }
+         Serial.println();
+         }
+      }
+#endif
+
+#ifdef OUTPUT_QUEUE
+  if( nQ = recordQueue1.available() )
+    {// Serial.print("Data Available ");
+      pq = recordQueue1.readBuffer();
+      for(int i=0; i<128; i++)
+         Serial.println(*(pq + i),7);
+      recordQueue1.freeBuffer();
+    }  
+#endif
+  }