Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include <lpfilter.h>
00017
00018 LPFilter::LPFilter(double deltaT, double cutoffFrequency, int numElements):
00019 initialized(false),
00020 noElements(0),
00021 omega_a(0.0),
00022 a0(0.0),
00023 a1(0.0),
00024 a2(0.0),
00025 b1(0.0),
00026 b2(0.0)
00027 {
00028 initialized = true;
00029 if(numElements<=0)
00030 {
00031 ROS_ERROR_STREAM("LPFilter was passed invalid number of elements. Not filtering.");
00032 initialized = false;
00033 }
00034 else
00035 {
00036 noElements = numElements;
00037 }
00038 if(deltaT<=0)
00039 {
00040 ROS_ERROR_STREAM("LPFilter was passed invalid deltaT. Not Filtering.");
00041 initialized = false;
00042 }
00043 if(cutoffFrequency <=0)
00044 {
00045 ROS_ERROR_STREAM("LPFilter was passed invalid cuttoffFrequency. Not Filtering.");
00046 initialized = false;
00047 }
00048 else
00049 {
00050 cutoffFrequency *= (2*M_PI);
00051 omega_a = tan(cutoffFrequency*deltaT/2.0);
00052 double den = omega_a*omega_a+sqrt(2.0*omega_a+1.0);
00053 a0 = (omega_a*omega_a)/den;
00054 a1 = (2.0*omega_a*omega_a)/den;
00055 a2 = a0;
00056 b1 = 2.0*(omega_a*omega_a-1.0)/den;
00057 b2 = (omega_a*omega_a-sqrt(2.0)*omega_a+1)/den;
00058 in1.resize(noElements);
00059 in2.resize(noElements);
00060 out1.resize(noElements);
00061 out2.resize(noElements);
00062 ROS_INFO_STREAM("cutoffFrequency: " << cutoffFrequency << ". omega_a: " << omega_a << ". den: " << den << ". a0: " << a0 << ". a1: " << a1 << ". a2: " << a2 << ". b1: " << b1 << ". b2: " << b2);
00063 }
00064 }
00065
00066 bool LPFilter::update(std::vector<double> input, std::vector<double>& output)
00067 {
00068 if(!initialized)
00069 {
00070 ROS_ERROR_STREAM("LPFilter was not initialized correctly. Not filtering data.");
00071 return false;
00072 }
00073 if(input.size() != in1.size() || output.size() != out1.size())
00074 {
00075 ROS_ERROR_STREAM("LPFilter incorrect input or output size");
00076 return false;
00077 }
00078 for(int i=0; i<noElements; i++)
00079 {
00080 output.at(i) = a0*input.at(i) + a1*in1.at(i) + a2*in2.at(i) - b1*out1.at(i) - b2*out2.at(i);
00081 out2.at(i) = out1.at(i);
00082 out1.at(i) = output.at(i);
00083 in2.at(i) = in1.at(i);
00084 in1.at(i) = input.at(i);
00085 }
00086 return true;
00087 }