Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Filter.h@46:fd5a62296b12, 2015-05-27 (annotated)

Committer:
pvaibhav
Date:
Wed May 27 13:01:43 2015 +0000
Revision:
46:fd5a62296b12
Parent:
40:8e852115fe55 
Code reformatted

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pvaibhav 40:8e852115fe55 1 #ifndef _H_FILTER_H
pvaibhav 40:8e852115fe55 2 #define _H_FILTER_H
pvaibhav 40:8e852115fe55 3
pvaibhav 40:8e852115fe55 4 #include "Utils.h"
pvaibhav 40:8e852115fe55 5
pvaibhav 40:8e852115fe55 6 /// \brief Low-pass filters an incoming stream of numbers.
pvaibhav 40:8e852115fe55 7 /// Implements a 1st-order IIR low-pass filter. The coefficient can be used to specify
pvaibhav 40:8e852115fe55 8 /// how much to filter.
pvaibhav 40:8e852115fe55 9 class LowPassFilter
pvaibhav 40:8e852115fe55 10 {
pvaibhav 40:8e852115fe55 11 protected:
pvaibhav 40:8e852115fe55 12 float y;
pvaibhav 40:8e852115fe55 13 float const alpha;
pvaibhav 46:fd5a62296b12 14
pvaibhav 40:8e852115fe55 15 public:
pvaibhav 40:8e852115fe55 16 /// Construct the filter with given coefficient.
pvaibhav 40:8e852115fe55 17 /// @param a filter coefficient (0.0 to 1.0), higher values result in more filtering.
pvaibhav 40:8e852115fe55 18 LowPassFilter(float const a) : y(0), alpha(a) {}
pvaibhav 46:fd5a62296b12 19
pvaibhav 40:8e852115fe55 20 /// Main processing function, passes given value to filter and returns the filter's output.
pvaibhav 40:8e852115fe55 21 /// Call repeatedly to filter an incoming stream of numbers.
pvaibhav 40:8e852115fe55 22 /// @param x one value to send to filter as input
pvaibhav 40:8e852115fe55 23 /// @return filtered output
pvaibhav 46:fd5a62296b12 24 float filter(float const x) {
pvaibhav 40:8e852115fe55 25 y = x + alpha * (y - x);
pvaibhav 40:8e852115fe55 26 return y;
pvaibhav 40:8e852115fe55 27 }
pvaibhav 46:fd5a62296b12 28
pvaibhav 40:8e852115fe55 29 /// Resets the internal state of the filter. Use to start filtering a new stream.
pvaibhav 46:fd5a62296b12 30 void reset() {
pvaibhav 40:8e852115fe55 31 y = 0;
pvaibhav 40:8e852115fe55 32 }
pvaibhav 40:8e852115fe55 33 };
pvaibhav 40:8e852115fe55 34
pvaibhav 40:8e852115fe55 35 /// 1st order high-pass filter.
pvaibhav 40:8e852115fe55 36 /// Used in exactly the same way as LowPassFilter, except that it's a high-pass. @see LowPassFilter
pvaibhav 40:8e852115fe55 37 class HighPassFilter
pvaibhav 40:8e852115fe55 38 {
pvaibhav 40:8e852115fe55 39 protected:
pvaibhav 40:8e852115fe55 40 float y;
pvaibhav 40:8e852115fe55 41 float x_1;
pvaibhav 40:8e852115fe55 42 float const alpha;
pvaibhav 40:8e852115fe55 43
pvaibhav 40:8e852115fe55 44 public:
pvaibhav 40:8e852115fe55 45 HighPassFilter(float const a) : y(0), x_1(0), alpha(1 - a) {}
pvaibhav 46:fd5a62296b12 46 float filter(float const x) {
pvaibhav 40:8e852115fe55 47 y = alpha * (y + x - x_1);
pvaibhav 40:8e852115fe55 48 x_1 = x;
pvaibhav 40:8e852115fe55 49 return y;
pvaibhav 40:8e852115fe55 50 }
pvaibhav 46:fd5a62296b12 51 void reset() {
pvaibhav 40:8e852115fe55 52 y = 0;
pvaibhav 40:8e852115fe55 53 x_1 = 0;
pvaibhav 40:8e852115fe55 54 }
pvaibhav 40:8e852115fe55 55 };
pvaibhav 40:8e852115fe55 56
pvaibhav 40:8e852115fe55 57 class Integrator
pvaibhav 40:8e852115fe55 58 {
pvaibhav 40:8e852115fe55 59 protected:
pvaibhav 40:8e852115fe55 60 float y;
pvaibhav 40:8e852115fe55 61 float dt;
pvaibhav 40:8e852115fe55 62 float clipmax;
pvaibhav 46:fd5a62296b12 63
pvaibhav 40:8e852115fe55 64 public:
pvaibhav 40:8e852115fe55 65 Integrator( float const _dt = 1,
pvaibhav 40:8e852115fe55 66 float const initial = 0,
pvaibhav 40:8e852115fe55 67 float const _max = 1000000) : y(initial),
pvaibhav 46:fd5a62296b12 68 dt(_dt),
pvaibhav 46:fd5a62296b12 69 clipmax(_max) {}
pvaibhav 46:fd5a62296b12 70 float integrate(float const x) {
pvaibhav 40:8e852115fe55 71 return (y = utils::clip(-clipmax, y + x * dt, clipmax));
pvaibhav 40:8e852115fe55 72 }
pvaibhav 46:fd5a62296b12 73 void reset(float const _dt = 1, float const initial = 0) {
pvaibhav 40:8e852115fe55 74 y = initial;
pvaibhav 40:8e852115fe55 75 dt = _dt;
pvaibhav 40:8e852115fe55 76 }
pvaibhav 40:8e852115fe55 77 };
pvaibhav 40:8e852115fe55 78
pvaibhav 40:8e852115fe55 79 class Differentiator
pvaibhav 40:8e852115fe55 80 {
pvaibhav 40:8e852115fe55 81 protected:
pvaibhav 40:8e852115fe55 82 float y;
pvaibhav 46:fd5a62296b12 83
pvaibhav 40:8e852115fe55 84 public:
pvaibhav 40:8e852115fe55 85 Differentiator() : y(0) {}
pvaibhav 46:fd5a62296b12 86 float differentiate(float const x) {
pvaibhav 40:8e852115fe55 87 return (y = x - y);
pvaibhav 40:8e852115fe55 88 }
pvaibhav 46:fd5a62296b12 89
pvaibhav 46:fd5a62296b12 90 void reset() {
pvaibhav 40:8e852115fe55 91 y = 0;
pvaibhav 40:8e852115fe55 92 }
pvaibhav 40:8e852115fe55 93 };
pvaibhav 40:8e852115fe55 94
pvaibhav 40:8e852115fe55 95 #endif//_H_FILTER_H