From 5771f116ca2fe9ff75df20643b68033367c34f64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 9 Jan 2021 21:04:58 +1100 Subject: [PATCH 1/7] Filter: added reset with value to LowPassFilter2p --- libraries/Filter/LowPassFilter2p.cpp | 17 +++++++++++++++++ libraries/Filter/LowPassFilter2p.h | 3 +++ .../LowPassFilter2p/LowPassFilter2p.cpp | 10 +++------- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp index 653c3a4a62..93f9bb86ef 100644 --- a/libraries/Filter/LowPassFilter2p.cpp +++ b/libraries/Filter/LowPassFilter2p.cpp @@ -17,6 +17,11 @@ T DigitalBiquadFilter::apply(const T &sample, const struct biquad_params &par return sample; } + if (!initialised) { + reset(sample, params); + initialised = true; + } + 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; @@ -29,6 +34,13 @@ T DigitalBiquadFilter::apply(const T &sample, const struct biquad_params &par template void DigitalBiquadFilter::reset() { _delay_element_1 = _delay_element_2 = T(); + initialised = false; +} + +template +void DigitalBiquadFilter::reset(const T &value, const struct biquad_params ¶ms) { + _delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2)); + initialised = true; } template @@ -99,6 +111,11 @@ void LowPassFilter2p::reset(void) { return _filter.reset(); } +template +void LowPassFilter2p::reset(const T &value) { + return _filter.reset(value, _params); +} + /* * Make an instances * Otherwise we have to move the constructor implementations to the header file :P diff --git a/libraries/Filter/LowPassFilter2p.h b/libraries/Filter/LowPassFilter2p.h index 65ed7c8ba4..5197f97579 100644 --- a/libraries/Filter/LowPassFilter2p.h +++ b/libraries/Filter/LowPassFilter2p.h @@ -39,11 +39,13 @@ class DigitalBiquadFilter { T apply(const T &sample, const struct biquad_params ¶ms); void reset(); + void reset(const T &value, const struct biquad_params ¶ms); static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret); private: T _delay_element_1; T _delay_element_2; + bool initialised; }; template @@ -59,6 +61,7 @@ class LowPassFilter2p { float get_sample_freq(void) const; T apply(const T &sample); void reset(void); + void reset(const T &value); protected: struct DigitalBiquadFilter::biquad_params _params; diff --git a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp index 4347c703e3..bdbe5a7c26 100644 --- a/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp +++ b/libraries/Filter/examples/LowPassFilter2p/LowPassFilter2p.cpp @@ -26,16 +26,12 @@ void loop() for(int16_t i = 0; i < 300; i++ ) { // new data value - const float new_value = sinf((float)i * 2 * M_PI * 5 / 50.0f); // 5hz - - // output to user - hal.console->printf("applying: %6.4f", (double)new_value); - + const float new_value = 17 + sinf((float)i * 2 * M_PI * 5 / 50.0f); // 5hz // apply new value and retrieved filtered result const float filtered_value = low_pass_filter.apply(new_value); - // display results - hal.console->printf("\toutput: %6.4f\n", (double)filtered_value); + // output to user + hal.console->printf("applying: %6.4f -> %6.4f\n", (double)new_value, filtered_value); hal.scheduler->delay(10); } From 7958d2d6c129ab4d8dfe0b8085ed5d1062d7f677 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Feb 2021 11:36:53 +1100 Subject: [PATCH 2/7] Filter: added a ComplementaryFilter library --- libraries/Filter/ComplementaryFilter.cpp | 91 ++++++++++++++++++++++++ libraries/Filter/ComplementaryFilter.h | 45 ++++++++++++ libraries/Filter/LowPassFilter2p.cpp | 1 + 3 files changed, 137 insertions(+) create mode 100644 libraries/Filter/ComplementaryFilter.cpp create mode 100644 libraries/Filter/ComplementaryFilter.h diff --git a/libraries/Filter/ComplementaryFilter.cpp b/libraries/Filter/ComplementaryFilter.cpp new file mode 100644 index 0000000000..fcae972fac --- /dev/null +++ b/libraries/Filter/ComplementaryFilter.cpp @@ -0,0 +1,91 @@ +/* + 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 . + */ + +#include "ComplementaryFilter.h" + +/* + complementary filter. Used to combine a reading that is long term + stable but has high freq noise with another reading that has less + high frequency noise and poor long term stability. + */ + +/* + set crossover frequency between low frequency and high frequency + components + */ +void ComplementaryFilter::set_cutoff_frequency(float freq_hz) +{ + cutoff_freq = freq_hz; +} + +/* + reset the filter to initial values + */ +void ComplementaryFilter::reset(float low_freq, float high_freq) +{ + lp.reset(low_freq, params); + hp.reset(high_freq, params); + out = low_freq; +} + +/* + apply a low freqency and high freqency input to give a complementary + filter result where signal below the cutoff comes from the low_freq + input and above the cutoff from high freqency input. We take a + timestamp on each input as the input data may vary somewhat in + frequency + */ +float ComplementaryFilter::apply(float low_freq, float high_freq, uint32_t time_us) +{ + if (!is_positive(cutoff_freq)) { + reset(low_freq, high_freq); + return out; + } + const uint32_t dt_us = time_us - last_sample_us; + + if (dt_us > 1e6) { + // if we have not updated for 1s then assume we've had a + // sensor outage and reset + reset(low_freq, high_freq); + } + last_sample_us = time_us; + + const float dt = MIN(dt_us * 1.0e-6, 1); + + // keep a low pass filter of the sample rate. Rapidly changing + // sample frequency in bi-quad filters leads to very nasty spikes + if (!is_positive(sample_freq)) { + sample_freq = 1.0/dt; + } else { + sample_freq = 0.99 * sample_freq + 0.01 * (1.0/dt); + } + + lp.compute_params(sample_freq, cutoff_freq, params); + + float hp_out = hp.apply(high_freq, params); + float lp_out = lp.apply(low_freq, params); + + out = (high_freq - hp_out) + lp_out; + + return out; +} + +/* + return the current value + */ +float ComplementaryFilter::get(void) +{ + return out; +} diff --git a/libraries/Filter/ComplementaryFilter.h b/libraries/Filter/ComplementaryFilter.h new file mode 100644 index 0000000000..97e8dd5115 --- /dev/null +++ b/libraries/Filter/ComplementaryFilter.h @@ -0,0 +1,45 @@ +/* + 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 . + */ + +#pragma once + +#include +#include "LowPassFilter2p.h" + +/* + complementary filter. Used to combine a reading that is long term + stable but has high freq noise with another reading that has less + high frequency noise and poor long term stability. + */ + +class ComplementaryFilter +{ +public: + void set_cutoff_frequency(float freq_hz); + void reset(float low_freq, float high_freq); + float apply(float low_freq, float high_freq, uint32_t time_us); + float get(void); + +private: + uint32_t last_sample_us; + float cutoff_freq; + float sample_freq; + // use 2-pole low pass filters to get a reasonably sharp cutoff + DigitalBiquadFilter::biquad_params params; + DigitalBiquadFilter lp, hp; + float out; +}; + + diff --git a/libraries/Filter/LowPassFilter2p.cpp b/libraries/Filter/LowPassFilter2p.cpp index 93f9bb86ef..e443d83787 100644 --- a/libraries/Filter/LowPassFilter2p.cpp +++ b/libraries/Filter/LowPassFilter2p.cpp @@ -125,3 +125,4 @@ template class LowPassFilter2p; template class LowPassFilter2p; template class LowPassFilter2p; template class LowPassFilter2p; +template class DigitalBiquadFilter; From 3bca7166ad8869c978d56489ee0ae872955b7f27 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 26 Feb 2021 13:05:17 +1100 Subject: [PATCH 3/7] Copter: use a 0.25Hz complementary filter for rangefinder data this should reduce the impact of noise in the rangefinder data --- ArduCopter/APM_Config.h | 2 +- ArduCopter/Copter.h | 2 ++ ArduCopter/sensors.cpp | 14 +++++++++++++- 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index d1e0a8dc55..ee3bae7917 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -5,7 +5,7 @@ #define ARMING_DELAY_SEC 3.0f -#define RANGEFINDER_WPNAV_FILT_HZ 3.0f +#define RANGEFINDER_WPNAV_FILT_HZ 0.25 #define LAND_DETECTOR_TRIGGER_SEC 0.5f diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 7242bdac9d..e3da5a256f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -63,6 +63,7 @@ #include // APM FIFO Buffer #include // APM relay #include +#include #include // needed for AHRS build #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library @@ -249,6 +250,7 @@ class Copter : public AP_HAL::HAL::Callbacks { uint32_t last_healthy_ms; float terrain_height_filt_cm; // terrain height relative to origin, filtered int8_t glitch_count; + ComplementaryFilter alt_cm_filt; // altitude filter } rangefinder_state = { false, false, 0}; #if RPM_ENABLED == ENABLED diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 35b537e115..130aec27a3 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -53,7 +53,19 @@ void Copter::read_rangefinder(void) // Remove rangefinder body offset - converting NED to NEU rangefinder_alt_meas += (ahrs.get_rotation_body_to_ned()*rangefinder_pos_offset_cm).z; - float terrain_height_meas = inertial_nav.get_altitude() - rangefinder_alt_meas; + // apply complementary filter + rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); + float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros()); + + // temporary debug log message to look at the + // performance of the complementary filter + DataFlash_Class::instance()->Log_Write("RF2", "TimeUS,LF,HF,Out", "Qfff", + AP_HAL::micros64(), + rangefinder_alt_meas*0.01, + inertial_nav.get_altitude()*0.01, + rangefinder_alt_flt*0.01); + + float terrain_height_meas = inertial_nav.get_altitude() - rangefinder_alt_flt; // filter rangefinder for use by AC_WPNav uint32_t tnow_ms = AP_HAL::millis(); From 53caa4e0b15f7f9e835b9125f857e5bcc930cef2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 3 Mar 2021 17:00:02 +1100 Subject: [PATCH 4/7] Copter: apply a slew to the complementary filter slew the time constant of the filter when in a loiter towards a much longer time constant to avoid long period noise from the rangefinder --- ArduCopter/APM_Config.h | 3 ++- ArduCopter/Copter.h | 1 + ArduCopter/mode.h | 3 +++ ArduCopter/sensors.cpp | 30 +++++++++++++++++++++++++++--- 4 files changed, 33 insertions(+), 4 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index ee3bae7917..79843b12e2 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -5,7 +5,8 @@ #define ARMING_DELAY_SEC 3.0f -#define RANGEFINDER_WPNAV_FILT_HZ 0.25 +#define RANGEFINDER_WPNAV_FILT_MIN_HZ 0.05 +#define RANGEFINDER_WPNAV_FILT_MAX_HZ 0.5 #define LAND_DETECTOR_TRIGGER_SEC 0.5f diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e3da5a256f..e57f571482 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -251,6 +251,7 @@ class Copter : public AP_HAL::HAL::Callbacks { float terrain_height_filt_cm; // terrain height relative to origin, filtered int8_t glitch_count; ComplementaryFilter alt_cm_filt; // altitude filter + float filter_hz; } rangefinder_state = { false, false, 0}; #if RPM_ENABLED == ENABLED diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 243c0f1e6c..73cb9c9264 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -305,6 +305,9 @@ class ModeAuto : public Mode { // for GCS_MAVLink to call: bool do_guided(const AP_Mission::Mission_Command& cmd); + // are we in a loiter? + bool in_loiter(void) const { return loiter_time != 0; } + protected: const char *name() const override { return "AUTO"; } diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 130aec27a3..7e0a95a112 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -53,17 +53,41 @@ void Copter::read_rangefinder(void) // Remove rangefinder body offset - converting NED to NEU rangefinder_alt_meas += (ahrs.get_rotation_body_to_ned()*rangefinder_pos_offset_cm).z; + /* + Apply a complementary filter to the rangefinder data, using the + inertial altitude for the high frequency component and the + rangefinder data for the low frequency component. + + When we are loitering shift the time constant of the + complementary filter towards a much lower frequency to filter + out even quite long period noise from the rangefinder. When not + loitering shift back to a shorter time period to handle changes + in terrain + */ + float target_filter_hz = RANGEFINDER_WPNAV_FILT_MAX_HZ; + if (control_mode == AUTO && + mission.get_current_nav_cmd().id == MAV_CMD_NAV_WAYPOINT && + mode_auto.in_loiter()) { + // we are holding position at a waypoint, slew the time constant + target_filter_hz = RANGEFINDER_WPNAV_FILT_MIN_HZ; + } + + // prevent sharp changes to the filter time constant + const float alt_filter_slew_alpha = 0.98; + rangefinder_state.filter_hz = rangefinder_state.filter_hz * alt_filter_slew_alpha + target_filter_hz * (1-alt_filter_slew_alpha); + // apply complementary filter - rangefinder_state.alt_cm_filt.set_cutoff_frequency(RANGEFINDER_WPNAV_FILT_HZ); + rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz); float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros()); // temporary debug log message to look at the // performance of the complementary filter - DataFlash_Class::instance()->Log_Write("RF2", "TimeUS,LF,HF,Out", "Qfff", + DataFlash_Class::instance()->Log_Write("RF2", "TimeUS,LF,HF,Out,FHz", "Qffff", AP_HAL::micros64(), rangefinder_alt_meas*0.01, inertial_nav.get_altitude()*0.01, - rangefinder_alt_flt*0.01); + rangefinder_alt_flt*0.01, + rangefinder_state.filter_hz); float terrain_height_meas = inertial_nav.get_altitude() - rangefinder_alt_flt; From bd0e4f802264413f74d18e39c8138cb5172cd12c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Apr 2021 10:57:08 +1000 Subject: [PATCH 5/7] Filter: improved reset handling of complementary filter --- libraries/Filter/ComplementaryFilter.cpp | 13 ++++++------- libraries/Filter/ComplementaryFilter.h | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/Filter/ComplementaryFilter.cpp b/libraries/Filter/ComplementaryFilter.cpp index fcae972fac..ecfbc2f1f1 100644 --- a/libraries/Filter/ComplementaryFilter.cpp +++ b/libraries/Filter/ComplementaryFilter.cpp @@ -33,11 +33,10 @@ void ComplementaryFilter::set_cutoff_frequency(float freq_hz) /* reset the filter to initial values */ -void ComplementaryFilter::reset(float low_freq, float high_freq) +void ComplementaryFilter::reset() { - lp.reset(low_freq, params); - hp.reset(high_freq, params); - out = low_freq; + lp.reset(); + hp.reset(); } /* @@ -50,15 +49,15 @@ void ComplementaryFilter::reset(float low_freq, float high_freq) float ComplementaryFilter::apply(float low_freq, float high_freq, uint32_t time_us) { if (!is_positive(cutoff_freq)) { - reset(low_freq, high_freq); - return out; + reset(); + return low_freq; } const uint32_t dt_us = time_us - last_sample_us; if (dt_us > 1e6) { // if we have not updated for 1s then assume we've had a // sensor outage and reset - reset(low_freq, high_freq); + reset(); } last_sample_us = time_us; diff --git a/libraries/Filter/ComplementaryFilter.h b/libraries/Filter/ComplementaryFilter.h index 97e8dd5115..637a5ff6d3 100644 --- a/libraries/Filter/ComplementaryFilter.h +++ b/libraries/Filter/ComplementaryFilter.h @@ -28,7 +28,7 @@ class ComplementaryFilter { public: void set_cutoff_frequency(float freq_hz); - void reset(float low_freq, float high_freq); + void reset(); float apply(float low_freq, float high_freq, uint32_t time_us); float get(void); From 900e56f16adc66c92b1a562825cfae814edcf806 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Apr 2021 10:57:49 +1000 Subject: [PATCH 6/7] Copter: use a filter reset when going to high cutoff in RF filter this prevents a numercial error on increase in filter cutoff frequency --- ArduCopter/sensors.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 7e0a95a112..63afa87492 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -76,6 +76,13 @@ void Copter::read_rangefinder(void) const float alt_filter_slew_alpha = 0.98; rangefinder_state.filter_hz = rangefinder_state.filter_hz * alt_filter_slew_alpha + target_filter_hz * (1-alt_filter_slew_alpha); + if (target_filter_hz > rangefinder_state.filter_hz) { + // when raising target freq we use a reset to prevent a numerical error in the complementary filter + rangefinder_state.filter_hz = target_filter_hz; + rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz); + rangefinder_state.alt_cm_filt.reset(); + } + // apply complementary filter rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz); float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros()); From c09a888b5ebde78e30419363ee99d1710a0cb95a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 3 Jun 2021 07:35:20 +1000 Subject: [PATCH 7/7] Copter: fixed rangefinder filter when out of range don't input invalid data into the filter, allowing it to reset if data is more than 1s old --- ArduCopter/sensors.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 63afa87492..14620856e3 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -72,20 +72,18 @@ void Copter::read_rangefinder(void) target_filter_hz = RANGEFINDER_WPNAV_FILT_MIN_HZ; } - // prevent sharp changes to the filter time constant - const float alt_filter_slew_alpha = 0.98; - rangefinder_state.filter_hz = rangefinder_state.filter_hz * alt_filter_slew_alpha + target_filter_hz * (1-alt_filter_slew_alpha); - - if (target_filter_hz > rangefinder_state.filter_hz) { - // when raising target freq we use a reset to prevent a numerical error in the complementary filter + if (!is_equal(target_filter_hz,rangefinder_state.filter_hz)) { + // when changing target freq we use a reset to prevent a numerical error in the complementary filter rangefinder_state.filter_hz = target_filter_hz; rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz); rangefinder_state.alt_cm_filt.reset(); } // apply complementary filter - rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz); - float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros()); + if (rangefinder_alt_meas_valid) { + rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros()); + } + const float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.get(); // temporary debug log message to look at the // performance of the complementary filter