TeamSurface / Library

P2 halbfertig

Fork of Library by St Knz

LowpassFilter.cpp@2:6dd39662e6e5, 2018-03-10 (annotated)

Committer:
kueenste
Date:
Sat Mar 10 13:47:28 2018 +0000
Revision:
2:6dd39662e6e5
Parent:
0:bb408887ab78 
funktioniert soso

Who changed what in which revision?

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