Diff: IIR_filter.cpp

Revision:

15:c70cad2f4e64

Parent:

14:9184aa9fdac7

--- a/IIR_filter.cpp	Mon Oct 28 07:53:02 2019 +0000
+++ b/IIR_filter.cpp	Thu Jan 16 09:12:50 2020 +0000
@@ -1,150 +1,148 @@
 #include "IIR_filter.h"
-#include "mbed.h"
-using namespace std;
 
 /*
   IIR filter implemention for the following filter types:
   init for: first order differentiatior:   G(s) = s/(T*s + 1)
             first order lowpass with gain  G(s) = K/(T*s + 1)
-            second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0*w0)        
+            second order lowpass with gain G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2)        
             nth order, with arbitrary values
-  the billinear transformation is used for s -> z
-  reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator
+  billinear transformation is used for s -> z
+  reseting the filter only makes sence for static signals, whatch out if you're using the differnetiator, static corresponds to output null
 */
 
 // G(s) = s/(T*s + 1)
-IIR_filter::IIR_filter(float T, float Ts){
+IIR_filter::IIR_filter(float T, float Ts) {
            
     // filter orders
-    nb = 1; // Filter Order
-    na = 1; // Filter Order
+    nb = 1;
+    na = 1;
     
     // filter coefficients
-    B = (double*)malloc((nb+1)*sizeof(double));
-    A = (double*)malloc(na*sizeof(double));    
-    B[0] = 2.0/(2.0*(double)T + (double)Ts);
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));    
+    B[0] = 2.0f/(2.0f*T + Ts);
     B[1] = -B[0];
-    A[0] = -(2.0*(double)T - (double)Ts)/(2.0*(double)T + (double)Ts);
+    A[0] = -(2.0f*T - Ts)/(2.0f*T + Ts);
     
     // signal arrays
-    uk = (double*)malloc((nb+1)*sizeof(double));
-    yk = (double*)malloc(na*sizeof(double));
-    uk[0]= uk[1] = 0.0;
-    yk[0] = 0.0;
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0] = uk[1] = 0.0f;
+    yk[0] = 0.0f;
     
     // dc-gain
-    this->K = 0.0;
+    this->K = 0.0f;
 }
 
 // G(s) = K/(T*s + 1)
-IIR_filter::IIR_filter(float T, float Ts, float K){
+IIR_filter::IIR_filter(float T, float Ts, float K) {
     
     // filter orders
-    nb = 1; // Filter Order
-    na = 1; // Filter Order
+    nb = 1;
+    na = 1;
     
     // filter coefficients
-    B = (double*)malloc((nb+1)*sizeof(double));
-    A = (double*)malloc(na*sizeof(double));      
-    B[0] = (double)Ts/((double)Ts + 2.0*(double)T);
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));      
+    B[0] = Ts/(Ts + 2.0f*T);
     B[1] = B[0];
-    A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 
+    A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); 
     
     // signal arrays
-    uk = (double*)malloc((nb+1)*sizeof(double));
-    yk = (double*)malloc(na*sizeof(double));
-    uk[0]= uk[1] = 0.0;
-    yk[0] = 0.0;
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0] = uk[1] = 0.0f;
+    yk[0] = 0.0f;
     
     // dc-gain
-    this->K = (double)K;
-}
-
-void IIR_filter::setup(float T, float Ts, float K){
-    
-    // filter orders
-    nb = 1; // Filter Order
-    na = 1; // Filter Order
-    
-    // filter coefficients
-    B = (double*)malloc((nb+1)*sizeof(double));
-    A = (double*)malloc(na*sizeof(double));      
-    B[0] = (double)Ts/((double)Ts + 2.0*(double)T);
-    B[1] = B[0];
-    A[0] = ((double)Ts - 2.0*(double)T)/((double)Ts + 2.0*(double)T); 
-    
-    // signal arrays
-    uk = (double*)malloc((nb+1)*sizeof(double));
-    yk = (double*)malloc(na*sizeof(double));
-    uk[0]= uk[1] = 0.0;
-    yk[0] = 0.0;
-    
-    // dc-gain
-    this->K = (double)K;
+    this->K = K;
 }
 
 // G(s) = K*w0^2/(s^2 + 2*D*w0*s + w0^2) 
-IIR_filter::IIR_filter(float w0, float D, float Ts, float K){
+IIR_filter::IIR_filter(float w0, float D, float Ts, float K) {
     
     // filter orders
-    nb = 2; // Filter Order
-    na = 2; // Filter Order
+    nb = 2;
+    na = 2;
     
     // filter coefficients
-    B = (double*)malloc((nb+1)*sizeof(double));
-    A = (double*)malloc(na*sizeof(double));
-    double k0 = (double)Ts*(double)Ts*(double)w0*(double)w0;
-    double k1 = 4.0*(double)D*(double)Ts*(double)w0;
-    double k2 = k0 + k1 + 4.0;    
-    B[0] = (double)K*k0/k2;
-    B[1] = 2.0*B[0];
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));
+    float k0 = Ts*Ts*w0*w0;
+    float k1 = 4.0f*D*Ts*w0;
+    float k2 = k0 + k1 + 4.0f;    
+    B[0] = K*k0/k2;
+    B[1] = 2.0f*B[0];
     B[2] = B[0]; 
-    A[0] = (2.0*k0 - 8.0)/k2;
-    A[1] = (k0 - k1 + 4.0)/k2;
+    A[0] = (2.0f*k0 - 8.0f)/k2;
+    A[1] = (k0 - k1 + 4.0f)/k2;
     
     // signal arrays
-    uk = (double*)malloc((nb+1)*sizeof(double));
-    yk = (double*)malloc(na*sizeof(double));
-    uk[0]= uk[1] = uk[2] = 0.0;
-    yk[0] = yk[1] = 0.0;
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0] = uk[1] = uk[2] = 0.0f;
+    yk[0] = yk[1] = 0.0f;
     
     // dc-gain
-    this->K = (double)K;
+    this->K = K;
 }
 
-IIR_filter::IIR_filter(float *b, float *a, int nb_, int na_){
+IIR_filter::IIR_filter(float *b, float *a, int nb, int na) {
     
     // filter orders
-    this->nb = nb_-1;    // Filter Order
-    this->na = na_;      // Filter Order
+    this->nb = nb - 1;
+    this->na = na;
     
     // filter coefficients
-    B = (double*)malloc((nb+1)*sizeof(double));
-    A = (double*)malloc(na*sizeof(double));
-    uk = (double*)malloc((nb+1)*sizeof(double));
-    yk = (double*)malloc(na*sizeof(double));
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
     
-    for(int k=0;k<=nb;k++){
-        B[k]=b[k];
-        uk[k]=0.0;
-        }
-    for(int k=0;k<na;k++){
+    for(uint8_t k=0;k<=nb;k++) {
+        B[k] = b[k];
+        uk[k] = 0.0f;
+    }
+    for(uint8_t k=0;k<na;k++) {
         A[k] = a[k];
-        yk[k] = 0.0;
-        }
+        yk[k] = 0.0f;
+    }
     
     // dc-gain
-    this->K = 1.0;
+    this->K = 1.0f;
 }
 
     
 IIR_filter::~IIR_filter() {} 
+
+void IIR_filter::setup(float T, float Ts, float K) {
     
+    // filter orders
+    nb = 1;
+    na = 1;
+    
+    // filter coefficients
+    B = (float*)malloc((nb+1)*sizeof(float));
+    A = (float*)malloc(na*sizeof(float));      
+    B[0] = Ts/(Ts + 2.0f*T);
+    B[1] = B[0];
+    A[0] = (Ts - 2.0f*T)/(Ts + 2.0f*T); 
+    
+    // signal arrays
+    uk = (float*)malloc((nb+1)*sizeof(float));
+    yk = (float*)malloc(na*sizeof(float));
+    uk[0] = uk[1] = 0.0f;
+    yk[0] = 0.0f;
+    
+    // dc-gain
+    this->K = K;
+}
+
 void IIR_filter::reset(float val) {
-    for(int k=0;k < nb;k++)
-        uk[k] = (double)val;
-    for(int k=0;k < na;k++)
-        yk[k] = (double)val*K;
+    for(uint8_t k=0;k < nb;k++)
+        uk[k] = val;
+    for(uint8_t k=0;k < na;k++)
+        yk[k] = val*K;
         
 }
 
@@ -154,17 +152,18 @@
     y(n) =  B[0]*u(k)   + B[1]*u(k-1) + ... + B[nb]*u(k-nb) + ...
           - A[0]*y(k-1) - A[1]*y(k-2) - ... - A[na]*y(n-na)
 */
-float IIR_filter::filter(double input){
-    for(int k = nb;k > 0;k--)    // shift input values back
+float IIR_filter::filter(float input) {
+    
+    for(uint8_t k = nb;k > 0;k--)    // shift input values back
         uk[k] = uk[k-1];
     uk[0] = input;
-    double ret = 0.0;
-    for(int k = 0;k <= nb;k++)
+    float ret = 0.0f;
+    for(uint8_t k = 0;k <= nb;k++)
         ret += B[k] * uk[k];
-    for(int k = 0;k < na;k++)
+    for(uint8_t k = 0;k < na;k++)
         ret -= A[k] * yk[k];
-    for(int k = na;k > 1;k--)
+    for(uint8_t k = na;k > 1;k--)
         yk[k-1] = yk[k-2];
     yk[0] = ret;
-    return (float)ret;
+    return ret;
 }