projectR講習2018 / Filter

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

Fork of Filter by Yuki Ueno

Filter.cpp@5:ab6351c18264, 2018-01-16 (annotated)

Committer:
uenono
Date:
Tue Jan 16 15:56:33 2018 +0000
Revision:
5:ab6351c18264
Parent:
4:fc9412c8334e
Child:
6:13ff4bea3c83
????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
uenono 0:17706252d1ed 1 #include "Filter.h"
uenono 0:17706252d1ed 2
uenono 1:637f9a61b133 3 Filter::Filter(double xint_time)
uenono 0:17706252d1ed 4 {
uenono 1:637f9a61b133 5 int_time = xint_time;
uenono 3:987d5d78f863 6 set_t = false;
uenono 0:17706252d1ed 7 }
uenono 0:17706252d1ed 8
uenono 4:fc9412c8334e 9 void Filter::setLowPassPara(double T, double init_data)
uenono 0:17706252d1ed 10 {
uenono 0:17706252d1ed 11 T_LPF = T;
uenono 4:fc9412c8334e 12 preOutput = init_data;
uenono 3:987d5d78f863 13 set_t = true;
uenono 0:17706252d1ed 14 }
uenono 0:17706252d1ed 15
uenono 0:17706252d1ed 16 double Filter::LowPassFilter(double input)
uenono 0:17706252d1ed 17 {
uenono 3:987d5d78f863 18 //static double preOutput = input;
uenono 3:987d5d78f863 19 if(!set_t) {
uenono 3:987d5d78f863 20 return input;
uenono 3:987d5d78f863 21 } else {
uenono 3:987d5d78f863 22 double Output = (int_time * input + T_LPF * preOutput)/(T_LPF + int_time);
uenono 3:987d5d78f863 23 preOutput = Output;
uenono 3:987d5d78f863 24 return Output;
uenono 3:987d5d78f863 25 }
uenono 0:17706252d1ed 26 }
uenono 0:17706252d1ed 27
uenono 5:ab6351c18264 28 void Filter::setNotchPara(double Omega, double int_data)
uenono 0:17706252d1ed 29 {
uenono 0:17706252d1ed 30 // 落としたい角周波数[rad/s]をOm_nに入れる
uenono 0:17706252d1ed 31 Om_n = Omega;
uenono 0:17706252d1ed 32 sq_Om = pow(Om_n, 2.0); // Om_nの2乗
uenono 2:a842c1a33e4f 33 sq_dt = pow(int_time, 2.0); // dtの2乗
uenono 5:ab6351c18264 34
uenono 5:ab6351c18264 35 n_preOutput[0] = int_data;
uenono 5:ab6351c18264 36 n_preOutput[1] = int_data;
uenono 5:ab6351c18264 37
uenono 5:ab6351c18264 38 n_preInput[0] = int_data;
uenono 5:ab6351c18264 39 n_preInput[1] = int_data;
uenono 0:17706252d1ed 40 }
uenono 0:17706252d1ed 41
uenono 0:17706252d1ed 42 double Filter::NotchFilter(double input)
uenono 0:17706252d1ed 43 {
uenono 5:ab6351c18264 44 double Output = (2*(1 + Om_n * int_time) * n_preOutput[0]-n_preOutput[1] + (1 + sq_Om * sq_dt) * input -2 * n_preInput[0] + n_preInput[1]) / (1 + 2 * Om_n * int_time + sq_Om * sq_dt);
uenono 3:987d5d78f863 45
uenono 5:ab6351c18264 46 n_preInput[1] = n_preInput[0];
uenono 5:ab6351c18264 47 n_preInput[0] = input;
uenono 3:987d5d78f863 48
uenono 5:ab6351c18264 49 n_preOutput[1] = n_preOutput[0];
uenono 5:ab6351c18264 50 n_preOutput[0] = Output;
uenono 3:987d5d78f863 51
uenono 0:17706252d1ed 52 return Output;
uenono 0:17706252d1ed 53 }