PES2_R2D2.0 / Mbed 2 deprecated Micromouse_alpha_copy_copy

Version12.04.18

Dependencies:   mbed

Fork of Micromouse_alpha_copy_copy by Patrick Ruesi

LowpassFilter.cpp@4:e3f388933954, 2018-04-12 (annotated)

Committer:
ruesipat
Date:
Thu Apr 12 16:14:02 2018 +0000
Revision:
4:e3f388933954
Parent:
2:592f01278db4 
Version12.04.18

Who changed what in which revision?

UserRevisionLine numberNew 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 }