Skip to content

Commit

Permalink
Filter: LowPassFilter: split into two classes for constant and variab…
Browse files Browse the repository at this point in the history 
…le dt
  • Loading branch information
@IamPete1
IamPete1 committed Aug 6, 2024
1 parent a6bbeb2 commit 262b34f
Show file tree
Hide file tree
Showing 2 changed files with 129 additions and 123 deletions.
148 changes: 73 additions & 75 deletions libraries/Filter/LowPassFilter.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,6 @@
//
/// @file LowPassFilter.cpp
/// @brief A class to implement a low pass filter without losing precision even for int types
/// the downside being that it's a little slower as it internally uses a float
/// and it consumes an extra 4 bytes of memory to hold the constant gain
/// @brief A class to implement a low pass filter

#ifndef HAL_DEBUG_BUILD
#define AP_INLINE_VECTOR_OPS
Expand All @@ -12,134 +10,134 @@
#include <AP_InternalError/AP_InternalError.h>

////////////////////////////////////////////////////////////////////////////////////////////
// DigitalLPF
// DigitalLPF, base class
////////////////////////////////////////////////////////////////////////////////////////////

template <class T>
DigitalLPF<T>::DigitalLPF() {
// built in initialization
_output = T();
output = T();
}

// add a new raw value to the filter, retrieve the filtered result
template <class T>
T DigitalLPF<T>::apply(const T &sample, float cutoff_freq, float dt) {
if (is_negative(cutoff_freq) || is_negative(dt)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
_output = sample;
return _output;
}
if (is_zero(cutoff_freq)) {
_output = sample;
return _output;
}
if (is_zero(dt)) {
return _output;
}
float rc = 1.0f/(M_2PI*cutoff_freq);
alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
_output += (sample - _output) * alpha;
if (!initialised) {
initialised = true;
_output = sample;
}
return _output;
}

template <class T>
T DigitalLPF<T>::apply(const T &sample) {
_output += (sample - _output) * alpha;
T DigitalLPF<T>::_apply(const T &sample, const float &alpha) {
output += (sample - output) * alpha;
if (!initialised) {
initialised = true;
_output = sample;
}
return _output;
}

template <class T>
void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) {
if (sample_freq <= 0) {
alpha = 1;
} else {
alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq);
output = sample;
}
return output;
}

// get latest filtered value from filter (equal to the value returned by latest call to apply method)
template <class T>
const T &DigitalLPF<T>::get() const {
return _output;
return output;
}

// Reset filter to given value
template <class T>
void DigitalLPF<T>::reset(T value) {
_output = value;
void DigitalLPF<T>::reset(const T &value) {
output = value;
initialised = true;
}

////////////////////////////////////////////////////////////////////////////////////////////
// LowPassFilter
////////////////////////////////////////////////////////////////////////////////////////////

// constructors
// Set reset flag such that the filter will be reset to the next value applied
template <class T>
LowPassFilter<T>::LowPassFilter() :
_cutoff_freq(0.0f) {}
void DigitalLPF<T>::reset() {
initialised = false;
}

template <class T>
LowPassFilter<T>::LowPassFilter(float cutoff_freq) :
_cutoff_freq(cutoff_freq) {}
template class DigitalLPF<float>;
template class DigitalLPF<Vector2f>;
template class DigitalLPF<Vector3f>;

////////////////////////////////////////////////////////////////////////////////////////////
// Low pass filter with constant time step
////////////////////////////////////////////////////////////////////////////////////////////

// constructor
template <class T>
LowPassFilter<T>::LowPassFilter(float sample_freq, float cutoff_freq)
LowPassFilterConstDt<T>::LowPassFilterConstDt(const float &sample_freq, const float &new_cutoff_freq)
{
set_cutoff_frequency(sample_freq, cutoff_freq);
set_cutoff_frequency(sample_freq, new_cutoff_freq);
}

// change parameters
template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
_cutoff_freq = cutoff_freq;
void LowPassFilterConstDt<T>::set_cutoff_frequency(const float &sample_freq, const float &new_cutoff_freq) {
cutoff_freq = new_cutoff_freq;

if (sample_freq <= 0) {
alpha = 1;
} else {
alpha = calc_lowpass_alpha_dt(1.0/sample_freq, cutoff_freq);
}
}

// return the cutoff frequency
template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) {
_cutoff_freq = cutoff_freq;
_filter.compute_alpha(sample_freq, cutoff_freq);
float LowPassFilterConstDt<T>::get_cutoff_freq(void) const {
return cutoff_freq;
}

// return the cutoff frequency
// add a new raw value to the filter, retrieve the filtered result
template <class T>
float LowPassFilter<T>::get_cutoff_freq(void) const {
return _cutoff_freq;
T LowPassFilterConstDt<T>::apply(const T &sample) {
return this->_apply(sample, alpha);
}

template class LowPassFilterConstDt<float>;
template class LowPassFilterConstDt<Vector2f>;
template class LowPassFilterConstDt<Vector3f>;

////////////////////////////////////////////////////////////////////////////////////////////
// Low pass filter with variable time step
////////////////////////////////////////////////////////////////////////////////////////////

template <class T>
T LowPassFilter<T>::apply(T sample, float dt) {
return _filter.apply(sample, _cutoff_freq, dt);
LowPassFilter<T>::LowPassFilter(const float &new_cutoff_freq)
{
set_cutoff_frequency(new_cutoff_freq);
}

// change parameters
template <class T>
T LowPassFilter<T>::apply(T sample) {
return _filter.apply(sample);
void LowPassFilter<T>::set_cutoff_frequency(const float &new_cutoff_freq) {
cutoff_freq = new_cutoff_freq;
}

// return the cutoff frequency
template <class T>
const T &LowPassFilter<T>::get() const {
return _filter.get();
float LowPassFilter<T>::get_cutoff_freq() const {
return cutoff_freq;
}

// add a new raw value to the filter, retrieve the filtered result
template <class T>
void LowPassFilter<T>::reset(T value) {
_filter.reset(value);
T LowPassFilter<T>::apply(const T &sample, const float &dt) {
if (is_negative(cutoff_freq) || is_negative(dt)) {
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
this->reset(sample);
return this->get();
}
if (is_zero(cutoff_freq)) {
this->reset(sample);
return this->get();
}
if (is_zero(dt)) {
return this->get();
}
const float rc = 1.0f/(M_2PI*cutoff_freq);
const float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
return this->_apply(sample, alpha);
}

/*
* Make an instances
* Otherwise we have to move the constructor implementations to the header file :P
*/
template class LowPassFilter<int>;
template class LowPassFilter<long>;
template class LowPassFilter<float>;
template class LowPassFilter<Vector2f>;
template class LowPassFilter<Vector3f>;
Expand Down
104 changes: 56 additions & 48 deletions libraries/Filter/LowPassFilter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,22 +15,20 @@

//
/// @file LowPassFilter.h
/// @brief A class to implement a low pass filter without losing precision even for int types
/// the downside being that it's a little slower as it internally uses a float
/// and it consumes an extra 4 bytes of memory to hold the constant gain
/// @brief A class to implement a low pass filter.

/*
Note that this filter can be used in 2 ways:
Two classes are provided:
1) providing dt on every sample, and calling apply like this:
LowPassFilter: providing dt on every sample, and calling apply like this:
// call once
filter.set_cutoff_frequency(frequency_hz);
// then on each sample
output = filter.apply(sample, dt);
2) providing a sample freq and cutoff_freq once at start
LowPassFilterConstDt: providing a sample freq and cutoff_freq once at start
// call once
filter.set_cutoff_frequency(sample_freq, frequency_hz);
Expand All @@ -45,79 +43,89 @@
#pragma once

#include <AP_Math/AP_Math.h>
#include "FilterClass.h"

// DigitalLPF implements the filter math
template <class T>
class DigitalLPF {
public:

// constructor
DigitalLPF();
// add a new raw value to the filter, retrieve the filtered result
T apply(const T &sample, float cutoff_freq, float dt);
T apply(const T &sample);

CLASS_NO_COPY(DigitalLPF);

void compute_alpha(float sample_freq, float cutoff_freq);

// get latest filtered value from filter (equal to the value returned by latest call to apply method)
const T &get() const;
void reset(T value);
void reset() {
initialised = false;
}

// Reset filter to given value
void reset(const T &value);

// Set reset flag such that the filter will be reset to the next value applied
void reset();

protected:
// add a new raw value to the filter, retrieve the filtered result
T _apply(const T &sample, const float &alpha);

private:
T _output;
float alpha = 1.0f;
T output;
bool initialised;
};

// LPF base class
// Low pass filter with constant time step
template <class T>
class LowPassFilter {
class LowPassFilterConstDt : public DigitalLPF<T> {
public:
LowPassFilter();
LowPassFilter(float cutoff_freq);
LowPassFilter(float sample_freq, float cutoff_freq);

CLASS_NO_COPY(LowPassFilter);
// constructors
LowPassFilterConstDt() {};
LowPassFilterConstDt(const float &sample_freq, const float &cutoff_freq);

CLASS_NO_COPY(LowPassFilterConstDt);

// change parameters
void set_cutoff_frequency(float cutoff_freq);
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
void set_cutoff_frequency(const float &sample_freq, const float &cutoff_freq);

// return the cutoff frequency
float get_cutoff_freq(void) const;
T apply(T sample, float dt);
T apply(T sample);
const T &get() const;
void reset(T value);
void reset(void) { _filter.reset(); }
float get_cutoff_freq() const;

protected:
float _cutoff_freq;
// add a new raw value to the filter, retrieve the filtered result
T apply(const T &sample);

private:
DigitalLPF<T> _filter;
float cutoff_freq;
float alpha;
};

// Uncomment this, if you decide to remove the instantiations in the implementation file
/*
template <class T>
LowPassFilter<T>::LowPassFilter() : _cutoff_freq(0.0f) {
}
// constructor
typedef LowPassFilterConstDt<float> LowPassFilterConstDtFloat;
typedef LowPassFilterConstDt<Vector2f> LowPassFilterConstDtVector2f;
typedef LowPassFilterConstDt<Vector3f> LowPassFilterConstDtVector3f;

// Low pass filter with variable time step
template <class T>
LowPassFilter<T>::LowPassFilter(float cutoff_freq) : _cutoff_freq(cutoff_freq) {
}
*/
class LowPassFilter : public DigitalLPF<T> {
public:

// constructors
LowPassFilter() {};
LowPassFilter(const float &cutoff_freq);

CLASS_NO_COPY(LowPassFilter);

// change parameters
void set_cutoff_frequency(const float &cutoff_freq);

// return the cutoff frequency
float get_cutoff_freq() const;

// add a new raw value to the filter, retrieve the filtered result
T apply(const T &sample, const float &dt);

private:
float cutoff_freq;
};

// typedefs for compatibility
typedef LowPassFilter<int> LowPassFilterInt;
typedef LowPassFilter<long> LowPassFilterLong;
typedef LowPassFilter<float> LowPassFilterFloat;
typedef LowPassFilter<Vector2f> LowPassFilterVector2f;
typedef LowPassFilter<Vector3f> LowPassFilterVector3f;

0 comments on commit 262b34f

Please sign in to comment.