Implemented first Hangar-Service
Dependencies: CalibrateMagneto QuaternionMath
Fork of SML2 by
Filter.h
- Committer:
- pvaibhav
- Date:
- 2015-05-27
- Revision:
- 46:fd5a62296b12
- Parent:
- 40:8e852115fe55
File content as of revision 46:fd5a62296b12:
#ifndef _H_FILTER_H #define _H_FILTER_H #include "Utils.h" /// \brief Low-pass filters an incoming stream of numbers. /// Implements a 1st-order IIR low-pass filter. The coefficient can be used to specify /// how much to filter. class LowPassFilter { protected: float y; float const alpha; public: /// Construct the filter with given coefficient. /// @param a filter coefficient (0.0 to 1.0), higher values result in more filtering. LowPassFilter(float const a) : y(0), alpha(a) {} /// Main processing function, passes given value to filter and returns the filter's output. /// Call repeatedly to filter an incoming stream of numbers. /// @param x one value to send to filter as input /// @return filtered output float filter(float const x) { y = x + alpha * (y - x); return y; } /// Resets the internal state of the filter. Use to start filtering a new stream. void reset() { y = 0; } }; /// 1st order high-pass filter. /// Used in exactly the same way as LowPassFilter, except that it's a high-pass. @see LowPassFilter class HighPassFilter { protected: float y; float x_1; float const alpha; public: HighPassFilter(float const a) : y(0), x_1(0), alpha(1 - a) {} float filter(float const x) { y = alpha * (y + x - x_1); x_1 = x; return y; } void reset() { y = 0; x_1 = 0; } }; class Integrator { protected: float y; float dt; float clipmax; public: Integrator( float const _dt = 1, float const initial = 0, float const _max = 1000000) : y(initial), dt(_dt), clipmax(_max) {} float integrate(float const x) { return (y = utils::clip(-clipmax, y + x * dt, clipmax)); } void reset(float const _dt = 1, float const initial = 0) { y = initial; dt = _dt; } }; class Differentiator { protected: float y; public: Differentiator() : y(0) {} float differentiate(float const x) { return (y = x - y); } void reset() { y = 0; } }; #endif//_H_FILTER_H