6
\$\begingroup\$

An embedded project my team is working on is having issues with sensor drift over time. To solve this, I thought it might work to use a high-pass filter, since the portion of the signal that we care about is generally at a significantly higher frequency than the drift. I'd like feedback on if this is a good general approach and if this implementation in particular follows good standards for performance, usability, and maintainability.

Side note: for logging, this project uses ArduinoLog.

drift_correct.hpp:

#include <cstdint>

/**
 * @class DriftCorrector
 * @brief Object offering drift-correction capabilities for real-valued signals
 * @details Uses a high-pass filter to correct for unintended long-term drift in a signal,
 *          assuming that changes below a minimum frequency are to be considered drift
 */
class DriftCorrector
{
private:
    float m_lastSampleUncorrected;
    float m_lastSampleCorrected;
    float m_timeSinceGoodSample; // to accurately keep track of time deltas even when samples
                                 // need to be thrown out
    float m_RC; // time constant (tau)

public:
    /**
     * @param cornerFrequency the maximum frequency (in Hz) of changes that should be
     *                        considered drift
     */
    DriftCorrector(float cornerFrequency = 0.1f) noexcept;
    /**
     * @brief Sends a value through for correction.
     *
     * @param value the uncorrected value, which will also be stored in the history
     * @param timeDelta the time (in seconds) since the last value was stored
     * @returns the value corrected for any drift that has been detected thus far.
     */
    [[nodiscard]] float next(float value, float timeDelta) noexcept;
};

drift_correct.cpp:

#include "drift_correct.hpp"
#include <ArduinoLog.h>

#include <cmath>
#include <limits>

DriftCorrector::DriftCorrector(float cornerFrequency = 0.1f)
{
    if (std::isnan(cornerFrequency) || std::isinf(cornerFrequency) || cornerFrequency <= 0.0f)
    {
        Log.errorln(
            "invalid corner frequency given to drift corrector, substituting with 0.1 Hz");
        cornerFrequency = 0.1f;
    }

    m_lastSampleCorrected = std::numeric_limits<float>::signaling_NaN();
    m_lastSampleUncorrected = std::numeric_limits<float>::signaling_NaN();
    m_timeSinceGoodSample = 0.0f;
    m_RC = 1.0f / (2.0f * PI * cornerFrequency);
}

float DriftCorrector::next(float sample, float timeDelta)
{
    // Algorithm from https://en.wikipedia.org/wiki/High-pass_filter (2023-02-28)

    if (std::isnan(sample) || std::isinf(sample))
    {
        Log.warningln("invalid sample passed into drift correction, ignoring");
        if (!std::isnan(timeDelta) && !std::isinf(timeDelta) && timeDelta > 0.0f)
        {
            m_timeSinceGoodSample += timeDelta;
        }
        return sample;
    }
    else if (std::isnan(timeDelta) || std::isinf(timeDelta) || timeDelta <= 0.0f)
    {
        Log.warningln("invalid time delta passed into drift correction, ignoring");
        return sample;
    }
    timeDelta += m_timeSinceGoodSample;
    m_timeSinceGoodSample = 0.0f;

    if (std::isnan(m_lastSampleCorrected) || std::isnan(m_lastSampleUncorrected))
    {
        // First sample taken in, just pass it straight through
        m_lastSampleCorrected = sample;
        m_lastSampleUncorrected = sample;
        return sample;
    }
    float alpha = m_RC / (m_RC + timeDelta);
    float corrected = alpha * (m_lastSampleCorrected + sample - m_lastSampleUncorrected);
    m_lastSampleCorrected = corrected;
    m_lastSampleUncorrected = sample;
    return corrected;
}

example usage:

DriftCorrector c(0.3f); // corner frequency of 0.3 Hz
for (;;)
{
    std::cout << c.next(sampler.getValue(), timer.sinceLastTick()) << std::endl;
}
\$\endgroup\$
2
  • \$\begingroup\$ This makes no sense. How does reducing the signal gain when below a cutoff (0.3Hz) correct the signals frequency? If the signal drifts too far you will get no signal above the noise. Are you sure you stated the questions title correctly? \$\endgroup\$
    – Blindman67
    Mar 3, 2023 at 15:14
  • \$\begingroup\$ @Blindman67 We're having issues with a sensor whose readings are generally accurate over short timespans but over long periods of time tend to drift away from the baseline significantly. The point of the high-pass filter is to drop out the low-frequency parts of the signal where the drift is found - i.e., since there should be no overall change in the signal over the course of a minute, any frequency component below ~1/60 Hz can (in theory) be ignored as drift. \$\endgroup\$ Mar 4, 2023 at 18:04

2 Answers 2

5
\$\begingroup\$

Not a full review, just some design ideas:

For a realtime system, I'd probably make the sampler object guarantee to produce valid values, rather than forcing all downstream code to check for NAN/INF. If your A/D converter really can produce invalid or missing readings, I'd have get_value take an bool &isValid argument, rather than passing NANs as flags.

Similarly, I'd guarantee that the clock API returns a value that is monotonically increasing, and not ever allow it to return NANs.

If the sample is NAN or the time is invalid, your code prints that it is "ignoring" the value, but then it returns that value anyway. I would return the lastSampleCorrected instead.

I think the timing logic is simpler if you pass the current clock reading into the DriftCorrector, and let it keep track of time_of_last_sample. Initialize it to a negative number as a 'not_yet_run' flag.

The caller becomes:

sample = sampler.getValue(isValid);
if (isValid) { cout << c.next(sample, timer.getTime()) << endl;

And next becomes something like this:

float DriftCorrector::next(float sample, float timeNow)
{
    float corrected = sample;
    if (m_time_of_last_sample < 0) { //first run
        m_time_of_last_sample = timeNow; 
    }
    else {
        float timeDelta = timeNow - m_time_of_last_sample;
        if (timeDelta > 0.0f) {
            float alpha = m_RC / (m_RC + timeDelta);
            corrected = alpha * (m_lastSampleCorrected + sample - m_lastSampleUncorrected);
            m_time_of_last_sample = timeNow; 
        }
        else { //ignore completely
            return m_lastSampleCorrected;
        }
    }
    m_lastSampleUncorrected = sample;
    m_lastSampleCorrected = corrected;
    return corrected;  
}
\$\endgroup\$
2
\$\begingroup\$

AShelly already made some great points.

Use std::isfinite()

Instead of checking whether both std::isnan() and std::isinf() are false, you can just check that std::isfinite() is true.

How exact does your high-pass filter need to be?

You have a lot of error handling in your code, but is it necessary at all? If you can expect that timeDelta is usually around the same value, because you are sampling at a regular interval, then I wouldn't bother looking at the time delta inside next() at all. Instead, pass the expected time interval to the constructor, and have the constructor pre-calculate m_alpha, which then replaces both m_timeSinceGoodSample and m_RC.

The rationale here is that if you are going to have invalid sample values and/or jumps in timeDelta, your corrected sample value is going to be imprecise anyway. Also, depending on what CPU your Arduino has and how often you are going to call next(), you might want to avoid spending a lot of CPU cycles in expensive instructions like floating point divisions.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.