diff --git a/AudioLMSDenoiseNotch_F32.cpp b/AudioLMSDenoiseNotch_F32.cpp index 8c9479b..fa2ebe1 100644 --- a/AudioLMSDenoiseNotch_F32.cpp +++ b/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; jdata[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); diff --git a/AudioLMSDenoiseNotch_F32.h b/AudioLMSDenoiseNotch_F32.h index 8efea85..01643cf 100644 --- a/AudioLMSDenoiseNotch_F32.h +++ b/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 #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 decay = 0.9952f; + float32_t beta = 0.03f; + float32_t decay = 0.995f; uint16_t numLeak = 0; }; #endif diff --git a/examples/LMS1DenoiseNotch/LMS1DenoiseNotch.ino b/examples/LMS1DenoiseNotch/LMS1DenoiseNotch.ino new file mode 100644 index 0000000..d65e0ee --- /dev/null +++ b/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 + +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 + }