1次遅れと2次遅れ,ノッチフィルターを実装
Fork of Filter by
Filter.cpp
- Committer:
- uenono
- Date:
- 2018-01-11
- Revision:
- 2:a842c1a33e4f
- Parent:
- 1:637f9a61b133
- Child:
- 3:987d5d78f863
File content as of revision 2:a842c1a33e4f:
#include "Filter.h" Filter::Filter(double xint_time) { int_time = xint_time; } void Filter::setLowPassPara(double T) { T_LPF = T; } double Filter::LowPassFilter(double input) { static double preOutput = 0.0; double Output = (int_time * input + T_LPF * preOutput)/(T_LPF + int_time); preOutput = Output; return Output; } void Filter::setNotchPara(double Omega) { // 落としたい角周波数[rad/s]をOm_nに入れる Om_n = Omega; sq_Om = pow(Om_n, 2.0); // Om_nの2乗 sq_dt = pow(int_time, 2.0); // dtの2乗 } double Filter::NotchFilter(double input) { static double preOutput[2] = {0.0, 0.0}; static double preInput[2] = {0.0, 0.0}; 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); preInput[1] = preInput[0]; preInput[0] = input; preOutput[1] = preOutput[0]; preOutput[0] = Output; return Output; }