IIR Direct Form II Filter Implementation on Teensy 4.0

Status
Not open for further replies.

Willum_

Member
Hello all,

I am a student helping a professor port some code from some dsp labs from a TI TMDSDSK6713 board onto the Teensy 4.0 and audio shield.

The labs include a moving average and IIR filter implementation. So far I've used https://forum.pjrc.com/index.php?threads/59171/ as a helpful reference in order to understand how I can access the data streams directly to implement the filters using the Aduio library. The only change that I really made was to extend the number of data blocks that are stored in a data buffer and made it circular to simplify the dsp algorithms.

I've gotten the moving average working correctly, so I'm fairly certain that the memory accessing of the circular data buffers are correct, but the IIR implementation is giving me trouble and i'm not quite sure what is breaking. The IIR filter is implemented in the Direct Form II method where the input is multiplied by the a coefficients to create an intermediate function that is then multiplied by the b coefficients to form the output. What I can see from the serial buffer is that after runs through the calculation of the first 20 or so values on start up and the values seem to blow up to a huge number and overflow. The filter itself was designed using MATLAB so it is stable. Also, I'm initializing the arrays of the input buffers to 0 so these initial values should be 0.

At this point i'm not sure if it's a problem with my code or is a Teensy specific problem so i'd appreciate the help!

Here is the code for the IIR Filter:

Code:
#include <Audio.h>
#include <stdio.h>

#define AUDIO_MASK 0x0000FFFF
#define CIRCULAR_BUFFER_LENGTH AUDIO_BLOCK_SAMPLES*4
#define CIRCULAR_BUFFER_MASK (CIRCULAR_BUFFER_LENGTH - 1)
#define MAX_SAMPLE_BLOCKS (CIRCULAR_BUFFER_LENGTH / AUDIO_BLOCK_SAMPLES)-1
//Audio library macro, AUDIO_BLOCK_SAMPLES = 128
#define IIR_ORDER   10
#define VOLUME_AMPLIFY 1

AudioInputI2S i2s_in;
AudioOutputI2S i2s_out;
AudioRecordQueue Q_in_L;
AudioRecordQueue Q_in_R;
AudioPlayQueue Q_out_L;
AudioPlayQueue Q_out_R;
AudioConnection patchCord1(i2s_in, 0, Q_in_L, 0);
AudioConnection patchCord2(i2s_in, 1, Q_in_R, 0);
AudioConnection patchCord3(Q_out_L, 0, i2s_out, 0);
AudioConnection patchCord4(Q_out_R, 0, i2s_out, 1);
AudioControlSGTL5000 sgtl5000;

const int myInput = AUDIO_INPUT_LINEIN;

double buf_L[CIRCULAR_BUFFER_LENGTH] = {0.0};
double buf_R[CIRCULAR_BUFFER_LENGTH] = {0.0};
int curSample = 0;
int curBlock = 0;

double a[] = {1.0,             // a0
              -9.7723,         // a1
              42.9767,         // a2
              -112.0082,       // a3
              191.5851,        // a4
              -224.7201,       // a5
              183.0563,        // a6
              -102.2575,       // a7
              37.4887,         // a8
              -8.1449,         // a9
              0.7964           // a10
             };      

double b[] = {0.0029E-015,    // b0
              0.0287E-015,    // b1
              0.1290E-015,    // b2
              0.344E-015,     // b3
              0.6022E-015,    // b4
              0.7226E-015,    // b5
              0.6022E-015,    // b6
              0.3441E-015,    // b7
              0.1290E-015,    // b8
              0.0287E-015,    // b9
              0.0029E-015     // b10
             };   
//500hz low pass

void setup(void)
{
  // Audio connections require memory. and the record queue
  // uses this memory to buffer incoming audio.
  AudioMemory(10);

  // Enable the audio shield. select input. and enable output
  sgtl5000.enable();
  sgtl5000.inputSelect(myInput);
  sgtl5000.volume(0.5);
  // Start the record queues
  Q_in_L.begin();
  Q_in_R.begin();
  
  Serial.begin(9600);
}

void loop(void)
{
  short *bp_L, *bp_R;

  // Wait for left and right input channels to have content
  while (!Q_in_L.available() && !Q_in_R.available());
  
  bp_L = Q_in_L.readBuffer();
  bp_R = Q_in_R.readBuffer();

  int startIndex = curBlock * AUDIO_BLOCK_SAMPLES;
  for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
    curSample = startIndex + i;
    buf_L[curSample] = (double)bp_L[i];
    buf_R[curSample] = (double)bp_R[i];
  }

  Q_in_L.freeBuffer();
  Q_in_R.freeBuffer();

  // Get pointers to "empty" output buffers
  bp_L = Q_out_L.getBuffer();
  bp_R = Q_out_R.getBuffer();
  
  //Temperary arrays to hold new values of current sample block
  double leftOut[AUDIO_BLOCK_SAMPLES] = {0.0};
  double rightOut[AUDIO_BLOCK_SAMPLES] = {0.0};
  
  // Operate on each value in current block that has been recieved
  for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
    curSample = startIndex + i;
    
    /* Start user code */
    //Type II direct implementation of IIR filter
    // w[curSample] = w[curSample] - a1*w1 - a2*w2 - ... - a[j]*w[n]
    for (int j = 1; j <= IIR_ORDER; j++)
    {
      buf_L[curSample] -= a[j] * buf_L[(curSample - j) & CIRCULAR_BUFFER_MASK];
      buf_R[curSample] -= a[j] * buf_R[(curSample - j) & CIRCULAR_BUFFER_MASK];
    }
    Serial.print(curSample);
    Serial.print(": ");
    Serial.print(buf_L[curSample],20);
    Serial.print("\n");
    leftOut[i] = 0;
    rightOut[i] = 0;
    // y = b0*w0 + b1*w1 + ... +b[n]*w[j]
    for(int j =0; j <= IIR_ORDER; j++)
    {
      leftOut[i] += b[j] * buf_L[(curSample - j) & CIRCULAR_BUFFER_MASK];
      rightOut[i] += b[j] * buf_R[(curSample - j) & CIRCULAR_BUFFER_MASK];
    }
    leftOut[i] *= VOLUME_AMPLIFY;
    rightOut[i]*= VOLUME_AMPLIFY;
    /* End user code */
    
  }

  // copy the processed data block back to the output
  for (int i = 0; i < AUDIO_BLOCK_SAMPLES; i++) {
    bp_L[i] = (short)leftOut[i];
    bp_R[i] = (short)rightOut[i];
  }

  curBlock++;
  curBlock &= MAX_SAMPLE_BLOCKS;

  // and play them back into the audio queues
  Q_out_L.playBuffer();
  Q_out_R.playBuffer();
}
 
it seems that the filter is NOT stable (most likely too high order)
try following Matlab code
Code:
a=[1.0, -9.7723, 42.9767, -112.0082, 191.5851, -224.7201,  183.0563, ...
    -102.2575, 37.4887, -8.1449, 0.7964];
b=[0.0029E-015, 0.0287E-015, 0.1290E-015, 0.344E-015,  0.6022E-015, ...
     0.7226E-015, 0.6022E-015, 0.3441E-015, 0.1290E-015, 0.0287E-015, ...
     0.0029E-015];

 x=randn(1000,1);
 y=filter(b,a,x);
 figure(1)
 plot([x,y])
 ylim(5*[-1,1])

The response does also not look like a well defined LP filter.
 
Last edited:
with the following coefficients
Code:
const double a[] = {
                   1,   -9.544621361945,    41.00487909342,   -104.4173708231,
      174.5348069734,   -200.0948614402,    159.3409444366,   -87.02835846808,
      31.20046029706,   -6.630002303527,   0.6341235963847
};
const double b[] = {
  2.628675777601e-15,2.628675777601e-14, 1.18290409992e-13,3.154410933121e-13,
  5.520219132962e-13,6.624262959554e-13,5.520219132962e-13,3.154410933121e-13,
   1.18290409992e-13,2.628675777601e-14,2.628675777601e-15
};

500 Hz LP Butterworth 44.1 kHz sampling

filter seems to be stable for a longer time, (>100 samples)
 
it seems that the filter is NOT stable (most likely too high order)
try following Matlab code
Code:
a=[1.0, -9.7723, 42.9767, -112.0082, 191.5851, -224.7201,  183.0563, ...
    -102.2575, 37.4887, -8.1449, 0.7964];
b=[0.0029E-015, 0.0287E-015, 0.1290E-015, 0.344E-015,  0.6022E-015, ...
     0.7226E-015, 0.6022E-015, 0.3441E-015, 0.1290E-015, 0.0287E-015, ...
     0.0029E-015];

 x=randn(1000,1);
 y=filter(b,a,x);
 figure(1)
 plot([x,y])
 ylim(5*[-1,1])

The response does also not look like a well defined LP filter.

I do see that now. All I did was was change the order from 10 to 8 and redesigned it, again using the MATLAB butter tool, and now it works! Thanks for the advice!

Why is it that the length of the filter caused it to be unstable exactly? I understand my filter coefficients aren't correct now looking at it using the fvtool, but yours looks a lot better but is still unstable.
 
My understanding is that Type II can easily be unstable:
you first have the AR and when the dynamic range of the computer is not sufficient (64 bit may not be sufficient) then solution oscillates and diverges.
Also 500 Hz relative to say 44100 Hz is a very challenging task for a filter, maybe decimation would help.
Also, using a 5 stage biquad filter may be more stable.
 
I was not aware of these limitations as i'm quite a novice in the world of DSP so thanks for the really helpful info! :)
 
BTW,
I just playing with IIR implementation
for Teensy you may consider the CMSIS DSP functions.
but for understanding I converted a cascaded Biquad to Matlab
Code:
ic=3; % number of biquads
% filter estimation
[A,B,C,D]=butter(2*ic,500*2/44100);
[sos,g]=ss2sos(A,B,C,D);

% simulate data
x=randn(1000,1);

% filtering
p=zeros(ic,4); % state memory
y=x;
for jj=1:ic
    b0=sos(jj,1); b1=sos(jj,2); b2=sos(jj,3); a1=sos(jj,5); a2=sos(jj,6);
    Xn1=p(jj,1);
    Xn2=p(jj,2);
    Yn1=p(jj,3);
    Yn2=p(jj,4);

    for ii=1:1000
        Xn=y(ii);
        acc=b0*Xn + b1*Xn1 + b2*Xn2 - a1*Yn1 - a2*Yn2;
        y(ii)=acc;
        Xn2=Xn1;
        Xn1=Xn;
        Yn2=Yn1;
        Yn1=acc;
    end
    p(jj,1)=Xn1;
    p(jj,2)=Xn2;
    p(jj,3)=Yn1;
    p(jj,4)=Yn2;
end
y=y*g;

%2nd implementation
z=x;
p=zeros(ic,4);
for ii=1:1000
    for jj=1:3
        b0=sos(jj,1); b1=sos(jj,2); b2=sos(jj,3); a1=sos(jj,5); a2=sos(jj,6);
        Xn1=p(jj,1);
        Xn2=p(jj,2);
        Yn1=p(jj,3);
        Yn2=p(jj,4);
        %
        Xn=z(ii);
        acc=b0*Xn + b1*Xn1 + b2*Xn2 - a1*Yn1 - a2*Yn2;
        z(ii)=acc;
        %
        p(jj,1)=Xn;
        p(jj,2)=Xn1;
        p(jj,3)=acc;
        p(jj,4)=Yn1;
    end
end
 z=z*g;
 
 %matlab function
 u =sosfilt(sos,x)*g;
 
 figure(1)
 hp=plot([x,y,z,u]);
 set(hp(2:end),'linewidth',2)

may it helps someone and can be used for playing
 
One problem is you are not using second order sections (SOS) so the filter is basically numerically unstable due to the high degree polynomial used.

Direct form realization is not normally used for high order IIR filters.

Here's am 11 pole Butterworth SOS filter for 500Hz low pass at fs=44100:
Code:
[[  9.15774145e-17   1.83154829e-16   9.15774145e-17   1.00000000e+00   -9.31184117e-01   0.00000000e+00]
 [  1.00000000e+00   2.00000000e+00   1.00000000e+00   1.00000000e+00   -1.86739454e+00   8.72142945e-01]
 [  1.00000000e+00   2.00000000e+00   1.00000000e+00   1.00000000e+00   -1.88222266e+00   8.87008776e-01]
 [  1.00000000e+00   2.00000000e+00   1.00000000e+00   1.00000000e+00   -1.90608201e+00   9.10928791e-01]
 [  1.00000000e+00   2.00000000e+00   1.00000000e+00   1.00000000e+00   -1.93763477e+00   9.42561787e-01]
 [  1.00000000e+00   1.00000000e+00   0.00000000e+00   1.00000000e+00   -1.97492205e+00   9.79943878e-01]]
The first 3 entries in each line are numerator (b), and the last three are denominator (a)

Evaluating using second order sections is usually the most stable filter realization. Second order filters are also called biquadratic,
as implemented in the Teensy audio library AudioFilterBiquad class, which can handle 1 to 4 biquadratic stages - you'd only need
a cascade of two of these objects to implement this filter.
 
Status
Not open for further replies.
Back
Top