Bremen Team - Hangar / SML2

Implemented first Hangar-Service

Dependencies:   CalibrateMagneto QuaternionMath

Fork of SML2 by TobyRich GmbH

Filter.h@40:8e852115fe55, 2015-05-26 (annotated)

Committer:
pvaibhav
Date:
Tue May 26 11:28:37 2015 +0000
Revision:
40:8e852115fe55
Child:
46:fd5a62296b12
SensorFusion base class and 6 axis derived class. utils, filter and pid classes moved inside SML2.

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 40:8e852115fe55 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 40:8e852115fe55 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 40:8e852115fe55 24 float filter(float const x)
pvaibhav 40:8e852115fe55 25 {
pvaibhav 40:8e852115fe55 26 y = x + alpha * (y - x);
pvaibhav 40:8e852115fe55 27 return y;
pvaibhav 40:8e852115fe55 28 }
pvaibhav 40:8e852115fe55 29
pvaibhav 40:8e852115fe55 30 /// Resets the internal state of the filter. Use to start filtering a new stream.
pvaibhav 40:8e852115fe55 31 void reset()
pvaibhav 40:8e852115fe55 32 {
pvaibhav 40:8e852115fe55 33 y = 0;
pvaibhav 40:8e852115fe55 34 }
pvaibhav 40:8e852115fe55 35 };
pvaibhav 40:8e852115fe55 36
pvaibhav 40:8e852115fe55 37 /// 1st order high-pass filter.
pvaibhav 40:8e852115fe55 38 /// Used in exactly the same way as LowPassFilter, except that it's a high-pass. @see LowPassFilter
pvaibhav 40:8e852115fe55 39 class HighPassFilter
pvaibhav 40:8e852115fe55 40 {
pvaibhav 40:8e852115fe55 41 protected:
pvaibhav 40:8e852115fe55 42 float y;
pvaibhav 40:8e852115fe55 43 float x_1;
pvaibhav 40:8e852115fe55 44 float const alpha;
pvaibhav 40:8e852115fe55 45
pvaibhav 40:8e852115fe55 46 public:
pvaibhav 40:8e852115fe55 47 HighPassFilter(float const a) : y(0), x_1(0), alpha(1 - a) {}
pvaibhav 40:8e852115fe55 48 float filter(float const x)
pvaibhav 40:8e852115fe55 49 {
pvaibhav 40:8e852115fe55 50 y = alpha * (y + x - x_1);
pvaibhav 40:8e852115fe55 51 x_1 = x;
pvaibhav 40:8e852115fe55 52 return y;
pvaibhav 40:8e852115fe55 53 }
pvaibhav 40:8e852115fe55 54 void reset()
pvaibhav 40:8e852115fe55 55 {
pvaibhav 40:8e852115fe55 56 y = 0;
pvaibhav 40:8e852115fe55 57 x_1 = 0;
pvaibhav 40:8e852115fe55 58 }
pvaibhav 40:8e852115fe55 59 };
pvaibhav 40:8e852115fe55 60
pvaibhav 40:8e852115fe55 61 class Integrator
pvaibhav 40:8e852115fe55 62 {
pvaibhav 40:8e852115fe55 63 protected:
pvaibhav 40:8e852115fe55 64 float y;
pvaibhav 40:8e852115fe55 65 float dt;
pvaibhav 40:8e852115fe55 66 float clipmax;
pvaibhav 40:8e852115fe55 67
pvaibhav 40:8e852115fe55 68 public:
pvaibhav 40:8e852115fe55 69 Integrator( float const _dt = 1,
pvaibhav 40:8e852115fe55 70 float const initial = 0,
pvaibhav 40:8e852115fe55 71 float const _max = 1000000) : y(initial),
pvaibhav 40:8e852115fe55 72 dt(_dt),
pvaibhav 40:8e852115fe55 73 clipmax(_max) {}
pvaibhav 40:8e852115fe55 74 float integrate(float const x)
pvaibhav 40:8e852115fe55 75 {
pvaibhav 40:8e852115fe55 76 return (y = utils::clip(-clipmax, y + x * dt, clipmax));
pvaibhav 40:8e852115fe55 77 }
pvaibhav 40:8e852115fe55 78 void reset(float const _dt = 1, float const initial = 0)
pvaibhav 40:8e852115fe55 79 {
pvaibhav 40:8e852115fe55 80 y = initial;
pvaibhav 40:8e852115fe55 81 dt = _dt;
pvaibhav 40:8e852115fe55 82 }
pvaibhav 40:8e852115fe55 83 };
pvaibhav 40:8e852115fe55 84
pvaibhav 40:8e852115fe55 85 class Differentiator
pvaibhav 40:8e852115fe55 86 {
pvaibhav 40:8e852115fe55 87 protected:
pvaibhav 40:8e852115fe55 88 float y;
pvaibhav 40:8e852115fe55 89
pvaibhav 40:8e852115fe55 90 public:
pvaibhav 40:8e852115fe55 91 Differentiator() : y(0) {}
pvaibhav 40:8e852115fe55 92 float differentiate(float const x)
pvaibhav 40:8e852115fe55 93 {
pvaibhav 40:8e852115fe55 94 return (y = x - y);
pvaibhav 40:8e852115fe55 95 }
pvaibhav 40:8e852115fe55 96
pvaibhav 40:8e852115fe55 97 void reset()
pvaibhav 40:8e852115fe55 98 {
pvaibhav 40:8e852115fe55 99 y = 0;
pvaibhav 40:8e852115fe55 100 }
pvaibhav 40:8e852115fe55 101 };
pvaibhav 40:8e852115fe55 102
pvaibhav 40:8e852115fe55 103 #endif//_H_FILTER_H