Project_WIPV_antiSlip / IIR

Infinite impulse response filter

Fork of IIR by Project_WIPV_antiSlip

IIR.cpp@3:7191d27f4855, 2017-02-10 (annotated)

Committer:
benson516
Date:
Fri Feb 10 18:25:55 2017 +0000
Revision:
3:7191d27f4855
Rewrite the library to a more formal form

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 3:7191d27f4855 1 #include "IIR.h"
benson516 3:7191d27f4855 2 /*-----------------------------*/
benson516 3:7191d27f4855 3 // Order of the numerator: m
benson516 3:7191d27f4855 4 // Orser of the denominator: n
benson516 3:7191d27f4855 5 //--------------------------------//
benson516 3:7191d27f4855 6 //
benson516 3:7191d27f4855 7 // y gain*( sum(i from 0 to m) b[i]*x[k-i] )
benson516 3:7191d27f4855 8 // --- = --------------------------------------------
benson516 3:7191d27f4855 9 // u 1 + sum( i from 1 to n) a[i]*y[k-i]
benson516 3:7191d27f4855 10 //
benson516 3:7191d27f4855 11 // y[k] = gain*( sum(i from 0 to m) b[i]*x[k-i] ) - sum( i from 1 to n) a[i]*y[k-i]
benson516 3:7191d27f4855 12 //
benson516 3:7191d27f4855 13 // Compatible with the coefficient generated by Matlab fdatool
benson516 3:7191d27f4855 14 //
benson516 3:7191d27f4855 15 // by C.F. Huang from LDSC
benson516 3:7191d27f4855 16 /*-----------------------------*/
benson516 3:7191d27f4855 17
benson516 3:7191d27f4855 18 Circular_buffer::Circular_buffer(){
benson516 3:7191d27f4855 19 n = 0;
benson516 3:7191d27f4855 20 ind = 0;
benson516 3:7191d27f4855 21 }
benson516 3:7191d27f4855 22 Circular_buffer::Circular_buffer(int n_in){
benson516 3:7191d27f4855 23 n = n_in;
benson516 3:7191d27f4855 24 A.resize(n);
benson516 3:7191d27f4855 25 for (size_t i=0; i< A.size(); i++)
benson516 3:7191d27f4855 26 A[i] = 0;
benson516 3:7191d27f4855 27
benson516 3:7191d27f4855 28 ind = 0;
benson516 3:7191d27f4855 29 }
benson516 3:7191d27f4855 30 //
benson516 3:7191d27f4855 31 void Circular_buffer::Init(int n_in){ // Input: buffer size
benson516 3:7191d27f4855 32 n = n_in;
benson516 3:7191d27f4855 33 A.resize(n);
benson516 3:7191d27f4855 34 for (size_t i=0; i< A.size(); i++)
benson516 3:7191d27f4855 35 A[i] = 0;
benson516 3:7191d27f4855 36 ind = 0;
benson516 3:7191d27f4855 37 }
benson516 3:7191d27f4855 38 //
benson516 3:7191d27f4855 39 float Circular_buffer::Get(int i){ // Get the value of the (t-i)th element, where t stands for the current sample
benson516 3:7191d27f4855 40 int ind_e = ind-i;
benson516 3:7191d27f4855 41 while (ind_e < 0){
benson516 3:7191d27f4855 42 ind_e += A.size();
benson516 3:7191d27f4855 43 }
benson516 3:7191d27f4855 44 return A[ind_e];
benson516 3:7191d27f4855 45 }
benson516 3:7191d27f4855 46 void Circular_buffer::Insert(float x_new){ // Pop the oldest element and push a new element
benson516 3:7191d27f4855 47 ind++;
benson516 3:7191d27f4855 48 while (ind >= A.size()){
benson516 3:7191d27f4855 49 ind -= A.size();
benson516 3:7191d27f4855 50 }
benson516 3:7191d27f4855 51 A[ind] = x_new;
benson516 3:7191d27f4855 52 }
benson516 3:7191d27f4855 53 void Circular_buffer::reset(float val){ // Reset all elements to val
benson516 3:7191d27f4855 54 for (size_t i = 0; i< A.size(); i++)
benson516 3:7191d27f4855 55 A[i] = val;
benson516 3:7191d27f4855 56 ind = 0;
benson516 3:7191d27f4855 57 }
benson516 3:7191d27f4855 58
benson516 3:7191d27f4855 59 // IIR
benson516 3:7191d27f4855 60 //---------------------------------------//
benson516 3:7191d27f4855 61 IIR::IIR(int m, int n): m(m),n(n) {
benson516 3:7191d27f4855 62 x.Init(m+1);
benson516 3:7191d27f4855 63 y.Init(n);
benson516 3:7191d27f4855 64 }
benson516 3:7191d27f4855 65 //
benson516 3:7191d27f4855 66 void IIR::Assign_parameters(float b_in[], float a_in[], float gain_in){
benson516 3:7191d27f4855 67 //b.assign(b_in,b_in+m+1);
benson516 3:7191d27f4855 68 // a.assign(a_in,a_in+n);
benson516 3:7191d27f4855 69 b.resize(m+1);
benson516 3:7191d27f4855 70 a.resize(n);
benson516 3:7191d27f4855 71 for (size_t i = 0; i < n; i++){
benson516 3:7191d27f4855 72 a[i] = a_in[i+1];
benson516 3:7191d27f4855 73 }
benson516 3:7191d27f4855 74 gain = gain_in;
benson516 3:7191d27f4855 75 for (size_t i = 0; i < b.size(); i++){
benson516 3:7191d27f4855 76 b[i] = gain*b_in[i];
benson516 3:7191d27f4855 77 }
benson516 3:7191d27f4855 78 }
benson516 3:7191d27f4855 79 //
benson516 3:7191d27f4855 80 float IIR::Iterate_once(float x_in){
benson516 3:7191d27f4855 81 x.Insert(x_in);
benson516 3:7191d27f4855 82 float y_new = 0;
benson516 3:7191d27f4855 83 // Numerator
benson516 3:7191d27f4855 84 for (int i = 0; i <= m; i++ ){
benson516 3:7191d27f4855 85 y_new += b[i]*x.Get(i);
benson516 3:7191d27f4855 86 }
benson516 3:7191d27f4855 87
benson516 3:7191d27f4855 88 // Denominator
benson516 3:7191d27f4855 89 for (int i = 0; i < n; i++){
benson516 3:7191d27f4855 90 y_new -= a[i]*y.Get(i);
benson516 3:7191d27f4855 91 }
benson516 3:7191d27f4855 92 y.Insert(y_new);
benson516 3:7191d27f4855 93 return y_new;
benson516 3:7191d27f4855 94 }
benson516 3:7191d27f4855 95 void IIR::Reset(float val){ // Rest all elements to val
benson516 3:7191d27f4855 96 x.reset(val);
benson516 3:7191d27f4855 97 y.reset(val);
benson516 3:7191d27f4855 98 }