Alexander Lüthard / Mbed 2 deprecated Micromouse

Alle Prinf's auskommentiert

Dependencies:   mbed

Fork of Micromouse by Helvijs Kiselis

LowpassFilter.cpp@19:e2cb5b8a7a29, 2018-05-10 (annotated)

Committer:
luethale
Date:
Thu May 10 16:33:32 2018 +0000
Revision:
19:e2cb5b8a7a29
Parent:
1:2b5f79285a3e 
Alle Printf's auskommentiert

Who changed what in which revision?

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