From 26db426f05657edcaf4bd5ea84d807e63c7ef181 Mon Sep 17 00:00:00 2001
From: Tom Brazier <tom_github@firstsolo.net>
Date: Fri, 2 Jun 2023 15:25:04 +0100
Subject: corrected EWA logic for large values of dt * responsiveness
---
filter-nm/ftnoir_filter_nm.cpp | 6 ++++--
1 file changed, 4 insertions(+), 2 deletions(-)
(limited to 'filter-nm')
diff --git a/filter-nm/ftnoir_filter_nm.cpp b/filter-nm/ftnoir_filter_nm.cpp
index b616f722..cd395f1b 100644
--- a/filter-nm/ftnoir_filter_nm.cpp
+++ b/filter-nm/ftnoir_filter_nm.cpp
@@ -36,8 +36,10 @@ void filter_nm::filter(const double* input, double* output)
for (int i = 0; i < 6; i++)
{
double speed = (input[i] - last_input[i]) / dt;
- speeds[i] += dt * (double)s.responsiveness[i] * (speed - speeds[i]);
- filtered_output[i] += dt * (double)s.responsiveness[i] * min(1.0, abs(speeds[i]) / (double)s.drift_speeds[i]) * (input[i] - filtered_output[i]);
+ double timescale = 1. / *(s.responsiveness[i]);
+ double alpha = dt / (dt + timescale);
+ speeds[i] += alpha * (speed - speeds[i]); // EWA
+ filtered_output[i] += alpha * min(1.0, abs(speeds[i]) / *(s.drift_speeds[i])) * (input[i] - filtered_output[i]);
}
}
--
cgit v1.2.3