projectR講習2018 / Filter

1次遅れと2次遅れ，ノッチフィルターを実装

Fork of Filter by Yuki Ueno

Filter.cpp@0:17706252d1ed, 2018-01-11 (annotated)

Committer:
uenono
Date:
Thu Jan 11 06:44:59 2018 +0000
Revision:
0:17706252d1ed
Child:
1:637f9a61b133
?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uenono 0:17706252d1ed 1 #include "Filter.h"
uenono 0:17706252d1ed 2 #include "define.h"
uenono 0:17706252d1ed 3
uenono 0:17706252d1ed 4 Filter::Filter()
uenono 0:17706252d1ed 5 {
uenono 0:17706252d1ed 6 }
uenono 0:17706252d1ed 7
uenono 0:17706252d1ed 8 void Filter::setLowPassPara(double T)
uenono 0:17706252d1ed 9 {
uenono 0:17706252d1ed 10 T_LPF = T;
uenono 0:17706252d1ed 11 }
uenono 0:17706252d1ed 12
uenono 0:17706252d1ed 13 double Filter::LowPassFilter(double input)
uenono 0:17706252d1ed 14 {
uenono 0:17706252d1ed 15 static double preOutput = 0.0;
uenono 0:17706252d1ed 16
uenono 0:17706252d1ed 17 double Output = (INT_TIME * input + T_LPF * preOutput)/(T_LPF + INT_TIME);
uenono 0:17706252d1ed 18
uenono 0:17706252d1ed 19 preOutput = Output;
uenono 0:17706252d1ed 20
uenono 0:17706252d1ed 21 return Output;
uenono 0:17706252d1ed 22 }
uenono 0:17706252d1ed 23
uenono 0:17706252d1ed 24 void Filter::setNotchPara(double Omega)
uenono 0:17706252d1ed 25 {
uenono 0:17706252d1ed 26 // 落としたい角周波数[rad/s]をOm_nに入れる
uenono 0:17706252d1ed 27 Om_n = Omega;
uenono 0:17706252d1ed 28 sq_Om = pow(Om_n, 2.0); // Om_nの2乗
uenono 0:17706252d1ed 29 sq_dt = pow(INT_TIME, 2.0); // dtの2乗
uenono 0:17706252d1ed 30
uenono 0:17706252d1ed 31 }
uenono 0:17706252d1ed 32
uenono 0:17706252d1ed 33 double Filter::NotchFilter(double input)
uenono 0:17706252d1ed 34 {
uenono 0:17706252d1ed 35 static double preOutput[2] = {0.0, 0.0};
uenono 0:17706252d1ed 36 static double preInput[2] = {0.0, 0.0};
uenono 0:17706252d1ed 37
uenono 0:17706252d1ed 38 double Output = (2*(1 + Om_n * INT_TIME) * preOutput[0]-preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * preInput[0] + preInput[1]) / (1 + 2 * Om_n * INT_TIME + sq_Om * sq_dt);
uenono 0:17706252d1ed 39
uenono 0:17706252d1ed 40 preInput[1] = preInput[0];
uenono 0:17706252d1ed 41 preInput[0] = input;
uenono 0:17706252d1ed 42
uenono 0:17706252d1ed 43 preOutput[1] = preOutput[0];
uenono 0:17706252d1ed 44 preOutput[0] = Output;
uenono 0:17706252d1ed 45
uenono 0:17706252d1ed 46 return Output;
uenono 0:17706252d1ed 47 }