Test program with the RT black boxes

Dependencies:   mbed

IIR_filter.cpp

Committer:
rtlabor
Date:
22 months ago
Revision:
0:15be70d21d7c
Child:
5:d6c7ccbbce78

File content as of revision 0:15be70d21d7c:

#include "IIR_filter.h"
#include "mbed.h"
using namespace std;
/*  IIR filter. implemention is for n-th order, init only for 1st order
    coeffissients based on bilinear transform if 1st order filter
    
              1                              Ts/(Ts+2*tau) * (z+1)
    G(s) = -------                  G(z) = ---------------------------
            tau*s+1                         z + (Ts-2*tau)/(Ts+2*tau)
    */
IIR_filter::IIR_filter(float tau,float Ts){
    b = (float*)malloc( 2 * sizeof(float) );
    a = (float*)malloc( 1 * sizeof(float) );
    uk = (float*)malloc( 2 * sizeof(float) );
    yk = (float*)malloc( 1 * sizeof(float) );
    nb = 1; // Filter Order
    na = 1; // Filter Order
    b[0] = Ts/(Ts+2.0f*tau);
    b[1] = b[0];
    a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
    uk[0]= uk[1] = 0.0f;
    yk[0] = 0.0f;
    this->dc = 1.0f;
    }
    // the following filter has in addition a dc-gain ( dc/(tau*s+1)  )
IIR_filter::IIR_filter(float tau,float Ts,float dc){
    b = (float*)malloc( 2 * sizeof(float) );
    a = (float*)malloc( 1 * sizeof(float) );
    uk = (float*)malloc( 2 * sizeof(float) );
    yk = (float*)malloc( 1 * sizeof(float) );
    nb = 1; // Filter Order
    na = 1; // Filter Order
    b[0] = dc * Ts/(Ts+2.0f*tau);
    b[1] = b[0];
    a[0] = (Ts-2.0f*tau)/(Ts+2.0f*tau);
    uk[0]= uk[1] = 0.0f;
    yk[0] = 0.0f;
    this->dc = dc;
    }
    
IIR_filter::~IIR_filter() {} 
    
void IIR_filter::reset(float val) {
    for(unsigned int k=0;k < nb;k++)
        uk[k] = val;
    for(unsigned int k=0;k < na;k++)
        yk[k] = val*dc;
        
}
/* the filter step: 
y(n) =  b[0]*u_k   + b[1]*u_k-1 + ... + b[nb]*u_k-nb
      - a[0]*y_k-1 - ... - a[na]*y_n-na  // mind: a[0] corresponds to z^(na-1)
*/
float IIR_filter::filter(float input){
    unsigned int k;
    for(k = nb;k > 0;k--)    // shift input values back
        uk[k] = uk[k-1];
    uk[0] = input;
    float ret = 0.0f;
    for(k = 0;k <= nb;k++)
        ret += b[k] * uk[k];
    for(k = 0;k < na;k++)
        ret -= a[k] * yk[k];
    for(k = na;k > 1;k--)
        yk[k-1] = yk[k-2];
    yk[0] = ret;
    return ret;
    }