projectR講習2018 / Filter

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

Fork of Filter by Yuki Ueno

Filter.cpp@6:13ff4bea3c83, 2018-03-22 (annotated)

Committer:
uenono
Date:
Thu Mar 22 12:36:55 2018 +0000
Revision:
6:13ff4bea3c83
Parent:
5:ab6351c18264 
second-order-lag???

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 6:13ff4bea3c83 28 void Filter::setSecondOrderPara(double xOmega, double xDzeta, double init_data)
uenono 6:13ff4bea3c83 29 {
uenono 6:13ff4bea3c83 30 omega = xOmega;
uenono 6:13ff4bea3c83 31 dzeta = xDzeta;
uenono 6:13ff4bea3c83 32 prev_output2 = prev_output1 = init_data;
uenono 6:13ff4bea3c83 33
uenono 6:13ff4bea3c83 34 set_secorder = true;
uenono 6:13ff4bea3c83 35 }
uenono 6:13ff4bea3c83 36
uenono 6:13ff4bea3c83 37 double Filter::SecondOrderLag(double input)
uenono 6:13ff4bea3c83 38 {
uenono 6:13ff4bea3c83 39 if(!set_secorder) {
uenono 6:13ff4bea3c83 40 return input;
uenono 6:13ff4bea3c83 41 } else {
uenono 6:13ff4bea3c83 42 double output = (2*prev_output1*(1+dzeta*omega*int_time) - prev_output2 + pow(omega, 2.0)*pow(int_time, 2.0)*input)/(1 + 2*dzeta*omega*int_time + pow(omega, 2.0)*pow(int_time, 2.0));
uenono 6:13ff4bea3c83 43 prev_output2 = prev_output1;
uenono 6:13ff4bea3c83 44 prev_output1 = output;
uenono 6:13ff4bea3c83 45 return output;
uenono 6:13ff4bea3c83 46 }
uenono 6:13ff4bea3c83 47 }
uenono 6:13ff4bea3c83 48
uenono 5:ab6351c18264 49 void Filter::setNotchPara(double Omega, double int_data)
uenono 0:17706252d1ed 50 {
uenono 0:17706252d1ed 51 // 落としたい角周波数[rad/s]をOm_nに入れる
uenono 0:17706252d1ed 52 Om_n = Omega;
uenono 0:17706252d1ed 53 sq_Om = pow(Om_n, 2.0); // Om_nの2乗
uenono 2:a842c1a33e4f 54 sq_dt = pow(int_time, 2.0); // dtの2乗
uenono 6:13ff4bea3c83 55
uenono 5:ab6351c18264 56 n_preOutput[0] = int_data;
uenono 5:ab6351c18264 57 n_preOutput[1] = int_data;
uenono 6:13ff4bea3c83 58
uenono 5:ab6351c18264 59 n_preInput[0] = int_data;
uenono 5:ab6351c18264 60 n_preInput[1] = int_data;
uenono 0:17706252d1ed 61 }
uenono 0:17706252d1ed 62
uenono 0:17706252d1ed 63 double Filter::NotchFilter(double input)
uenono 0:17706252d1ed 64 {
uenono 5:ab6351c18264 65 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 66
uenono 5:ab6351c18264 67 n_preInput[1] = n_preInput[0];
uenono 5:ab6351c18264 68 n_preInput[0] = input;
uenono 3:987d5d78f863 69
uenono 5:ab6351c18264 70 n_preOutput[1] = n_preOutput[0];
uenono 5:ab6351c18264 71 n_preOutput[0] = Output;
uenono 3:987d5d78f863 72
uenono 0:17706252d1ed 73 return Output;
uenono 0:17706252d1ed 74 }