mirror of https://github.com/ArduPilot/ardupilot
Filter: improved low pass filter allows setting gain using time_step and cutoff freq
This commit is contained in:
parent
16b5b00368
commit
957d366cca
|
@ -23,7 +23,10 @@ class LowPassFilter : public Filter<T>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
// constructor
|
// 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
|
// apply - Add a new raw value to the filter, retrieve the filtered result
|
||||||
virtual T apply(T sample);
|
virtual T apply(T sample);
|
||||||
|
@ -39,7 +42,7 @@ public:
|
||||||
};
|
};
|
||||||
|
|
||||||
private:
|
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
|
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
|
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 //////////////////////////////////////////////////////////////
|
// Constructor //////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
template <class T>
|
template <class T>
|
||||||
LowPassFilter<T>::LowPassFilter(float gain) :
|
LowPassFilter<T>::LowPassFilter() :
|
||||||
Filter<T>(),
|
Filter<T>(),
|
||||||
_gain(gain),
|
_alpha(1),
|
||||||
_base_value_set(false)
|
_base_value_set(false)
|
||||||
{
|
{};
|
||||||
// bounds checking on _gain
|
|
||||||
if( _gain > 1.0) {
|
// F_Cut = 1; % Hz
|
||||||
_gain = 1.0;
|
//RC = 1/(2*pi*F_Cut);
|
||||||
}else if( _gain < 0.0 ) {
|
//Alpha = Ts/(Ts + RC);
|
||||||
_gain = 0.0;
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
// Public Methods //////////////////////////////////////////////////////////////
|
// Public Methods //////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
template <class T>
|
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
|
// initailise _base_value if required
|
||||||
if( !_base_value_set ) {
|
if( !_base_value_set ) {
|
||||||
|
@ -84,7 +99,8 @@ T LowPassFilter<T>:: apply(T sample)
|
||||||
}
|
}
|
||||||
|
|
||||||
// do the filtering
|
// 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 the value. Should be no need to check limits
|
||||||
return (T)_base_value;
|
return (T)_base_value;
|
||||||
|
|
|
@ -15,8 +15,8 @@
|
||||||
////////////////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////////////////
|
||||||
FastSerialPort0(Serial); // FTDI/console
|
FastSerialPort0(Serial); // FTDI/console
|
||||||
|
|
||||||
// create a global instance of the class instead of local to avoid memory fragmentation
|
// create a global instance of the class
|
||||||
LowPassFilterInt16 low_pass_filter(0.02); // simple low pass filter which applies 2% of new data to old data
|
LowPassFilterFloat low_pass_filter;
|
||||||
|
|
||||||
// setup routine
|
// setup routine
|
||||||
void setup()
|
void setup()
|
||||||
|
@ -27,6 +27,10 @@ void setup()
|
||||||
// introduction
|
// introduction
|
||||||
Serial.printf("ArduPilot LowPassFilter test ver 1.0\n\n");
|
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
|
// Wait for the serial connection
|
||||||
delay(500);
|
delay(500);
|
||||||
}
|
}
|
||||||
|
@ -35,27 +39,25 @@ void setup()
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
int16_t i;
|
int16_t i;
|
||||||
int16_t new_value;
|
float new_value;
|
||||||
int16_t filtered_value;
|
float filtered_value;
|
||||||
|
|
||||||
// reset value to 100. If not reset the filter will start at the first value entered
|
// 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 data value
|
||||||
new_value = 105;
|
new_value = sin((float)i*2*M_PI*5/50.0); // 5hz
|
||||||
|
|
||||||
// output to user
|
// 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
|
// apply new value and retrieved filtered result
|
||||||
filtered_value = low_pass_filter.apply(new_value);
|
filtered_value = low_pass_filter.apply(new_value);
|
||||||
|
|
||||||
// display results
|
// display results
|
||||||
Serial.printf("\toutput: %d\n\n",(int)filtered_value);
|
Serial.printf("\toutput: %6.4f\n",(float)filtered_value);
|
||||||
}
|
}
|
||||||
delay(10000);
|
delay(10000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue