Claudio Citterio / RomeLib

ggg

LowpassFilter.cpp@0:df3d4b033d11, 2019-02-27 (annotated)

Committer:
cittecla
Date:
Wed Feb 27 15:14:27 2019 +0000
Revision:
0:df3d4b033d11
P1 finished

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 0:df3d4b033d11 1 /*
cittecla 0:df3d4b033d11 2 * LowpassFilter.cpp
cittecla 0:df3d4b033d11 3 * Copyright (c) 2019, ZHAW
cittecla 0:df3d4b033d11 4 * All rights reserved.
cittecla 0:df3d4b033d11 5 */
cittecla 0:df3d4b033d11 6
cittecla 0:df3d4b033d11 7 #include <cmath>
cittecla 0:df3d4b033d11 8 #include "LowpassFilter.h"
cittecla 0:df3d4b033d11 9
cittecla 0:df3d4b033d11 10 using namespace std;
cittecla 0:df3d4b033d11 11
cittecla 0:df3d4b033d11 12 /**
cittecla 0:df3d4b033d11 13 * Creates a LowpassFilter object with a default cutoff frequency of 1000 [rad/s].
cittecla 0:df3d4b033d11 14 */
cittecla 0:df3d4b033d11 15 LowpassFilter::LowpassFilter() {
cittecla 0:df3d4b033d11 16
cittecla 0:df3d4b033d11 17 period = 1.0f;
cittecla 0:df3d4b033d11 18 frequency = 1000.0f;
cittecla 0:df3d4b033d11 19
cittecla 0:df3d4b033d11 20 a11 = (1.0f+frequency*period)*exp(-frequency*period);
cittecla 0:df3d4b033d11 21 a12 = period*exp(-frequency*period);
cittecla 0:df3d4b033d11 22 a21 = -frequency*frequency*period*exp(-frequency*period);
cittecla 0:df3d4b033d11 23 a22 = (1.0f-frequency*period)*exp(-frequency*period);
cittecla 0:df3d4b033d11 24 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
cittecla 0:df3d4b033d11 25 b2 = period*exp(-frequency*period);
cittecla 0:df3d4b033d11 26
cittecla 0:df3d4b033d11 27 x1 = 0.0f;
cittecla 0:df3d4b033d11 28 x2 = 0.0f;
cittecla 0:df3d4b033d11 29 }
cittecla 0:df3d4b033d11 30
cittecla 0:df3d4b033d11 31 /**
cittecla 0:df3d4b033d11 32 * Deletes the LowpassFilter object.
cittecla 0:df3d4b033d11 33 */
cittecla 0:df3d4b033d11 34 LowpassFilter::~LowpassFilter() {}
cittecla 0:df3d4b033d11 35
cittecla 0:df3d4b033d11 36 /**
cittecla 0:df3d4b033d11 37 * Resets the filtered value to zero.
cittecla 0:df3d4b033d11 38 */
cittecla 0:df3d4b033d11 39 void LowpassFilter::reset() {
cittecla 0:df3d4b033d11 40
cittecla 0:df3d4b033d11 41 x1 = 0.0f;
cittecla 0:df3d4b033d11 42 x2 = 0.0f;
cittecla 0:df3d4b033d11 43 }
cittecla 0:df3d4b033d11 44
cittecla 0:df3d4b033d11 45 /**
cittecla 0:df3d4b033d11 46 * Resets the filtered value to a given value.
cittecla 0:df3d4b033d11 47 * @param value the value to reset the filter to.
cittecla 0:df3d4b033d11 48 */
cittecla 0:df3d4b033d11 49 void LowpassFilter::reset(float value) {
cittecla 0:df3d4b033d11 50
cittecla 0:df3d4b033d11 51 x1 = value/frequency/frequency;
cittecla 0:df3d4b033d11 52 x2 = (x1-a11*x1-b1*value)/a12;
cittecla 0:df3d4b033d11 53 }
cittecla 0:df3d4b033d11 54
cittecla 0:df3d4b033d11 55 /**
cittecla 0:df3d4b033d11 56 * Sets the sampling period of the filter.
cittecla 0:df3d4b033d11 57 * This is typically the sampling period of the periodic task of a controller that uses this filter.
cittecla 0:df3d4b033d11 58 * @param the sampling period, given in [s].
cittecla 0:df3d4b033d11 59 */
cittecla 0:df3d4b033d11 60 void LowpassFilter::setPeriod(float period) {
cittecla 0:df3d4b033d11 61
cittecla 0:df3d4b033d11 62 this->period = period;
cittecla 0:df3d4b033d11 63
cittecla 0:df3d4b033d11 64 a11 = (1.0f+frequency*period)*exp(-frequency*period);
cittecla 0:df3d4b033d11 65 a12 = period*exp(-frequency*period);
cittecla 0:df3d4b033d11 66 a21 = -frequency*frequency*period*exp(-frequency*period);
cittecla 0:df3d4b033d11 67 a22 = (1.0f-frequency*period)*exp(-frequency*period);
cittecla 0:df3d4b033d11 68 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
cittecla 0:df3d4b033d11 69 b2 = period*exp(-frequency*period);
cittecla 0:df3d4b033d11 70 }
cittecla 0:df3d4b033d11 71
cittecla 0:df3d4b033d11 72 /**
cittecla 0:df3d4b033d11 73 * Sets the cutoff frequency of this filter.
cittecla 0:df3d4b033d11 74 * @param frequency the cutoff frequency of the filter in [rad/s].
cittecla 0:df3d4b033d11 75 */
cittecla 0:df3d4b033d11 76 void LowpassFilter::setFrequency(float frequency) {
cittecla 0:df3d4b033d11 77
cittecla 0:df3d4b033d11 78 this->frequency = frequency;
cittecla 0:df3d4b033d11 79
cittecla 0:df3d4b033d11 80 a11 = (1.0f+frequency*period)*exp(-frequency*period);
cittecla 0:df3d4b033d11 81 a12 = period*exp(-frequency*period);
cittecla 0:df3d4b033d11 82 a21 = -frequency*frequency*period*exp(-frequency*period);
cittecla 0:df3d4b033d11 83 a22 = (1.0f-frequency*period)*exp(-frequency*period);
cittecla 0:df3d4b033d11 84 b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency;
cittecla 0:df3d4b033d11 85 b2 = period*exp(-frequency*period);
cittecla 0:df3d4b033d11 86 }
cittecla 0:df3d4b033d11 87
cittecla 0:df3d4b033d11 88 /**
cittecla 0:df3d4b033d11 89 * Gets the current cutoff frequency of this filter.
cittecla 0:df3d4b033d11 90 * @return the current cutoff frequency in [rad/s].
cittecla 0:df3d4b033d11 91 */
cittecla 0:df3d4b033d11 92 float LowpassFilter::getFrequency() {
cittecla 0:df3d4b033d11 93
cittecla 0:df3d4b033d11 94 return frequency;
cittecla 0:df3d4b033d11 95 }
cittecla 0:df3d4b033d11 96
cittecla 0:df3d4b033d11 97 /**
cittecla 0:df3d4b033d11 98 * Filters a value.
cittecla 0:df3d4b033d11 99 * @param value the original unfiltered value.
cittecla 0:df3d4b033d11 100 * @return the filtered value.
cittecla 0:df3d4b033d11 101 */
cittecla 0:df3d4b033d11 102 float LowpassFilter::filter(float value) {
cittecla 0:df3d4b033d11 103
cittecla 0:df3d4b033d11 104 float x1old = x1;
cittecla 0:df3d4b033d11 105 float x2old = x2;
cittecla 0:df3d4b033d11 106
cittecla 0:df3d4b033d11 107 x1 = a11*x1old+a12*x2old+b1*value;
cittecla 0:df3d4b033d11 108 x2 = a21*x1old+a22*x2old+b2*value;
cittecla 0:df3d4b033d11 109
cittecla 0:df3d4b033d11 110 return frequency*frequency*x1;
cittecla 0:df3d4b033d11 111 }