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