fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib: merge LowPassFilter2p, LowPassFilter2pArray, and LowPassFilter2pVector3f

Browse Source 
- fix reset for direct form 2
 - push NAN checks out to filter users
This commit is contained in:
Daniel Agar
2021-05-31 10:26:45 -04:00
parent 2a792ca201
commit 5493d96d17
23 changed files with 210 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Functions.hpp
+11
View File
@@ -41,6 +41,7 @@
#include "Limits.hpp"
#include <px4_platform_common/defines.h>
#include <matrix/matrix/math.hpp>
namespace math
@@ -223,4 +224,14 @@ constexpr int16_t negate<int16_t>(int16_t value)
return (value == INT16_MIN) ? INT16_MAX : -value;
}
inline bool isFinite(const float &value)
{
return PX4_ISFINITE(value);
}
inline bool isFinite(const matrix::Vector3f &value)
{
return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));
}
} /* namespace math */
src/lib/mathlib/math/filter/LowPassFilter2p.cpp
-90
View File
@@ -1,90 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "LowPassFilter2p.hpp"
#include <math.h>
namespace math
{
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
_sample_freq = sample_freq;
// reset delay elements on filter change
_delay_element_1 = 0.0f;
_delay_element_2 = 0.0f;
if (_cutoff_freq <= 0.0f) {
// no filtering
_b0 = 1.0f;
_b1 = 0.0f;
_b2 = 0.0f;
_a1 = 0.0f;
_a2 = 0.0f;
return;
}
const float fr = sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr);
const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
_b0 = ohm * ohm / c;
_b1 = 2.0f * _b0;
_b2 = _b0;
_a1 = 2.0f * (ohm * ohm - 1.0f) / c;
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}
float LowPassFilter2p::reset(float sample)
{
const float dval = sample / (_b0 + _b1 + _b2);
if (PX4_ISFINITE(dval)) {
_delay_element_1 = dval;
_delay_element_2 = dval;
} else {
_delay_element_1 = sample;
_delay_element_2 = sample;
}
return apply(sample);
}
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+92 -20
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,21 +31,24 @@
*
****************************************************************************/
/// @file LowPassFilter.h
/// @file LowPassFilter2p.hpp
/// @brief A class to implement a second order low pass filter
/// Author: Leonard Hall <LeonardTHall@gmail.com>
/// Adapted for PX4 by Andrew Tridgell
#pragma once
#include <px4_platform_common/defines.h>
#include <mathlib/math/Functions.hpp>
#include <float.h>
#include <matrix/math.hpp>
namespace math
{
class __EXPORT LowPassFilter2p
template<typename T>
class LowPassFilter2p
{
public:
LowPassFilter2p(float sample_freq, float cutoff_freq)
{
// set initial parameters
@@ -53,24 +56,49 @@ public:
}
// Change filter parameters
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
void set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2)
|| !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
disable();
return;
}
// reset delay elements on filter change
_delay_element_1 = {};
_delay_element_2 = {};
_cutoff_freq = math::constrain(cutoff_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
_sample_freq = sample_freq;
const float fr = sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr);
const float c = 1.f + 2.f * cosf(M_PI_F / 4.f) * ohm + ohm * ohm;
_b0 = ohm * ohm / c;
_b1 = 2.f * _b0;
_b2 = _b0;
_a1 = 2.f * (ohm * ohm - 1.f) / c;
_a2 = (1.f - 2.f * cosf(M_PI_F / 4.f) * ohm + ohm * ohm) / c;
if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) {
disable();
}
}
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
inline float apply(float sample)
inline T apply(const T &sample)
{
// Direct Form II implementation
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
T delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
if (!PX4_ISFINITE(delay_element_0)) {
// don't allow bad values to propagate via the filter
delay_element_0 = sample;
}
const float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
const T output{delay_element_0 *_b0 + _delay_element_1 *_b1 + _delay_element_2 * _b2};
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
@@ -78,19 +106,63 @@ public:
return output;
}
// Filter array of samples in place using the Direct form II.
inline void applyArray(T samples[], int num_samples)
{
for (int n = 0; n < num_samples; n++) {
samples[n] = apply(samples[n]);
}
}
// Return the cutoff frequency
float get_cutoff_freq() const { return _cutoff_freq; }
// Return the sample frequency
float get_sample_freq() const { return _sample_freq; }
float getMagnitudeResponse(float frequency) const;
// Reset the filter state to this value
float reset(float sample);
T reset(const T &sample)
{
const T input = isFinite(sample) ? sample : T{};
if (fabsf(1 + _a1 + _a2) > FLT_EPSILON) {
_delay_element_1 = _delay_element_2 = input / (1 + _a1 + _a2);
if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) {
_delay_element_1 = _delay_element_2 = input;
}
} else {
_delay_element_1 = _delay_element_2 = input;
}
return apply(input);
}
void disable()
{
// no filtering
_sample_freq = 0.f;
_cutoff_freq = 0.f;
_delay_element_1 = {};
_delay_element_2 = {};
_b0 = 1.f;
_b1 = 0.f;
_b2 = 0.f;
_a1 = 0.f;
_a2 = 0.f;
}
protected:
T _delay_element_1{}; // buffered sample -1
T _delay_element_2{}; // buffered sample -2
float _cutoff_freq{0.f};
float _sample_freq{0.f};
// All the coefficients are normalized by a0, so a0 becomes 1 here
float _a1{0.f};
float _a2{0.f};
@@ -98,8 +170,8 @@ protected:
float _b1{0.f};
float _b2{0.f};
float _delay_element_1{0.f}; // buffered sample -1
float _delay_element_2{0.f}; // buffered sample -2
float _cutoff_freq{0.f};
float _sample_freq{0.f};
};
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp
-75
View File
@@ -1,75 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/// @file LowPassFilter2pArray.hpp
/// @brief A class to implement a second order low pass filter
#pragma once
#include "LowPassFilter2p.hpp"
#include <px4_platform_common/defines.h>
namespace math
{
class LowPassFilter2pArray : public LowPassFilter2p
{
public:
LowPassFilter2pArray(float sample_freq, float cutoff_freq) : LowPassFilter2p(sample_freq, cutoff_freq)
{
}
// Filter array of samples in place using the Direct form II.
inline void apply(float samples[], uint8_t num_samples)
{
for (int n = 0; n < num_samples; n++) {
// Direct Form II implementation
float delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2};
// don't allow bad values to propagate via the filter
if (!PX4_ISFINITE(delay_element_0)) {
delay_element_0 = samples[n];
}
samples[n] = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
}
}
};
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2pVector3f.cpp
-92
View File
@@ -1,92 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "LowPassFilter2pVector3f.hpp"
#include <px4_platform_common/defines.h>
#include <math.h>
namespace math
{
void LowPassFilter2pVector3f::set_cutoff_frequency(float sample_freq, float cutoff_freq)
{
_cutoff_freq = cutoff_freq;
_sample_freq = sample_freq;
// reset delay elements on filter change
_delay_element_1.zero();
_delay_element_2.zero();
if (_cutoff_freq <= 0.0f) {
// no filtering
_b0 = 1.0f;
_b1 = 0.0f;
_b2 = 0.0f;
_a1 = 0.0f;
_a2 = 0.0f;
return;
}
const float fr = sample_freq / _cutoff_freq;
const float ohm = tanf(M_PI_F / fr);
const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;
_b0 = ohm * ohm / c;
_b1 = 2.0f * _b0;
_b2 = _b0;
_a1 = 2.0f * (ohm * ohm - 1.0f) / c;
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;
}
matrix::Vector3f LowPassFilter2pVector3f::reset(const matrix::Vector3f &sample)
{
const matrix::Vector3f dval = sample / (_b0 + _b1 + _b2);
if (PX4_ISFINITE(dval(0)) && PX4_ISFINITE(dval(1)) && PX4_ISFINITE(dval(2))) {
_delay_element_1 = dval;
_delay_element_2 = dval;
} else {
_delay_element_1 = sample;
_delay_element_2 = sample;
}
return apply(sample);
}
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp
-99
View File
@@ -1,99 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/// @file LowPassFilter2pVector3f.hpp
/// @brief A class to implement a second order low pass filter on a Vector3f
/// Based on LowPassFilter2p.hpp by Leonard Hall <LeonardTHall@gmail.com>
#pragma once
#include <matrix/math.hpp>
namespace math
{
class LowPassFilter2pVector3f
{
public:
LowPassFilter2pVector3f(float sample_freq, float cutoff_freq)
{
// set initial parameters
set_cutoff_frequency(sample_freq, cutoff_freq);
}
// Change filter parameters
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
/**
* Add a new raw value to the filter
*
* @return retrieve the filtered result
*/
inline matrix::Vector3f apply(const matrix::Vector3f &sample)
{
// do the filtering
const matrix::Vector3f delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
const matrix::Vector3f output{delay_element_0 *_b0 + _delay_element_1 *_b1 + _delay_element_2 * _b2};
_delay_element_2 = _delay_element_1;
_delay_element_1 = delay_element_0;
return output;
}
// Return the cutoff frequency
float get_cutoff_freq() const { return _cutoff_freq; }
// Return the sample frequency
float get_sample_freq() const { return _sample_freq; }
// Reset the filter state to this value
matrix::Vector3f reset(const matrix::Vector3f &sample);
private:
float _cutoff_freq{0.0f};
float _sample_freq{0.0f};
float _a1{0.0f};
float _a2{0.0f};
float _b0{0.0f};
float _b1{0.0f};
float _b2{0.0f};
matrix::Vector3f _delay_element_1{0.0f, 0.0f, 0.0f}; // buffered sample -1
matrix::Vector3f _delay_element_2{0.0f, 0.0f, 0.0f}; // buffered sample -2
};
} // namespace math
src/lib/mathlib/math/filter/LowPassFilter2pVector3fTest.cpp
+2 -2
View File
@@ -40,7 +40,7 @@
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/defines.h>
#include "LowPassFilter2pVector3f.hpp"
#include "LowPassFilter2p.hpp"
using matrix::Vector3f;
@@ -49,7 +49,7 @@ class LowPassFilter2pVector3fTest : public ::testing::Test
public:
void runSimulatedFilter(const Vector3f &signal_freq_hz, const Vector3f &phase_delay_deg, const Vector3f &gain_db);
math::LowPassFilter2pVector3f _lpf{800.f, 30.f};
math::LowPassFilter2p<Vector3f> _lpf{800.f, 30.f};
const float _epsilon_near = 0.08f;
};
src/lib/mathlib/math/filter/NotchFilter.hpp
+1 -12
View File
@@ -42,7 +42,7 @@
#pragma once
#include <px4_platform_common/defines.h>
#include <mathlib/math/Functions.hpp>
#include <cmath>
#include <float.h>
#include <matrix/math.hpp>
@@ -50,16 +50,6 @@
namespace math
{
inline bool isFinite(const float &value)
{
return PX4_ISFINITE(value);
}
inline bool isFinite(const matrix::Vector3f &value)
{
return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));
}
template<typename T>
class NotchFilter
{
@@ -67,7 +57,6 @@ public:
NotchFilter() = default;
~NotchFilter() = default;
void setParameters(float sample_freq, float notch_freq, float bandwidth);
/**