From 82e5b48f0f0c1667c089d6c7e02e80085f999482 Mon Sep 17 00:00:00 2001 From: Donovan Baarda Date: Sun, 19 Oct 2014 01:08:17 +1100 Subject: Change EWMA2 to use a dynamic dt using a timer. --- ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp | 48 ++++++++++++++--------------- ftnoir_filter_ewma2/ftnoir_filter_ewma2.h | 13 +++++--- 2 files changed, 31 insertions(+), 30 deletions(-) (limited to 'ftnoir_filter_ewma2') diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp index b25fc50b..8520eb71 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp @@ -22,17 +22,9 @@ // to minSmooth at a rate controlled by the powCurve setting. -FTNoIR_Filter::FTNoIR_Filter() : - first_run(true), - // Currently facetracknoir/tracker.cpp updates every dt=3ms. All - // filter alpha values are calculated as alpha=dt/(dt+RC) and - // need to be updated when tracker.cpp changes. - // TODO(abo): Change this to use a dynamic dt using a timer. - // Deltas are smoothed over the last 1/60sec (16ms). - delta_alpha(0.003/(0.003 + 0.016)), - // Noise is smoothed over the last 60sec. - noise_alpha(0.003/(0.003 + 60.0)) +FTNoIR_Filter::FTNoIR_Filter() { + reset(); } void FTNoIR_Filter::receiveSettings() @@ -40,40 +32,46 @@ void FTNoIR_Filter::receiveSettings() s.b->reload(); } -void FTNoIR_Filter::filter(const double *target_camera_position, - double *new_camera_position) +void FTNoIR_Filter::reset() { - double new_delta, new_noise, norm_noise; - double smoothing, RC, alpha; + timer.invalidate(); +} - //On the first run, initialize filter states to target intput. - if (first_run==true) { +void FTNoIR_Filter::filter(const double *target_camera_position, + double *new_camera_position) +{ + // Start the timer and initialise filter state if it's not running. + if (!timer.isValid()) { + timer.start(); for (int i=0;i<6;i++) { output[i] = target_camera_position[i]; delta[i] = 0.0; noise[i] = 0.0; } - first_run=false; } - + // Get the time in seconds since last run and restart the timer. + auto dt = timer.restart() / 1000.0f; + // Calculate delta_alpha and noise_alpha from dt. + double delta_alpha = dt/(dt + delta_RC); + double noise_alpha = dt/(dt + noise_RC); // Calculate the new camera position. for (int i=0;i<6;i++) { // Calculate the current and smoothed delta. - new_delta = target_camera_position[i]-output[i]; + double new_delta = target_camera_position[i]-output[i]; delta[i] = delta_alpha*new_delta + (1.0-delta_alpha)*delta[i]; // Calculate the current and smoothed noise variance. - new_noise = delta[i]*delta[i]; + double new_noise = delta[i]*delta[i]; noise[i] = noise_alpha*new_noise + (1.0-noise_alpha)*noise[i]; // Normalise the noise between 0->1 for 0->9 variances (0->3 stddevs). - norm_noise = std::min(new_noise/(9.0*noise[i]), 1.0); + double norm_noise = std::min(new_noise/(9.0*noise[i]), 1.0); // Calculate the smoothing 0.0->1.0 from the normalized noise. // TODO(abo): change kSmoothingScaleCurve to a float where 1.0 is sqrt(norm_noise). - smoothing = 1.0 - pow(norm_noise, s.kSmoothingScaleCurve/20.0); + double smoothing = 1.0 - pow(norm_noise, s.kSmoothingScaleCurve/20.0); // Currently min/max smoothing are ints 0->100. We want 0.0->3.0 seconds. // TODO(abo): Change kMinSmoothing, kMaxSmoothing to floats 0.0->3.0 seconds RC. - RC = 3.0*(s.kMinSmoothing + smoothing*(s.kMaxSmoothing - s.kMinSmoothing))/100.0; - // TODO(abo): Change this to use a dynamic dt using a timer. - alpha = 0.003/(0.003 + RC); + double RC = 3.0*(s.kMinSmoothing + smoothing*(s.kMaxSmoothing - s.kMinSmoothing))/100.0; + // Calculate the dynamic alpha. + double alpha = dt/(dt + RC); // Calculate the new output position. output[i] = alpha*target_camera_position[i] + (1.0-alpha)*output[i]; new_camera_position[i] = output[i]; diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h index 0388c5f0..d311d0e5 100644 --- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h +++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h @@ -2,6 +2,7 @@ #include "opentrack/plugin-api.hpp" #include "ui_ftnoir_ewma_filtercontrols.h" +#include #include #include #include "opentrack/options.hpp" @@ -24,17 +25,19 @@ class FTNoIR_Filter : public IFilter { public: FTNoIR_Filter(); - void reset() { first_run=true; } + void reset(); void filter(const double *target_camera_position, - double *new_camera_position); + double *new_camera_position); void receiveSettings(); private: - bool first_run; - double delta_alpha; - double noise_alpha; + // Deltas are smoothed over the last 1/60sec (16ms). + const double delta_RC = 0.016; + // Noise is smoothed over the last 60sec. + const double noise_RC = 60.0; double delta[6]; double noise[6]; double output[6]; + QElapsedTimer timer; settings s; }; -- cgit v1.2.3