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