LowpassFilter.cpp

00001 #include "LowpassFilter.h"
00002 
00003 //E. Hess
00004 //LowpassFilter.cpp
00005 
00006 using namespace std;
00007 
00008 /**
00009  * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s].
00010  */
00011 LowpassFilter::LowpassFilter() {
00012     
00013     period = 1.0f;
00014     frequency = 1000.0f;
00015     
00016     a11 = (1.0f+frequency*period)*exp(-frequency*period);
00017     a12 = period*exp(-frequency*period);
00018     a21 = -frequency*frequency*period*exp(-frequency*period);
00019     a22 = (1.0f-frequency*period)*exp(-frequency*period);
00020     b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
00021     b2 = period*exp(-frequency*period);
00022     
00023     x1 = 0.0f;
00024     x2 = 0.0f;
00025 }
00026 
00027 /**
00028  * Deletes the LowpassFilter object.
00029  */
00030 LowpassFilter::~LowpassFilter() {}
00031 
00032 /**
00033  * Resets the filtered value to zero.
00034  */
00035 void LowpassFilter::reset() {
00036     
00037     x1 = 0.0f;
00038     x2 = 0.0f;
00039 }
00040 
00041 /**
00042  * Resets the filtered value to a given value.
00043  * @param value the value to reset the filter to.
00044  */
00045 void LowpassFilter::reset(float value) {
00046     
00047     x1 = value/frequency/frequency;
00048     x2 = (x1-a11*x1-b1*value)/a12;
00049 }
00050 
00051 /**
00052  * Sets the sampling period of the filter.
00053  * This is typically the sampling period of the realtime thread of a controller that uses this filter.
00054  * @param the sampling period, given in [s].
00055  */
00056 void LowpassFilter::setPeriod(float period) {
00057     
00058     this->period = period;
00059     
00060     a11 = (1.0f+frequency*period)*exp(-frequency*period);
00061     a12 = period*exp(-frequency*period);
00062     a21 = -frequency*frequency*period*exp(-frequency*period);
00063     a22 = (1.0f-frequency*period)*exp(-frequency*period);
00064     b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
00065     b2 = period*exp(-frequency*period);
00066 }
00067 
00068 /**
00069  * Sets the corner frequency of this filter.
00070  * @param frequency the corner frequency of the filter in [rad/s].
00071  */
00072 void LowpassFilter::setFrequency(float frequency) {
00073     
00074     this->frequency = frequency;
00075     
00076     a11 = (1.0f+frequency*period)*exp(-frequency*period);
00077     a12 = period*exp(-frequency*period);
00078     a21 = -frequency*frequency*period*exp(-frequency*period);
00079     a22 = (1.0f-frequency*period)*exp(-frequency*period);
00080     b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
00081     b2 = period*exp(-frequency*period);
00082 }
00083 
00084 /**
00085  * Gets the current corner frequency of this filter.
00086  * @return the current corner frequency in [rad/s].
00087  */
00088 float LowpassFilter::getFrequency() {
00089     
00090     return frequency;
00091 }
00092 
00093 /**
00094  * Filters a value.
00095  * @param value the original unfiltered value.
00096  * @return the filtered value.
00097  */
00098 float LowpassFilter::filter(float value) {
00099 
00100     float x1old = x1;
00101     float x2old = x2;
00102     
00103     x1 = a11*x1old+a12*x2old+b1*value;
00104     x2 = a21*x1old+a22*x2old+b2*value;
00105     
00106     return frequency*frequency*x1;
00107 }