LowpassFilter.cpp

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