Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:25:31 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

mathlib: Notchfilter updates

Browse Source 
- merge NotchFilterArray into regular NotchFilter (apply vs applyArray)
 - only use direct form 1 to prevent reset confusion
 - safe default field initialization
 - update VehicleAngularVelocity usage
This commit is contained in:
Daniel Agar
2021-05-25 12:14:24 -04:00
parent 055b5404c3
commit f25a70a674
5 changed files with 76 additions and 191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/filter/NotchFilter.hpp
+65 -63
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2019 PX4 Development Team. All rights reserved.
* Copyright (C) 2019-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
@@ -71,32 +71,15 @@ public:
void setParameters(float sample_freq, float notch_freq, float bandwidth);
/**
* Add a new raw value to the filter using the Direct form II
* Add a new raw value to the filter using the Direct Form I
*
* @return retrieve the filtered result
*/
inline T apply(const T &sample)
{
// Direct Form II implementation
const T delay_element_0{sample - _delay_element_1 *_a1 - _delay_element_2 * _a2};
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;
return output;
}
/**
* Add a new raw value to the filter using the Direct Form I
*
* @return retrieve the filtered result
*/
inline T applyDF1(const T &sample)
{
// Direct Form I implementation
const T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 *
_delay_element_output_2;
T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 *
_delay_element_output_2;
// shift inputs
_delay_element_2 = _delay_element_1;
@@ -109,6 +92,14 @@ public:
return output;
}
// Filter array of samples in place using the direct form I
inline void applyArray(T samples[], int num_samples)
{
for (int n = 0; n < num_samples; n++) {
samples[n] = apply(samples[n]);
}
}
float getNotchFreq() const { return _notch_freq; }
float getBandwidth() const { return _bandwidth; }
@@ -150,53 +141,77 @@ public:
_b2 = b[2];
}
void reset(const T &sample)
{
const T input = isFinite(sample) ? sample : T{};
T reset(const T &sample);
_delay_element_1 = _delay_element_2 = input;
_delay_element_output_1 = _delay_element_output_2 = input * (_b0 + _b1 + _b2) / (1 + _a1 + _a2);
if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) {
_delay_element_output_1 = _delay_element_output_2 = {};
}
}
void disable()
{
// no filtering
_notch_freq = 0.f;
_bandwidth = 0.f;
_sample_freq = 0.f;
_delay_element_1 = {};
_delay_element_2 = {};
_delay_element_output_1 = {};
_delay_element_output_2 = {};
_b0 = 1.f;
_b1 = 0.f;
_b2 = 0.f;
_a1 = 0.f;
_a2 = 0.f;
}
protected:
T _delay_element_1{};
T _delay_element_2{};
T _delay_element_output_1{};
T _delay_element_output_2{};
// All the coefficients are normalized by a0, so a0 becomes 1 here
float _a1{0.f};
float _a2{0.f};
float _b0{1.f};
float _b1{0.f};
float _b2{0.f};
float _notch_freq{};
float _bandwidth{};
float _sample_freq{};
// All the coefficients are normalized by a0, so a0 becomes 1 here
float _a1{};
float _a2{};
float _b0{};
float _b1{};
float _b2{};
T _delay_element_1;
T _delay_element_2;
T _delay_element_output_1;
T _delay_element_output_2;
};
/**
* Initialises the filter by setting its parameters and coefficients.
* If using the direct form I (applyDF1) method, allows to dynamically
* Using the direct form I method, allows to dynamically
* update the filtered frequency, refresh rate and quality factor while
* conserving the filter's history
*/
template<typename T>
void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float bandwidth)
{
_notch_freq = notch_freq;
_bandwidth = bandwidth;
_sample_freq = sample_freq;
if (notch_freq <= 0.f) {
// no filtering
_b0 = 1.0f;
_b1 = 0.0f;
_b2 = 0.0f;
_a1 = 0.0f;
_a2 = 0.0f;
if ((sample_freq <= 0.f) || (notch_freq <= 0.f) || (bandwidth <= 0.f) || (notch_freq >= sample_freq / 2)
|| !isFinite(sample_freq) || !isFinite(notch_freq) || !isFinite(bandwidth)) {
disable();
return;
}
_notch_freq = math::constrain(notch_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
_bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2);
_sample_freq = sample_freq;
const float alpha = tanf(M_PI_F * bandwidth / sample_freq);
const float beta = -cosf(2.f * M_PI_F * notch_freq / sample_freq);
const float a0_inv = 1.f / (alpha + 1.f);
@@ -207,23 +222,10 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
_a1 = _b1;
_a2 = (1.f - alpha) * a0_inv;
}
template<typename T>
T NotchFilter<T>::reset(const T &sample)
{
T dval = sample;
if (fabsf(_b0 + _b1 + _b2) > FLT_EPSILON) {
dval = dval / (_b0 + _b1 + _b2);
if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) {
disable();
}
_delay_element_1 = dval;
_delay_element_2 = dval;
_delay_element_output_1 = {};
_delay_element_output_2 = {};
return apply(sample);
}
} // namespace math
src/lib/mathlib/math/filter/NotchFilterArray.hpp
-117
View File
@@ -1,117 +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 NotchFilter.hpp
*
* @brief Notch filter with array input/output
*
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
* @author Samuel Garcin <samuel.garcin@wecorpindustries.com>
*/
#pragma once
#include "NotchFilter.hpp"
namespace math
{
template<typename T>
class NotchFilterArray : public NotchFilter<T>
{
using NotchFilter<T>::_delay_element_1;
using NotchFilter<T>::_delay_element_2;
using NotchFilter<T>::_delay_element_output_1;
using NotchFilter<T>::_delay_element_output_2;
using NotchFilter<T>::_a1;
using NotchFilter<T>::_a2;
using NotchFilter<T>::_b0;
using NotchFilter<T>::_b1;
using NotchFilter<T>::_b2;
public:
NotchFilterArray() = default;
~NotchFilterArray() = default;
// Filter array of samples in place using the Direct form II.
inline void apply(T samples[], uint8_t num_samples)
{
for (int n = 0; n < num_samples; n++) {
// Direct Form II implementation
T delay_element_0{samples[n] - _delay_element_1 *_a1 - _delay_element_2 * _a2};
// don't allow bad values to propagate via the filter
if (!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;
}
}
/**
* Add new raw values to the filter using the Direct form I.
*
* @return retrieve the filtered result
*/
inline void applyDF1(T samples[], uint8_t num_samples)
{
for (int n = 0; n < num_samples; n++) {
// Direct Form II implementation
T output = _b0 * samples[n] + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 -
_a2 * _delay_element_output_2;
// don't allow bad values to propagate via the filter
if (!isFinite(output)) {
output = samples[n];
}
// shift inputs
_delay_element_2 = _delay_element_1;
_delay_element_1 = samples[n];
// shift outputs
_delay_element_output_2 = _delay_element_output_1;
_delay_element_output_1 = output;
// writes value to array
samples[n] = output;
}
}
};
} // namespace math
src/lib/mathlib/math/filter/NotchFilterTest.cpp
+4 -4
View File
@@ -117,7 +117,7 @@ TEST_F(NotchFilterTest, filteringLowSideDF1)
for (int i = 0; i < 1000; i++) {
float input = sinf(omega * t);
float output_expected = sinf(omega * t - phase_delay);
out = _notch_float.applyDF1(input);
out = _notch_float.apply(input);
t = i * dt;
// Let some time for the filter to settle
@@ -167,7 +167,7 @@ TEST_F(NotchFilterTest, filteringHighSideDF1)
for (int i = 0; i < 1000; i++) {
float input = sinf(omega * t);
float output_expected = sinf(omega * t + phase_delay);
out = _notch_float.applyDF1(input);
out = _notch_float.apply(input);
t = i * dt;
// Let some time for the filter to settle
@@ -213,7 +213,7 @@ TEST_F(NotchFilterTest, filterOnNotchDF1)
for (int i = 0; i < 1000; i++) {
float input = sinf(omega * t);
out = _notch_float.applyDF1(input);
out = _notch_float.apply(input);
t = i * dt;
// Let some time for the filter to settle
@@ -282,7 +282,7 @@ TEST_F(NotchFilterTest, filterVector3fDF1)
const Vector3f input(sinf(omega(0) * t), sinf(omega(1) * t), sinf(omega(2) * t));
const Vector3f arg = omega * t + phase_delay;
const Vector3f output_expected(sinf(arg(0)), 0.f, sinf(arg(2)));
out = _notch_vector3f.applyDF1(input);
out = _notch_vector3f.apply(input);
t = i * dt;
// Let some time for the filter to settle
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+3 -3
View File
@@ -545,7 +545,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
for (auto &dnf : _dynamic_notch_filter_esc_rpm) {
for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
if (dnf[harmonic][axis].getNotchFreq() > 0.f) {
dnf[harmonic][axis].applyDF1(data, N);
dnf[harmonic][axis].applyArray(data, N);
} else {
break;
@@ -562,7 +562,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
for (auto &dnf : _dynamic_notch_filter_fft) {
if (dnf[axis].getNotchFreq() > 0.f) {
dnf[axis].applyDF1(data, N);
dnf[axis].applyArray(data, N);
}
}
@@ -573,7 +573,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
// Apply general notch filter (IMU_GYRO_NF_FREQ)
if (_notch_filter_velocity[axis].getNotchFreq() > 0.f) {
_notch_filter_velocity[axis].apply(data, N);
_notch_filter_velocity[axis].applyArray(data, N);
}
// Apply general low-pass filter (IMU_GYRO_CUTOFF)
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+4 -4
View File
@@ -38,7 +38,7 @@
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
#include <lib/mathlib/math/filter/NotchFilterArray.hpp>
#include <lib/mathlib/math/filter/NotchFilter.hpp>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_config.h>
@@ -133,7 +133,7 @@ private:
// angular velocity filters
math::LowPassFilter2pArray _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
math::NotchFilterArray<float> _notch_filter_velocity[3] {};
math::NotchFilter<float> _notch_filter_velocity[3] {};
#if !defined(CONSTRAINED_FLASH)
@@ -148,8 +148,8 @@ private:
static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) / sizeof(
sensor_gyro_fft_s::peak_frequencies_x[0]);
math::NotchFilterArray<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
math::NotchFilterArray<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
math::NotchFilter<float> _dynamic_notch_filter_esc_rpm[MAX_NUM_ESC_RPM][MAX_NUM_ESC_RPM_HARMONICS][3] {};
math::NotchFilter<float> _dynamic_notch_filter_fft[MAX_NUM_FFT_PEAKS][3] {};
perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};