Added a complementary filter to rangefinder data (for 3.6)

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

@@ -63,6 +63,7 @@
 #include <AP_Buffer/AP_Buffer.h>          // APM FIFO Buffer
 #include <AP_Relay/AP_Relay.h>           // APM relay
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
+#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
@@ -249,6 +250,8 @@ 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
+        float filter_hz;
     } rangefinder_state = { false, false, 0};
 
 #if RPM_ENABLED == ENABLED

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

ArduCopter/sensors.cpp

@@ -53,7 +53,48 @@ 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 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;
+    }
+
+    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
+    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
+    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_state.filter_hz);
+
+    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();

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