rangefinder complementary filter for 4.0 (#176)

* Filter: added reset with value to LowPassFilter2p

* Filter: added a ComplementaryFilter library

* Copter: use a complementary filter for rangefinder data

this should reduce the impact of noise in the rangefinder data. Filter
cutoff frequency is low in loiter and higher otherwise

Co-authored-by: Alice Zielman <[email protected]>

Author: tridge

ArduCopter/APM_Config.h

@@ -5,7 +5,8 @@
 
 #define ARMING_DELAY_SEC 3.0f
 
-#define RANGEFINDER_WPNAV_FILT_HZ 3.0f
+#define RANGEFINDER_WPNAV_FILT_MIN_HZ 0.05
+#define RANGEFINDER_WPNAV_FILT_MAX_HZ 0.5
 
 #define LAND_DETECTOR_TRIGGER_SEC 0.5f
 

ArduCopter/Copter.h

@@ -49,6 +49,7 @@
 #include <AP_Motors/AP_Motors.h>          // AP Motors library
 #include <AP_Stats/AP_Stats.h>     // statistics library
 #include <Filter/Filter.h>             // Filter library
+#include <Filter/ComplementaryFilter.h>
 #include <AP_Airspeed/AP_Airspeed.h>        // needed for AHRS build
 #include <AP_Vehicle/AP_Vehicle.h>         // needed for AHRS build
 #include <AP_InertialNav/AP_InertialNav.h>     // ArduPilot Mega inertial navigation library
@@ -281,7 +282,8 @@ class Copter : public AP_Vehicle {
         int16_t alt_cm;     // tilt compensated altitude (in cm) from rangefinder
         float inertial_alt_cm; // inertial alt at time of last rangefinder sample
         uint32_t last_healthy_ms;
-        LowPassFilterFloat alt_cm_filt; // altitude filter
+        ComplementaryFilter alt_cm_filt; // altitude filter
+        float filter_hz;
         int16_t alt_cm_glitch_protected;    // last glitch protected altitude
         int8_t glitch_count;    // non-zero number indicates rangefinder is glitching
         uint32_t glitch_cleared_ms; // system time glitch cleared

ArduCopter/mode.h

@@ -366,6 +366,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; }
+    
     AP_Mission mission{
         FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command &),
         FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command, bool, const AP_Mission::Mission_Command &),
@@ -1470,4 +1473,4 @@ class ModeAutorotate : public Mode {
     void warning_message(uint8_t message_n);    //Handles output messages to the terminal
 
 };
-#endif
+#endif

ArduCopter/sensors.cpp

@@ -83,9 +83,44 @@ void Copter::read_rangefinder(void)
         if (rf_state.alt_healthy) {
             if (timed_out) {
                 // reset filter if we haven't used it within the last second
-                rf_state.alt_cm_filt.reset(rf_state.alt_cm);
+                rf_state.alt_cm_filt.reset();
             } else {
-                rf_state.alt_cm_filt.apply(rf_state.alt_cm, 0.05f);
+                /*
+                  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 == Mode::Number::AUTO &&
+                    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;
+                }
+
+                if (!is_equal(rangefinder_state.filter_hz, target_filter_hz)) {
+                    rf_state.alt_cm_filt.set_cutoff_frequency(target_filter_hz);
+                    rf_state.alt_cm_filt.reset();
+                    rangefinder_state.filter_hz = target_filter_hz;
+                }
+
+                rf_state.alt_cm_filt.apply(rf_state.alt_cm, inertial_nav.get_altitude(), AP_HAL::micros());
+
+                // temporary debug log message to look at the
+                // performance of the complementary filter
+                AP::logger().Write("RF2", "TimeUS,LF,HF,Out,FHz", "Qffff",
+                                   AP_HAL::micros64(),
+                                   rf_state.alt_cm*0.01,
+                                   inertial_nav.get_altitude()*0.01,
+                                   rf_state.alt_cm_filt.get()*0.01,
+                                   rf_state.filter_hz);
+
             }
             rf_state.last_healthy_ms = now;
         }

libraries/Filter/ComplementaryFilter.cpp

@@ -0,0 +1,90 @@
+/*
+   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/>.
+ */
+
+#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()
+{
+    lp.reset();
+    hp.reset();
+}
+
+/*
+  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();
+        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();
+    }
+    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;
+}

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_Math/AP_Math.h>
+#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 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<float>::biquad_params params;
+    DigitalBiquadFilter<float> lp, hp;
+    float out;
+};
+
+

libraries/Filter/LowPassFilter2p.cpp

@@ -17,6 +17,11 @@ T DigitalBiquadFilter<T>::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<T>::apply(const T &sample, const struct biquad_params &par
 template <class T>
 void DigitalBiquadFilter<T>::reset() { 
     _delay_element_1 = _delay_element_2 = T();
+    initialised = false;
+}
+
+template <class T>
+void DigitalBiquadFilter<T>::reset(const T &value, const struct biquad_params &params) {
+    _delay_element_1 = _delay_element_2 = value * (1.0 / (1 + params.a1 + params.a2));
+    initialised = true;
 }
 
 template <class T>
@@ -99,6 +111,11 @@ void LowPassFilter2p<T>::reset(void) {
     return _filter.reset();
 }
 
+template <class T>
+void LowPassFilter2p<T>::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
@@ -108,3 +125,4 @@ template class LowPassFilter2p<long>;
 template class LowPassFilter2p<float>;
 template class LowPassFilter2p<Vector2f>;
 template class LowPassFilter2p<Vector3f>;
+template class DigitalBiquadFilter<float>;

libraries/Filter/LowPassFilter2p.h

@@ -39,11 +39,13 @@ class DigitalBiquadFilter {
 
     T apply(const T &sample, const struct biquad_params &params);
     void reset();
+    void reset(const T &value, const struct biquad_params &params);
     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 <class T>
@@ -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<T>::biquad_params _params;

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);
     }