Filter: improved low pass filter allows setting gain using time_step and cutoff freq

libraries/Filter/LowPassFilter.h

@@ -23,7 +23,10 @@ class LowPassFilter : public Filter<T>
 {
 public:
     // constructor
-    LowPassFilter(float gain);
+    LowPassFilter();
+
+    virtual void    set_cutoff_frequency(float time_step, float cutoff_freq);
+    virtual void    set_time_constant(float time_step, float time_constant);
 
     // apply - Add a new raw value to the filter, retrieve the filtered result
     virtual T        apply(T sample);
@@ -39,7 +42,7 @@ public:
     };
 
 private:
-    float           _gain;                      // gain value  (like 0.02) applied to each new value
+    float           _alpha;             // gain value  (like 0.02) applied to each new value
     bool            _base_value_set;    // true if the base value has been set
     float           _base_value;        // the number of samples in the filter, maxes out at size of the filter
 };
@@ -59,23 +62,35 @@ typedef LowPassFilter<float> LowPassFilterFloat;
 // Constructor    //////////////////////////////////////////////////////////////
 
 template <class T>
-LowPassFilter<T>::LowPassFilter(float gain) :
+LowPassFilter<T>::LowPassFilter() :
     Filter<T>(),
-    _gain(gain),
+    _alpha(1),
     _base_value_set(false)
-{
-    // bounds checking on _gain
-    if( _gain > 1.0) {
-        _gain = 1.0;
-    }else if( _gain < 0.0 ) {
-        _gain = 0.0;
-    }
-};
+{};
+
+//    F_Cut = 1; % Hz
+//RC = 1/(2*pi*F_Cut);
+//Alpha = Ts/(Ts + RC);
 
 // Public Methods //////////////////////////////////////////////////////////////
 
 template <class T>
-T LowPassFilter<T>::        apply(T sample)
+void LowPassFilter<T>::set_cutoff_frequency(float time_step, float cutoff_freq)
+{
+    // calculate alpha
+    float rc = 1/(2*M_PI*cutoff_freq);
+    _alpha = time_step / (time_step + rc);
+}
+
+template <class T>
+void LowPassFilter<T>::set_time_constant(float time_step, float time_constant)
+{
+    // calculate alpha
+    _alpha = time_step / (time_constant + time_step);
+}
+
+template <class T>
+T LowPassFilter<T>::apply(T sample)
 {
     // initailise _base_value if required
     if( !_base_value_set ) {
@@ -84,7 +99,8 @@ T LowPassFilter<T>::        apply(T sample)
     }
 
     // do the filtering
-    _base_value = _gain * (float)sample + (1.0 - _gain) * _base_value;
+    //_base_value = _alpha * (float)sample + (1.0 - _alpha) * _base_value;
+    _base_value = _base_value + _alpha * ((float)sample - _base_value);
 
     // return the value.  Should be no need to check limits
     return (T)_base_value;

libraries/Filter/examples/LowPassFilter/LowPassFilter.pde

@@ -15,8 +15,8 @@
 ////////////////////////////////////////////////////////////////////////////////
 FastSerialPort0(Serial);        // FTDI/console
 
-// create a global instance of the class instead of local to avoid memory fragmentation
-LowPassFilterInt16 low_pass_filter(0.02);  // simple low pass filter which applies 2% of new data to old data
+// create a global instance of the class
+LowPassFilterFloat low_pass_filter;
 
 // setup routine
 void setup()
@@ -27,6 +27,10 @@ void setup()
     // introduction
     Serial.printf("ArduPilot LowPassFilter test ver 1.0\n\n");
 
+    // set-up filter
+    low_pass_filter.set_time_constant(0.02, 0.015);
+    //low_pass_filter.set_cutoff_frequency(0.02, 1.0);
+
     // Wait for the serial connection
     delay(500);
 }
@@ -35,27 +39,25 @@ void setup()
 void loop()
 {
     int16_t i;
-    int16_t new_value;
-    int16_t filtered_value;
+    float new_value;
+    float filtered_value;
 
     // reset value to 100.  If not reset the filter will start at the first value entered
-    low_pass_filter.reset(100);
+    low_pass_filter.reset(0);
 
-    for( i=0; i<210; i++ ) {
+    for( i=0; i<300; i++ ) {
 
         // new data value
-        new_value = 105;
+        new_value = sin((float)i*2*M_PI*5/50.0);  // 5hz
 
         // output to user
-        Serial.printf("applying: %d",(int)new_value);
+        Serial.printf("applying: %6.4f",(float)new_value);
 
         // apply new value and retrieved filtered result
         filtered_value = low_pass_filter.apply(new_value);
 
         // display results
-        Serial.printf("\toutput: %d\n\n",(int)filtered_value);
+        Serial.printf("\toutput: %6.4f\n",(float)filtered_value);
     }
     delay(10000);
 }
-
-