From 7252c00161959b3b7ce9d78506ec6cf026f24d5e Mon Sep 17 00:00:00 2001 From: GO63-samara Date: Tue, 14 Apr 2020 16:27:59 +0400 Subject: Fix the jump of Yaw and Roll when crossing the border +/-180 degrees When crossing the border on Yaw or Roll +/-180 degrees, the Octopus makes a full rotation of 360 degrees. This made it difficult to use an inertial tracker in a wireless VR. --- filter-ewma2/ftnoir_filter_ewma2.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) (limited to 'filter-ewma2') diff --git a/filter-ewma2/ftnoir_filter_ewma2.cpp b/filter-ewma2/ftnoir_filter_ewma2.cpp index ce6cd040..09e6f5d7 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) { +#define FULL_TURN 360.0 +#define HALF_TURN 180.0 // 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]; } } -- cgit v1.2.3