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

Filter: Template implementation for <Filter> 

There are implementations for float, Vector2f and Vector3f for the Low
Pass Filter and the *2p filter.  I tried to implement these filters
with one common template implementation.  This implementation saves
some lines of code and reduced the redundancy.  One could save even
more code if the currently overloaded isinf/isnan functions and checks
can be removed.

Signed-off-by: Daniel Frenzel <dgdanielf@gmail.com>
This commit is contained in:
dgrat 2015-08-19 14:15:14 +02:00 committed by Andrew Tridgell
parent 37336bbbe1
commit 58e2ac4e56
4 changed files with 251 additions and 231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
libraries/Filter/LowPassFilter.cpp Normal file
View File

@ -0,0 +1,94 @@
//
/// @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
#include "LowPassFilter.h"
////////////////////////////////////////////////////////////////////////////////////////////
// DigitalLPF
////////////////////////////////////////////////////////////////////////////////////////////
template <class T>
DigitalLPF<T>::DigitalLPF() {
// built in initialization
_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 (cutoff_freq <= 0.0f || dt <= 0.0f) {
_output = sample;
return _output;
}
float rc = 1.0f/(M_2PI_F*cutoff_freq);
float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
_output += (sample - _output) * alpha;
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;
}
template <class T>
void DigitalLPF<T>::reset(T value) {
_output = value;
}
////////////////////////////////////////////////////////////////////////////////////////////
// LowPassFilter
////////////////////////////////////////////////////////////////////////////////////////////
template <class T>
LowPassFilter<T>::LowPassFilter() : _cutoff_freq(0.0f) {
}
// constructor
template <class T>
LowPassFilter<T>::LowPassFilter(float cutoff_freq) : _cutoff_freq(cutoff_freq) {
}
// change parameters
template <class T>
void LowPassFilter<T>::set_cutoff_frequency(float cutoff_freq) {
_cutoff_freq = cutoff_freq;
}
// return the cutoff frequency
template <class T>
float LowPassFilter<T>::get_cutoff_freq(void) const {
return _cutoff_freq;
}
template <class T>
T LowPassFilter<T>::apply(T sample, float dt) {
return _filter.apply(sample, _cutoff_freq, dt);
}
template <class T>
const T &LowPassFilter<T>::get() const {
return _filter.get();
}
template <class T>
void LowPassFilter<T>::reset(T value) {
_filter.reset(value);
}
/*
* 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<double>;
template class LowPassFilter<Vector2f>;
template class LowPassFilter<Vector3f>;

167
libraries/Filter/LowPassFilter.h
View File

@ -27,156 +27,67 @@
#include "FilterClass.h"
// DigitalLPF implements the filter math
class DigitalLPF
{
template <class T>
class DigitalLPF {
public:
// constructor
DigitalLPF() :
_output(0.0f) {}
struct lpf_params {
float cutoff_freq;
float sample_freq;
float alpha;
};
};
DigitalLPF();
// add a new raw value to the filter, retrieve the filtered result
float apply(float sample, float cutoff_freq, float dt) {
if (cutoff_freq <= 0.0f || dt <= 0.0f) {
_output = sample;
return _output;
}
float rc = 1.0f/(M_2PI_F*cutoff_freq);
float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
_output += (sample - _output) * alpha;
return _output;
}
T apply(const T &sample, float cutoff_freq, float dt);
// get latest filtered value from filter (equal to the value returned by latest call to apply method)
float get() const {
return _output;
}
void reset(float value) { _output = value; }
const T &get() const;
void reset(T value);
private:
float _output;
T _output;
};
// LPF base class
class LowPassFilter
{
template <class T>
class LowPassFilter {
public:
LowPassFilter() :
_cutoff_freq(0.0f) { }
// constructor
LowPassFilter(float cutoff_freq) :
_cutoff_freq(cutoff_freq) { }
LowPassFilter();
LowPassFilter(float cutoff_freq);
// change parameters
void set_cutoff_frequency(float cutoff_freq) {
_cutoff_freq = cutoff_freq;
}
void set_cutoff_frequency(float cutoff_freq);
// return the cutoff frequency
float get_cutoff_freq(void) const {
return _cutoff_freq;
}
float get_cutoff_freq(void) const;
T apply(T sample, float dt);
const T &get() const;
void reset(T value);
protected:
float _cutoff_freq;
};
// LPF for a single float
class LowPassFilterFloat : public LowPassFilter
{
public:
LowPassFilterFloat() :
LowPassFilter() {}
LowPassFilterFloat(float cutoff_freq):
LowPassFilter(cutoff_freq) {}
float apply(float sample, float dt) {
return _filter.apply(sample, _cutoff_freq, dt);
}
float get() const {
return _filter.get();
}
void reset(float value) {
_filter.reset(value);
}
private:
DigitalLPF _filter;
};
// LPF for a 2D vector
class LowPassFilterVector2f : public LowPassFilter
{
public:
LowPassFilterVector2f() :
LowPassFilter() {}
LowPassFilterVector2f(float cutoff_freq) :
LowPassFilter(cutoff_freq) {}
Vector2f apply(const Vector2f &sample, float dt) {
Vector2f ret;
ret.x = _filter_x.apply(sample.x, _cutoff_freq, dt);
ret.y = _filter_y.apply(sample.y, _cutoff_freq, dt);
return ret;
}
void reset(const Vector2f& value) {
_filter_x.reset(value.x);
_filter_y.reset(value.y);
}
private:
DigitalLPF _filter_x;
DigitalLPF _filter_y;
DigitalLPF<T> _filter;
};
// LPF for 3D vector
class LowPassFilterVector3f : public LowPassFilter
{
public:
LowPassFilterVector3f() :
LowPassFilter() {}
// 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) {
}
*/
LowPassFilterVector3f(float cutoff_freq) :
LowPassFilter(cutoff_freq) {}
// typedefs for compatibility
typedef LowPassFilter<int> LowPassFilterInt;
typedef LowPassFilter<long> LowPassFilterLong;
typedef LowPassFilter<float> LowPassFilterFloat;
typedef LowPassFilter<double> LowPassFilterDouble;
typedef LowPassFilter<Vector2f> LowPassFilterVector2f;
typedef LowPassFilter<Vector3f> LowPassFilterVector3f;
Vector3f apply(const Vector3f &sample, float dt) {
Vector3f ret;
ret.x = _filter_x.apply(sample.x, _cutoff_freq, dt);
ret.y = _filter_y.apply(sample.y, _cutoff_freq, dt);
ret.z = _filter_z.apply(sample.z, _cutoff_freq, dt);
return ret;
}
// get latest filtered value from filter (equal to the value returned by latest call to apply method)
Vector3f get() const {
Vector3f ret;
ret.x = _filter_x.get();
ret.y = _filter_y.get();
ret.z = _filter_z.get();
return ret;
}
void reset(const Vector3f& value) {
_filter_x.reset(value.x);
_filter_y.reset(value.y);
_filter_z.reset(value.z);
}
private:
DigitalLPF _filter_x;
DigitalLPF _filter_y;
DigitalLPF _filter_z;
};
#endif // __LOW_PASS_FILTER_H__
#endif // __LOW_PASS_FILTER_H__

104
libraries/Filter/LowPassFilter2p.cpp
View File

@ -1,39 +1,24 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/// @file LowPassFilter.cpp
/// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com>
#include <inttypes.h>
#include <AP_Math/AP_Math.h>
#include "LowPassFilter2p.h"
float DigitalBiquadFilter::apply(float sample, const struct biquad_params &params)
{
////////////////////////////////////////////////////////////////////////////////////////////
// DigitalBiquadFilter
////////////////////////////////////////////////////////////////////////////////////////////
template <class T>
DigitalBiquadFilter<T>::DigitalBiquadFilter() {
_delay_element_1 = T();
_delay_element_2 = T();
}
template <class T>
T DigitalBiquadFilter<T>::apply(const T &sample, const struct biquad_params &params) {
if(is_zero(params.cutoff_freq) || is_zero(params.sample_freq)) {
return sample;
}
float delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
if (isnan(delay_element_0) || isinf(delay_element_0)) {
delay_element_0 = sample;
}
float output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
T delay_element_0 = sample - _delay_element_1 * params.a1 - _delay_element_2 * params.a2;
T output = delay_element_0 * params.b0 + _delay_element_1 * params.b1 + _delay_element_2 * params.b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
@ -41,8 +26,13 @@ float DigitalBiquadFilter::apply(float sample, const struct biquad_params &param
return output;
}
void DigitalBiquadFilter::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret)
{
template <class T>
void DigitalBiquadFilter<T>::reset() {
_delay_element_1 = _delay_element_2 = T();
}
template <class T>
void DigitalBiquadFilter<T>::compute_params(float sample_freq, float cutoff_freq, biquad_params &ret) {
ret.cutoff_freq = cutoff_freq;
ret.sample_freq = sample_freq;
@ -56,3 +46,53 @@ void DigitalBiquadFilter::compute_params(float sample_freq, float cutoff_freq, b
ret.a1 = 2.0f*(ohm*ohm-1.0f)/c;
ret.a2 = (1.0f-2.0f*cosf(PI/4.0f)*ohm+ohm*ohm)/c;
}
////////////////////////////////////////////////////////////////////////////////////////////
// LowPassFilter2p
////////////////////////////////////////////////////////////////////////////////////////////
template <class T>
LowPassFilter2p<T>::LowPassFilter2p() {
memset(&_params, 0, sizeof(_params) );
}
// constructor
template <class T>
LowPassFilter2p<T>::LowPassFilter2p(float sample_freq, float cutoff_freq) {
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
}
// change parameters
template <class T>
void LowPassFilter2p<T>::set_cutoff_frequency(float sample_freq, float cutoff_freq) {
DigitalBiquadFilter<T>::compute_params(sample_freq, cutoff_freq, _params);
}
// return the cutoff frequency
template <class T>
float LowPassFilter2p<T>::get_cutoff_freq(void) const {
return _params.cutoff_freq;
}
template <class T>
float LowPassFilter2p<T>::get_sample_freq(void) const {
return _params.sample_freq;
}
template <class T>
T LowPassFilter2p<T>::apply(const T &sample) {
return _filter.apply(sample, _params);
}
/*
* Make an instances
* Otherwise we have to move the constructor implementations to the header file :P
*/
template class LowPassFilter2p<int>;
template class LowPassFilter2p<long>;
template class LowPassFilter2p<float>;
template class LowPassFilter2p<double>;
template class LowPassFilter2p<Vector2f>;
template class LowPassFilter2p<Vector3f>;

117
libraries/Filter/LowPassFilter2p.h
View File

@ -18,17 +18,17 @@
#ifndef LOWPASSFILTER2P_H
#define LOWPASSFILTER2P_H
#include <AP_Math/AP_Math.h>
#include <math.h>
#include <inttypes.h>
/// @file LowPassFilter2p.h
/// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com>
class DigitalBiquadFilter
{
/// @authors: Leonard Hall <LeonardTHall@gmail.com>, template implmentation: Daniel Frenzel <dgdanielf@gmail.com>
template <class T>
class DigitalBiquadFilter {
public:
DigitalBiquadFilter() :
_delay_element_1(0.0f),
_delay_element_2(0.0f){}
struct biquad_params {
float cutoff_freq;
float sample_freq;
@ -38,84 +38,59 @@ public:
float b1;
float b2;
};
DigitalBiquadFilter();
float apply(float sample, const struct biquad_params &params);
void reset() { _delay_element_1 = _delay_element_2 = 0.0f; }
T apply(const T &sample, const struct biquad_params &params);
void reset();
static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret);
private:
float _delay_element_1;
float _delay_element_2;
T _delay_element_1;
T _delay_element_2;
};
class LowPassFilter2p
{
template <class T>
class LowPassFilter2p {
public:
LowPassFilter2p() { memset(&_params, 0, sizeof(_params)); }
LowPassFilter2p();
// constructor
LowPassFilter2p(float sample_freq, float cutoff_freq) {
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
}
LowPassFilter2p(float sample_freq, float cutoff_freq);
// change parameters
void set_cutoff_frequency(float sample_freq, float cutoff_freq) {
DigitalBiquadFilter::compute_params(sample_freq, cutoff_freq, _params);
}
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
// return the cutoff frequency
float get_cutoff_freq(void) const {
return _params.cutoff_freq;
}
float get_sample_freq(void) const {
return _params.sample_freq;
}
float get_cutoff_freq(void) const;
float get_sample_freq(void) const;
T apply(const T &sample);
protected:
struct DigitalBiquadFilter::biquad_params _params;
};
class LowPassFilter2pfloat : public LowPassFilter2p
{
public:
LowPassFilter2pfloat() :
LowPassFilter2p() {}
LowPassFilter2pfloat(float sample_freq, float cutoff_freq):
LowPassFilter2p(sample_freq,cutoff_freq) {}
float apply(float sample) {
return _filter.apply(sample, _params);
}
struct DigitalBiquadFilter<T>::biquad_params _params;
private:
DigitalBiquadFilter _filter;
DigitalBiquadFilter<T> _filter;
};
class LowPassFilter2pVector3f : public LowPassFilter2p
{
public:
LowPassFilter2pVector3f() :
LowPassFilter2p() {}
// Uncomment this, if you decide to remove the instantiations in the implementation file
/*
template <class T>
LowPassFilter2p<T>::LowPassFilter2p() {
memset(&_params, 0, sizeof(_params) );
}
LowPassFilter2pVector3f(float sample_freq, float cutoff_freq) :
LowPassFilter2p(sample_freq,cutoff_freq) {}
// constructor
template <class T>
LowPassFilter2p<T>::LowPassFilter2p(float sample_freq, float cutoff_freq) {
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
}
*/
Vector3f apply(const Vector3f &sample) {
Vector3f ret;
ret.x = _filter_x.apply(sample.x, _params);
ret.y = _filter_y.apply(sample.y, _params);
ret.z = _filter_z.apply(sample.z, _params);
return ret;
}
private:
DigitalBiquadFilter _filter_x;
DigitalBiquadFilter _filter_y;
DigitalBiquadFilter _filter_z;
};
typedef LowPassFilter2p<int> LowPassFilter2pInt;
typedef LowPassFilter2p<long> LowPassFilter2pLong;
typedef LowPassFilter2p<float> LowPassFilter2pFloat;
typedef LowPassFilter2p<double> LowPassFilter2pDouble;
typedef LowPassFilter2p<Vector2f> LowPassFilter2pVector2f;
typedef LowPassFilter2p<Vector3f> LowPassFilter2pVector3f;
#endif // LOWPASSFILTER2P_H
#endif // LOWPASSFILTER2P_H