Butterworth Filter in C++, benötige Hilfe bei Feinabstimmung und Fehlersuche

[Michael D. (michael86)]

Ich versuche mich gerade an einem Butterworth Filter in C++ um 
Positions-Daten zu filtern.
Den Filter-Algorithmus habe ich von verschiedenen Github-Seiten und 
selbst daran rummodifiziert.

Es scheint so, als würde der funktionieren, garantieren kann ich es aber 
auch nicht.
Jedenfalls filtert er nicht wie gewünscht. Bei mir ist Input = Output, 
jedenfalls meistens oder ziemlich nahe dran.
Habe schon die Variablen cutoff_frequency_ und sampling_rate_ verändert 
aber hier scheint es mir so zu sein, dass diese nahezu keine Auswirkung 
haben.
Die Messwerte sollten konstant sein (also keine Schwankungen), da das 
Objekt nicht bewegt wurde.

std::vector<double> butterworth_filter(const std::vector<double>& input, double cutoff_frequency, double sampling_rate, int order)
{
    int N = input.size();
    std::vector<double> output(N, 0.0);

    // cf, init
    double nyquist = 0.5 * sampling_rate;
    double normalized_cutoff = cutoff_frequency / nyquist;

    std::vector<double> a(order + 1, 0.0);
    std::vector<double> b(order + 1, 0.0);

    double theta = M_PI * normalized_cutoff;
    double d = 1.0 / std::tan(theta);
    double d_squared = d * d;

    a[0] = 1.0 / (1.0 + std::sqrt(2.0) * d + d_squared);
    a[1] = 2.0 * a[0];
    a[2] = a[0];
    b[1] = 2.0 * (1.0 - d_squared) * a[0];
    b[2] = (1.0 - std::sqrt(2.0) * d + d_squared) * a[0];

    for (int i = 2; i < N; ++i)
    {
#ifdef NDEBUG
#else
        printf("\nFiltering index: %i\n",i);
#endif
        output[i] = a[0] * input[i] + a[1] * input[i - 1] + a[2] * input[i - 2] - b[1] * output[i - 1] - b[2] * output[i - 2];
    }

    return output;
}

class Filter
{
public:
    Filter() {}

    void set_history_size(size_t new_size)
    {
        history_size_ = new_size;
        if (x_history_.size() > history_size_) x_history_.resize(history_size_);
        if (y_history_.size() > history_size_) y_history_.resize(history_size_);
        if (z_history_.size() > history_size_) z_history_.resize(history_size_);
    }

    geometry_msgs::msg::TransformStamped apply_filter(const geometry_msgs::msg::TransformStamped& transform)
    {
        x_history_.push_back(transform.transform.translation.x);
        y_history_.push_back(transform.transform.translation.y);
        z_history_.push_back(transform.transform.translation.z);

        if (x_history_.size() > history_size_) x_history_.pop_front();
        if (y_history_.size() > history_size_) y_history_.pop_front();
        if (z_history_.size() > history_size_) z_history_.pop_front();

#ifdef NDEBUG
#else
        printf("\nHistory Size: %zu", history_size_);
        printf("\nDeque x_history_ size: %zu", x_history_.size());
        printf("\nDeque y_history_ size: %zu", y_history_.size());
        printf("\nDeque z_history_ size: %zu", z_history_.size());
#endif
        geometry_msgs::msg::TransformStamped filtered_transform = transform;

        if (x_history_.size() >= history_size_)
        {
            std::vector<double> x_filtered = butterworth_filter(
                std::vector<double>(x_history_.begin(), x_history_.end()), cutoff_frequency_, sampling_rate_, order_);
            std::vector<double> y_filtered = butterworth_filter(
                std::vector<double>(y_history_.begin(), y_history_.end()), cutoff_frequency_, sampling_rate_, order_);
            std::vector<double> z_filtered = butterworth_filter(
                std::vector<double>(z_history_.begin(), z_history_.end()), cutoff_frequency_, sampling_rate_, order_);

            filtered_transform.transform.translation.x = x_filtered.back();
            filtered_transform.transform.translation.y = y_filtered.back();
            filtered_transform.transform.translation.z = z_filtered.back();
        }

        return filtered_transform;
    }

private:
    std::deque<double> x_history_;
    std::deque<double> y_history_;
    std::deque<double> z_history_;
    size_t history_size_ = 20;
    double cutoff_frequency_ = 10.0;
    double sampling_rate_ = 100.0;
    int order_ = 2;
};

Wo genau liegt der Fehler?
Im Anhang der vollständige Code und ein Mitschnitt der Messdaten.

[Detlef _. (detlef_a)]

Hi. Sieht doch eigentlich schick aus. Der Fehler wird in der Berechnung 
der coff. a und b liegen. Für ein cutoff und samplerate a und b händisch 
berechnen und vergleichen. Btw überall heißen die vorwärtskoeffizienten 
b und die Rückkoppelkoeffizienten a, bei dir umgekehrt.
Cheers
Detlef

[The A. (the_a343)]

Hi,

immer schwer zu sagen, woran es fehlt ... :-O

ich benutze die CMSIS Funktionen
https://arm-software.github.io/CMSIS-DSP/main/group__groupFilters.html

und zum Berechnen der Koeffizienten den Micromodeler
https://www.micromodeler.com/dsp/

der kann auch Code ausgeben.

PS: als Testdaten kannst du ja mal ein Sprung auf 1 oder verschiedene 
Frequenzen benutzen ..

HTH, Adib

[Falk S. (falk_s831)]

Deine Funktion ist ja auch recht großzügig bezüglich möglicher 
Eingabeparameter, würd ich sagen: schreib dir doch 'ne kleine Testsuite, 
welche Deine Funktion benutzt und wo Du Deine Berechnungs-Ergebnisse für 
die gegebenen Input-Stimuli gegen plausibilisieren kannst.

Der Code ist ja mit deinen STL-Containern ziemlich portabel, sollte also 
nix dagegen sprechen, mal GTest oder boost::test ne Chance zu geben.