kazunori ogasawara / DigitalFilter

ver.beta0

DigitalFilter.cpp@0:b7a168dd2734, 2014-09-10 (annotated)

Committer:
macht
Date:
Wed Sep 10 04:28:53 2014 +0000
Revision:
0:b7a168dd2734
Digital Filter Libraries beta ver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
macht 0:b7a168dd2734 1 #include "DigitalFilter.h"
macht 0:b7a168dd2734 2
macht 0:b7a168dd2734 3 DigitalFilter::DigitalFilter(FilterType filter_type,float fs){
macht 0:b7a168dd2734 4 fs_ = fs;
macht 0:b7a168dd2734 5 ts_ = 1.0/fs_;
macht 0:b7a168dd2734 6 filter_type_ = filter_type;
macht 0:b7a168dd2734 7 }
macht 0:b7a168dd2734 8
macht 0:b7a168dd2734 9 int DigitalFilter::init(){
macht 0:b7a168dd2734 10 float a0_tmp;
macht 0:b7a168dd2734 11 gain_ = 1.0;
macht 0:b7a168dd2734 12 switch(filter_type_){
macht 0:b7a168dd2734 13
macht 0:b7a168dd2734 14 //1st order lowpass filter. Need to set fcl before calling init().
macht 0:b7a168dd2734 15 //transfer function in S space H(s) = 1/(1+tau*s)
macht 0:b7a168dd2734 16 //transfer function in Z space H(z) = {b0+b1*z^(-1)}/{1+a1*z^(-1)}
macht 0:b7a168dd2734 17 case LPF1:
macht 0:b7a168dd2734 18 delay_step_ = 1;
macht 0:b7a168dd2734 19 a_.resize(2);
macht 0:b7a168dd2734 20 b_.resize(2);
macht 0:b7a168dd2734 21 u_.resize(2);
macht 0:b7a168dd2734 22 y_.resize(2);
macht 0:b7a168dd2734 23 tau_ = 1.0/(2.0*pi_*fc_);
macht 0:b7a168dd2734 24 a0_tmp = ts_+2.0*tau_;
macht 0:b7a168dd2734 25 b_[0] = ts_/a0_tmp;
macht 0:b7a168dd2734 26 b_[1] = b_[0];
macht 0:b7a168dd2734 27 a_[0] = 1.0;
macht 0:b7a168dd2734 28 a_[1] = (ts_-2.0*tau_)/a0_tmp;
macht 0:b7a168dd2734 29 this->reset();
macht 0:b7a168dd2734 30 break;
macht 0:b7a168dd2734 31
macht 0:b7a168dd2734 32 //2nd order lowpass filter. Need to set fcl and zeta before calling init().
macht 0:b7a168dd2734 33 //transfer function in S space H(s) = 1/(1+2*zeta*tau*s+tau^2*s^2)
macht 0:b7a168dd2734 34 case LPF2:
macht 0:b7a168dd2734 35 delay_step_ = 2;
macht 0:b7a168dd2734 36 a_.resize(3);
macht 0:b7a168dd2734 37 b_.resize(3);
macht 0:b7a168dd2734 38 u_.resize(3);
macht 0:b7a168dd2734 39 y_.resize(3);
macht 0:b7a168dd2734 40
macht 0:b7a168dd2734 41 tau_ = 1.0/(2.0*pi_*fc_);
macht 0:b7a168dd2734 42 a0_tmp = ts_*ts_+4.0*zeta_*tau_*ts_+4.0*tau_*tau_;
macht 0:b7a168dd2734 43 b_[0] = (ts_*ts_)/a0_tmp;
macht 0:b7a168dd2734 44 b_[1] = (2.0*ts_*ts_)/a0_tmp;
macht 0:b7a168dd2734 45 b_[2] = (ts_*ts_)/a0_tmp;
macht 0:b7a168dd2734 46 a_[0] = 1.0;
macht 0:b7a168dd2734 47 a_[1] = (2.0*ts_*ts_-8.0*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 48 a_[2] = (ts_*ts_-4*zeta_*tau_*ts_+4*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 49 this->reset();
macht 0:b7a168dd2734 50 break;
macht 0:b7a168dd2734 51 //2nd order highpass filter. Need to set fch and zeta before calling init().
macht 0:b7a168dd2734 52 //transfer function in S space H(s) = S^2/(1+2*zeta*tau*s+tau^2*s^2)
macht 0:b7a168dd2734 53 case HPF2:
macht 0:b7a168dd2734 54 delay_step_ = 2;
macht 0:b7a168dd2734 55 a_.resize(3);
macht 0:b7a168dd2734 56 b_.resize(3);
macht 0:b7a168dd2734 57 u_.resize(3);
macht 0:b7a168dd2734 58 y_.resize(3);
macht 0:b7a168dd2734 59
macht 0:b7a168dd2734 60 tau_ = 1.0/(2.0*pi_*fc_);
macht 0:b7a168dd2734 61 a0_tmp = ts_*ts_+4.0*zeta_*tau_*ts_+4.0*tau_*tau_;
macht 0:b7a168dd2734 62 b_[0] = (4.0*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 63 b_[1] = (-8.0*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 64 b_[2] = (4.0*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 65 a_[0] = 1.0;
macht 0:b7a168dd2734 66 a_[1] = (2.0*ts_*ts_-8*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 67 a_[2] = (ts_*ts_-4*zeta_*tau_*ts_+4*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 68 this->reset();
macht 0:b7a168dd2734 69 /*
macht 0:b7a168dd2734 70 omega = 2.0*pi_*fch_;
macht 0:b7a168dd2734 71 delay_step_ = 2;
macht 0:b7a168dd2734 72 a_.resize(3);
macht 0:b7a168dd2734 73 b_.resize(3);
macht 0:b7a168dd2734 74 u_.resize(3);
macht 0:b7a168dd2734 75 y_.resize(3);
macht 0:b7a168dd2734 76 a0_tmp_ = 4.0+4.0*zeta_*omega*ts_+ts_*ts_*omega*omega;
macht 0:b7a168dd2734 77 b_[0] = 4.0/a0_tmp_;
macht 0:b7a168dd2734 78 b_[1] = -8.0/a0_tmp_;
macht 0:b7a168dd2734 79 b_[2] = b_[0];
macht 0:b7a168dd2734 80 a_[0] = 1.0;
macht 0:b7a168dd2734 81 a_[1] = (-8.0+2.0*ts_*ts_*omega*omega)/a0_tmp_;
macht 0:b7a168dd2734 82 a_[2] = (4.0-4.0*zeta_*omega*ts_+ts_*ts_*omega*omega)/a0_tmp_;
macht 0:b7a168dd2734 83 this->reset();
macht 0:b7a168dd2734 84 */
macht 0:b7a168dd2734 85 break;
macht 0:b7a168dd2734 86
macht 0:b7a168dd2734 87 //Resonance filter.Need to set fc and zeta before calling init().
macht 0:b7a168dd2734 88 case RESONANCE:
macht 0:b7a168dd2734 89 delay_step_ = 2;
macht 0:b7a168dd2734 90 a_.resize(3);
macht 0:b7a168dd2734 91 b_.resize(3);
macht 0:b7a168dd2734 92 u_.resize(3);
macht 0:b7a168dd2734 93 y_.resize(3);
macht 0:b7a168dd2734 94
macht 0:b7a168dd2734 95 tau_=1.0/(2*pi_*fc_);
macht 0:b7a168dd2734 96 a0_tmp = ts_*ts_+4.0*zeta_*tau_*ts_+4.0*tau_*tau_;
macht 0:b7a168dd2734 97 b_[0] = (4.0*zeta_*tau_*ts_)/a0_tmp;
macht 0:b7a168dd2734 98 b_[1] = 0;
macht 0:b7a168dd2734 99 b_[2] = (-4.0*zeta_*tau_*ts_)/a0_tmp;
macht 0:b7a168dd2734 100 a_[0] = 1.0;
macht 0:b7a168dd2734 101 a_[1] = (2.0*ts_*ts_-8*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 102 a_[2] = (ts_*ts_-4*zeta_*tau_*ts_+4*tau_*tau_)/a0_tmp;
macht 0:b7a168dd2734 103 //gain_ = 1.0/a_[0];
macht 0:b7a168dd2734 104 this->reset();
macht 0:b7a168dd2734 105 break;
macht 0:b7a168dd2734 106 case HILBERT:
macht 0:b7a168dd2734 107 float sum_tmp = 0;
macht 0:b7a168dd2734 108 delay_step_ = tap_;
macht 0:b7a168dd2734 109 a_.resize(tap_);
macht 0:b7a168dd2734 110 b_.resize(tap_);
macht 0:b7a168dd2734 111 u_.resize(tap_);
macht 0:b7a168dd2734 112 y_.resize(tap_);
macht 0:b7a168dd2734 113 vectorSetAllToZero(a_);
macht 0:b7a168dd2734 114 for(int i=1;i<tap_;i++){
macht 0:b7a168dd2734 115 b_[i-1] = (2.0/(i-(tap_+1)/2.0)*pi_)*sin((i-(tap_+1)/2)*pi_/2.0)*sin((i-(tap_+1)/2)*pi_/2.0);
macht 0:b7a168dd2734 116 }
macht 0:b7a168dd2734 117 b_[(int)((tap_-1)/2)] = 0;
macht 0:b7a168dd2734 118
macht 0:b7a168dd2734 119 //normalize filter coefficients b
macht 0:b7a168dd2734 120 for(int i=0;i<tap_;i++){
macht 0:b7a168dd2734 121 sum_tmp += b_[i]*b_[i];
macht 0:b7a168dd2734 122 }
macht 0:b7a168dd2734 123 sum_tmp = sqrt(sum_tmp);
macht 0:b7a168dd2734 124 for(int i=0;i<tap_;i++){
macht 0:b7a168dd2734 125 b_[i] =b_[i] / sum_tmp;
macht 0:b7a168dd2734 126 }
macht 0:b7a168dd2734 127 this->reset();
macht 0:b7a168dd2734 128 break;
macht 0:b7a168dd2734 129 default:
macht 0:b7a168dd2734 130 delay_step_ = 0;
macht 0:b7a168dd2734 131 break;
macht 0:b7a168dd2734 132 }
macht 0:b7a168dd2734 133 return delay_step_;
macht 0:b7a168dd2734 134 }
macht 0:b7a168dd2734 135
macht 0:b7a168dd2734 136 //Calcurate new output of digial filter.
macht 0:b7a168dd2734 137 //Using direct type 2 IIRfilter.
macht 0:b7a168dd2734 138 float DigitalFilter::update(float input){
macht 0:b7a168dd2734 139 float output = 0.0;
macht 0:b7a168dd2734 140 vectorRightShift(u_);
macht 0:b7a168dd2734 141 vectorRightShift(y_);
macht 0:b7a168dd2734 142 u_[0] = gain_*input;
macht 0:b7a168dd2734 143 for(int i = 0;i < (int)u_.size();i++){
macht 0:b7a168dd2734 144 output += b_[i] * u_[i];
macht 0:b7a168dd2734 145 }
macht 0:b7a168dd2734 146 for(int i = 1;i < (int)y_.size();i++){
macht 0:b7a168dd2734 147 output -= a_[i] * y_[i];
macht 0:b7a168dd2734 148 }
macht 0:b7a168dd2734 149 y_[0] = output;
macht 0:b7a168dd2734 150 if(filter_type_ == HILBERT){
macht 0:b7a168dd2734 151 output = (float)sqrt(output*output+u_[(int)((tap_-1)/2)]*u_[(int)((tap_-1)/2)]);
macht 0:b7a168dd2734 152 }
macht 0:b7a168dd2734 153 return output;
macht 0:b7a168dd2734 154 }
macht 0:b7a168dd2734 155
macht 0:b7a168dd2734 156 void DigitalFilter::reset(){
macht 0:b7a168dd2734 157 this->vectorSetAllToZero(u_);
macht 0:b7a168dd2734 158 this->vectorSetAllToZero(y_);
macht 0:b7a168dd2734 159 }
macht 0:b7a168dd2734 160
macht 0:b7a168dd2734 161
macht 0:b7a168dd2734 162 //accesor of fc(center frequency)
macht 0:b7a168dd2734 163 void DigitalFilter::set_fc(float fc){
macht 0:b7a168dd2734 164 fc_ = fc;
macht 0:b7a168dd2734 165 }
macht 0:b7a168dd2734 166 //accesor of zeta(decrement)
macht 0:b7a168dd2734 167 void DigitalFilter::set_zeta(float zeta){
macht 0:b7a168dd2734 168 zeta_ = zeta;
macht 0:b7a168dd2734 169 }
macht 0:b7a168dd2734 170
macht 0:b7a168dd2734 171 //accesor of quality factor
macht 0:b7a168dd2734 172 void DigitalFilter::set_Q(float q){
macht 0:b7a168dd2734 173 Q_ = q;
macht 0:b7a168dd2734 174 }
macht 0:b7a168dd2734 175
macht 0:b7a168dd2734 176 //accesor of quality factor
macht 0:b7a168dd2734 177 void DigitalFilter::set_tap(int tap){
macht 0:b7a168dd2734 178 tap_ = tap;
macht 0:b7a168dd2734 179 }
macht 0:b7a168dd2734 180
macht 0:b7a168dd2734 181 //get delay step of designed filter.
macht 0:b7a168dd2734 182 int DigitalFilter::get_delay_step(){
macht 0:b7a168dd2734 183 return delay_step_;
macht 0:b7a168dd2734 184 }
macht 0:b7a168dd2734 185
macht 0:b7a168dd2734 186 //right shift elements of vector
macht 0:b7a168dd2734 187 void DigitalFilter::vectorRightShift(vector<float> &array){
macht 0:b7a168dd2734 188 for(int i = array.size();i > 1;i--){
macht 0:b7a168dd2734 189 array[i-1] = array[i-2];
macht 0:b7a168dd2734 190 }
macht 0:b7a168dd2734 191 array[0] = 0.0;
macht 0:b7a168dd2734 192 }
macht 0:b7a168dd2734 193
macht 0:b7a168dd2734 194 //set 0 to all elements of vector
macht 0:b7a168dd2734 195 void DigitalFilter::vectorSetAllToZero(vector<float> &array){
macht 0:b7a168dd2734 196 for(int i = 0;i < (int)array.size();i++){
macht 0:b7a168dd2734 197 array[i] = 0.0;
macht 0:b7a168dd2734 198 }
macht 0:b7a168dd2734 199 }
macht 0:b7a168dd2734 200
macht 0:b7a168dd2734 201
macht 0:b7a168dd2734 202
macht 0:b7a168dd2734 203
macht 0:b7a168dd2734 204
macht 0:b7a168dd2734 205
macht 0:b7a168dd2734 206
macht 0:b7a168dd2734 207
macht 0:b7a168dd2734 208
macht 0:b7a168dd2734 209