diff options
| -rw-r--r-- | filter-accela/ftnoir_filter_accela.cpp | 5 | ||||
| -rw-r--r-- | filter-ewma2/ftnoir_filter_ewma2.cpp | 14 |
2 files changed, 17 insertions, 2 deletions
diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp index 44a341e0..b70f08cf 100644 --- a/filter-accela/ftnoir_filter_accela.cpp +++ b/filter-accela/ftnoir_filter_accela.cpp @@ -64,6 +64,9 @@ static void do_deltas(const double* deltas, double* output, F&& fun) void accela::filter(const double* input, double *output) { + static constexpr double full_turn = 360.0; + static constexpr double half_turn = 180.0; + if (unlikely(first_run)) { first_run = false; @@ -98,6 +101,7 @@ void accela::filter(const double* input, double *output) for (unsigned i = 3; i < 6; i++) { double d = input[i] - last_output[i]; + if (fabs(d) > half_turn) d -= copysign(full_turn, d); if (fabs(d) > rot_dz) d -= copysign(rot_dz, d); @@ -134,6 +138,7 @@ void accela::filter(const double* input, double *output) { output[k] *= dt; output[k] += last_output[k]; + if (fabs(output[k]) > half_turn) output[k] -= copysign(full_turn, output[k]); last_output[k] = output[k]; } diff --git a/filter-ewma2/ftnoir_filter_ewma2.cpp b/filter-ewma2/ftnoir_filter_ewma2.cpp index ce6cd040..25902280 100644 --- a/filter-ewma2/ftnoir_filter_ewma2.cpp +++ b/filter-ewma2/ftnoir_filter_ewma2.cpp @@ -26,6 +26,8 @@ ewma::ewma() = default; void ewma::filter(const double *input, double *output) { + static constexpr double full_turn = 360; + static constexpr double half_turn = 180; // Start the timer and initialise filter state if it's not running. if (first_run) { @@ -57,7 +59,13 @@ void ewma::filter(const double *input, double *output) using std::pow; // Calculate the current and smoothed delta. - double delta = input[i] - last_output[i]; + double input_value = input[i]; + double delta = input_value - last_output[i]; + if (fabs(delta) > half_turn) + { + delta -= copysign(full_turn, delta); + input_value -= copysign(full_turn, input_value); + } last_delta[i] = delta_alpha*delta + (1.0-delta_alpha)*last_delta[i]; // Calculate the current and smoothed noise variance. double noise = last_delta[i]*last_delta[i]; @@ -70,7 +78,9 @@ void ewma::filter(const double *input, double *output) // Calculate the dynamic alpha. double alpha = dt/(dt + RC); // Calculate the new output position. - output[i] = last_output[i] = alpha*input[i] + (1.0-alpha)*last_output[i]; + output[i] = alpha*input_value + (1.0-alpha)*last_output[i]; + if (fabs(output[i]) > half_turn) output[i] -= copysign(full_turn, output[i]); + last_output[i] = output[i]; } } |