![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
hallo
Fork of PES1 by
EncoderCounter/LowpassFilter.cpp@21:69ee872b8ee9, 2017-05-16 (annotated)
- Committer:
- itslinear
- Date:
- Tue May 16 16:09:08 2017 +0000
- Revision:
- 21:69ee872b8ee9
- Parent:
- 18:3ee1b02ed3aa
sieg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
itslinear | 18:3ee1b02ed3aa | 1 | /* |
itslinear | 18:3ee1b02ed3aa | 2 | * LowpassFilter.cpp |
itslinear | 18:3ee1b02ed3aa | 3 | * Copyright (c) 2016, ZHAW |
itslinear | 18:3ee1b02ed3aa | 4 | * All rights reserved. |
itslinear | 18:3ee1b02ed3aa | 5 | */ |
itslinear | 18:3ee1b02ed3aa | 6 | |
itslinear | 18:3ee1b02ed3aa | 7 | #include "LowpassFilter.h" |
itslinear | 18:3ee1b02ed3aa | 8 | |
itslinear | 18:3ee1b02ed3aa | 9 | using namespace std; |
itslinear | 18:3ee1b02ed3aa | 10 | |
itslinear | 18:3ee1b02ed3aa | 11 | /** |
itslinear | 18:3ee1b02ed3aa | 12 | * Creates a LowpassFilter object with a default corner frequency of 1000 [rad/s]. |
itslinear | 18:3ee1b02ed3aa | 13 | */ |
itslinear | 18:3ee1b02ed3aa | 14 | LowpassFilter::LowpassFilter() { |
itslinear | 18:3ee1b02ed3aa | 15 | |
itslinear | 18:3ee1b02ed3aa | 16 | period = 1.0f; |
itslinear | 18:3ee1b02ed3aa | 17 | frequency = 1000.0f; |
itslinear | 18:3ee1b02ed3aa | 18 | |
itslinear | 18:3ee1b02ed3aa | 19 | a11 = (1.0f+frequency*period)*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 20 | a12 = period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 21 | a21 = -frequency*frequency*period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 22 | a22 = (1.0f-frequency*period)*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 23 | b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; |
itslinear | 18:3ee1b02ed3aa | 24 | b2 = period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 25 | |
itslinear | 18:3ee1b02ed3aa | 26 | x1 = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 27 | x2 = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 28 | } |
itslinear | 18:3ee1b02ed3aa | 29 | |
itslinear | 18:3ee1b02ed3aa | 30 | /** |
itslinear | 18:3ee1b02ed3aa | 31 | * Deletes the LowpassFilter object. |
itslinear | 18:3ee1b02ed3aa | 32 | */ |
itslinear | 18:3ee1b02ed3aa | 33 | LowpassFilter::~LowpassFilter() {} |
itslinear | 18:3ee1b02ed3aa | 34 | |
itslinear | 18:3ee1b02ed3aa | 35 | /** |
itslinear | 18:3ee1b02ed3aa | 36 | * Resets the filtered value to zero. |
itslinear | 18:3ee1b02ed3aa | 37 | */ |
itslinear | 18:3ee1b02ed3aa | 38 | void LowpassFilter::reset() { |
itslinear | 18:3ee1b02ed3aa | 39 | |
itslinear | 18:3ee1b02ed3aa | 40 | x1 = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 41 | x2 = 0.0f; |
itslinear | 18:3ee1b02ed3aa | 42 | } |
itslinear | 18:3ee1b02ed3aa | 43 | |
itslinear | 18:3ee1b02ed3aa | 44 | /** |
itslinear | 18:3ee1b02ed3aa | 45 | * Resets the filtered value to a given value. |
itslinear | 18:3ee1b02ed3aa | 46 | * @param value the value to reset the filter to. |
itslinear | 18:3ee1b02ed3aa | 47 | */ |
itslinear | 18:3ee1b02ed3aa | 48 | void LowpassFilter::reset(float value) { |
itslinear | 18:3ee1b02ed3aa | 49 | |
itslinear | 18:3ee1b02ed3aa | 50 | x1 = value/frequency/frequency; |
itslinear | 18:3ee1b02ed3aa | 51 | x2 = (x1-a11*x1-b1*value)/a12; |
itslinear | 18:3ee1b02ed3aa | 52 | } |
itslinear | 18:3ee1b02ed3aa | 53 | |
itslinear | 18:3ee1b02ed3aa | 54 | /** |
itslinear | 18:3ee1b02ed3aa | 55 | * Sets the sampling period of the filter. |
itslinear | 18:3ee1b02ed3aa | 56 | * This is typically the sampling period of the realtime thread of a controller that uses this filter. |
itslinear | 18:3ee1b02ed3aa | 57 | * @param the sampling period, given in [s]. |
itslinear | 18:3ee1b02ed3aa | 58 | */ |
itslinear | 18:3ee1b02ed3aa | 59 | void LowpassFilter::setPeriod(float period) { |
itslinear | 18:3ee1b02ed3aa | 60 | |
itslinear | 18:3ee1b02ed3aa | 61 | this->period = period; |
itslinear | 18:3ee1b02ed3aa | 62 | |
itslinear | 18:3ee1b02ed3aa | 63 | a11 = (1.0f+frequency*period)*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 64 | a12 = period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 65 | a21 = -frequency*frequency*period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 66 | a22 = (1.0f-frequency*period)*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 67 | b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; |
itslinear | 18:3ee1b02ed3aa | 68 | b2 = period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 69 | } |
itslinear | 18:3ee1b02ed3aa | 70 | |
itslinear | 18:3ee1b02ed3aa | 71 | /** |
itslinear | 18:3ee1b02ed3aa | 72 | * Sets the corner frequency of this filter. |
itslinear | 18:3ee1b02ed3aa | 73 | * @param frequency the corner frequency of the filter in [rad/s]. |
itslinear | 18:3ee1b02ed3aa | 74 | */ |
itslinear | 18:3ee1b02ed3aa | 75 | void LowpassFilter::setFrequency(float frequency) { |
itslinear | 18:3ee1b02ed3aa | 76 | |
itslinear | 18:3ee1b02ed3aa | 77 | this->frequency = frequency; |
itslinear | 18:3ee1b02ed3aa | 78 | |
itslinear | 18:3ee1b02ed3aa | 79 | a11 = (1.0f+frequency*period)*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 80 | a12 = period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 81 | a21 = -frequency*frequency*period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 82 | a22 = (1.0f-frequency*period)*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 83 | b1 = (1.0f-(1.0f+frequency*period)*exp(-frequency*period))/frequency/frequency; |
itslinear | 18:3ee1b02ed3aa | 84 | b2 = period*exp(-frequency*period); |
itslinear | 18:3ee1b02ed3aa | 85 | } |
itslinear | 18:3ee1b02ed3aa | 86 | |
itslinear | 18:3ee1b02ed3aa | 87 | /** |
itslinear | 18:3ee1b02ed3aa | 88 | * Gets the current corner frequency of this filter. |
itslinear | 18:3ee1b02ed3aa | 89 | * @return the current corner frequency in [rad/s]. |
itslinear | 18:3ee1b02ed3aa | 90 | */ |
itslinear | 18:3ee1b02ed3aa | 91 | float LowpassFilter::getFrequency() { |
itslinear | 18:3ee1b02ed3aa | 92 | |
itslinear | 18:3ee1b02ed3aa | 93 | return frequency; |
itslinear | 18:3ee1b02ed3aa | 94 | } |
itslinear | 18:3ee1b02ed3aa | 95 | |
itslinear | 18:3ee1b02ed3aa | 96 | /** |
itslinear | 18:3ee1b02ed3aa | 97 | * Filters a value. |
itslinear | 18:3ee1b02ed3aa | 98 | * @param value the original unfiltered value. |
itslinear | 18:3ee1b02ed3aa | 99 | * @return the filtered value. |
itslinear | 18:3ee1b02ed3aa | 100 | */ |
itslinear | 18:3ee1b02ed3aa | 101 | float LowpassFilter::filter(float value) { |
itslinear | 18:3ee1b02ed3aa | 102 | |
itslinear | 18:3ee1b02ed3aa | 103 | float x1old = x1; |
itslinear | 18:3ee1b02ed3aa | 104 | float x2old = x2; |
itslinear | 18:3ee1b02ed3aa | 105 | |
itslinear | 18:3ee1b02ed3aa | 106 | x1 = a11*x1old+a12*x2old+b1*value; |
itslinear | 18:3ee1b02ed3aa | 107 | x2 = a21*x1old+a22*x2old+b2*value; |
itslinear | 18:3ee1b02ed3aa | 108 | |
itslinear | 18:3ee1b02ed3aa | 109 | return frequency*frequency*x1; |
itslinear | 18:3ee1b02ed3aa | 110 | } |