Drift correction for sensor readings using a high-pass filter

Question

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

Answer 1

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

Answer 2

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.