From f314ef0afb4f9f5eb2a3bbd7f49d23fc77728330 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 16 Oct 2015 21:38:44 +0200 Subject: accela: also filter out NaNs on tracking start --- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp index 81ef3948..3e049eb0 100644 --- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp +++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp @@ -64,9 +64,13 @@ void FTNoIR_Filter::filter(const double* input, double *output) { for (int i = 0; i < 6; i++) { - output[i] = input[i]; - last_output[i] = input[i]; - smoothed_input[i] = input[i]; + const bool drop = std::isnan(input[i]) || std::isinf(input[i]); + double f = input[i]; + if (drop) + f = 0; + output[i] = f; + last_output[i] = f; + smoothed_input[i] = f; } first_run = false; t.start(); -- cgit v1.2.3