Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Filter: LowPassFilter: split into two classes for constant and variable dt

This commit is contained in:
Iampete1 2024-07-27 13:16:20 +01:00 committed by Andrew Tridgell
parent 7d7333a91f
commit e2ce21a237
2 changed files with 134 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
libraries/Filter/LowPassFilter.cpp
View File

@ -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
@ -12,52 +10,65 @@
#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;
T DigitalLPF<T>::_apply(const T &sample, const float &alpha) {
output += (sample - output) * alpha;
if (!initialised) {
initialised = true;
_output = sample;
output = sample;
}
return _output;
return output;
}
// get latest filtered value from filter (equal to the value returned by latest call to apply method)
template <class T>
T DigitalLPF<T>::apply(const T &sample) {
_output += (sample - _output) * alpha;
if (!initialised) {
const T &DigitalLPF<T>::get() const {
return output;
}
// Reset filter to given value
template <class T>
void DigitalLPF<T>::reset(const T &value) {
output = value;
initialised = true;
_output = sample;
}
return _output;
}
// Set reset flag such that the filter will be reset to the next value applied
template <class T>
void DigitalLPF<T>::compute_alpha(float sample_freq, float cutoff_freq) {
void DigitalLPF<T>::reset() {
initialised = false;
}
template class DigitalLPF<float>;
template class DigitalLPF<Vector2f>;
template class DigitalLPF<Vector3f>;
////////////////////////////////////////////////////////////////////////////////////////////
// Low pass filter with constant time step
////////////////////////////////////////////////////////////////////////////////////////////
// constructor
template <class T>
LowPassFilterConstDt<T>::LowPassFilterConstDt(const float &sample_freq, const float &new_cutoff_freq)
{
set_cutoff_frequency(sample_freq, new_cutoff_freq);
}
// change parameters
template <class T>
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 {
@ -65,81 +76,68 @@ void DigitalLPF<T>::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)
// return the cutoff frequency
template <class T>
const T &DigitalLPF<T>::get() const {
return _output;
float LowPassFilterConstDt<T>::get_cutoff_freq(void) const {
return cutoff_freq;
}
// add a new raw value to the filter, retrieve the filtered result
template <class T>
void DigitalLPF<T>::reset(T value) {
_output = value;
initialised = true;
T LowPassFilterConstDt<T>::apply(const T &sample) {
return this->_apply(sample, alpha);
}
template class LowPassFilterConstDt<float>;
template class LowPassFilterConstDt<Vector2f>;
template class LowPassFilterConstDt<Vector3f>;
////////////////////////////////////////////////////////////////////////////////////////////
// LowPassFilter
// Low pass filter with variable time step
////////////////////////////////////////////////////////////////////////////////////////////
// constructors
template <class T>
LowPassFilter<T>::LowPassFilter() :
_cutoff_freq(0.0f) {}
template <class T>
LowPassFilter<T>::LowPassFilter(float cutoff_freq) :
_cutoff_freq(cutoff_freq) {}
template <class T>
LowPassFilter<T>::LowPassFilter(float sample_freq, float cutoff_freq)
LowPassFilter<T>::LowPassFilter(const float &new_cutoff_freq)
{
set_cutoff_frequency(sample_freq, cutoff_freq);
set_cutoff_frequency(new_cutoff_freq);
}
// change parameters
template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
_cutoff_freq = cutoff_freq;
}
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);
void LowPassFilter<T>::set_cutoff_frequency(const float &new_cutoff_freq) {
cutoff_freq = new_cutoff_freq;
}
// return the cutoff frequency
template <class T>
float LowPassFilter<T>::get_cutoff_freq(void) const {
return _cutoff_freq;
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>
T LowPassFilter<T>::apply(T sample, float dt) {
return _filter.apply(sample, _cutoff_freq, dt);
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();
}
template <class T>
T LowPassFilter<T>::apply(T sample) {
return _filter.apply(sample);
if (is_zero(cutoff_freq)) {
this->reset(sample);
return this->get();
}
template <class T>
const T &LowPassFilter<T>::get() const {
return _filter.get();
if (is_zero(dt)) {
return this->get();
}
template <class T>
void LowPassFilter<T>::reset(T value) {
_filter.reset(value);
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>;

106
libraries/Filter/LowPassFilter.h
View File

@ -15,14 +15,12 @@
//
/// @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);
@ -30,7 +28,7 @@
// 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);
@ -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);
// constructors
LowPassFilterConstDt() {};
LowPassFilterConstDt(const float &sample_freq, const float &cutoff_freq);
CLASS_NO_COPY(LowPassFilterConstDt);
// change parameters
void set_cutoff_frequency(const float &sample_freq, 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);
private:
float cutoff_freq;
float alpha;
};
typedef LowPassFilterConstDt<float> LowPassFilterConstDtFloat;
typedef LowPassFilterConstDt<Vector2f> LowPassFilterConstDtVector2f;
typedef LowPassFilterConstDt<Vector3f> LowPassFilterConstDtVector3f;
// Low pass filter with variable time step
template <class T>
class LowPassFilter : public DigitalLPF<T> {
public:
// constructors
LowPassFilter() {};
LowPassFilter(const float &cutoff_freq);
CLASS_NO_COPY(LowPassFilter);
// 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 &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, const float &dt);
private:
DigitalLPF<T> _filter;
float cutoff_freq;
};
// Uncomment this, if you decide to remove the instantiations in the implementation file
/*
template <class T>
LowPassFilter<T>::LowPassFilter() : _cutoff_freq(0.0f) {
}
// constructor
template <class T>
LowPassFilter<T>::LowPassFilter(float cutoff_freq) : _cutoff_freq(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;