Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
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?
User | Revision | Line number | New 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 |