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