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

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;

};