Merge pull request #83 from dbaarda/dev/ewma2

Dev/ewma2

2 files changed, 44 insertions, 55 deletions

diff --git a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
index c7169faa..320b95ad 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.cpp
@@ -1,19 +1,9 @@
-/*** Written by Donovan Baarda
-* 
-* This program is free software; you can redistribute it and/or modify it       *
-* under the terms of the GNU General Public License as published by the         *
-* Free Software Foundation; either version 3 of the License, or (at your        *
-* option) any later version.                                                    *
-*                                                                               *
-* This program is distributed in the hope that it will be useful, but           *
-* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY    *
-* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for   *
-* more details.                                                                 *
-*                                                                               *
-* You should have received a copy of the GNU General Public License along       *
-* with this program; if not, see <http://www.gnu.org/licenses/>.                *
-*                                                                               *
-********************************************************************************/
+/* Copyright (c) 2014 Donovan Baarda <abo@minkirri.apana.org.au>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
 #include "ftnoir_filter_ewma2.h"
 #include <cmath>
 #include <QDebug>
@@ -22,12 +12,26 @@
 #include <algorithm>
 #include <QMutexLocker>
 
+// Exponentially Weighted Moving Average (EWMA) Filter with dynamic smoothing.
+//
+// This filter tries to adjust the amount of filtering to minimize lag when
+// moving, and minimize noise when still. It uses the delta filtered over the
+// last 1/60sec (16ms) compared to the delta's average noise variance over
+// the last 60sec to try and detect movement vs noise. As the delta increases
+// from 0 to 3 stdevs of the noise, the filtering scales down from maxSmooth
+// to minSmooth at a rate controlled by the powCurve setting.
+
+
 FTNoIR_Filter::FTNoIR_Filter() :
     first_run(true),
-    // Deltas are smoothed over the last 3 frames (0.1sec at 30fps).
-    delta_smoothing(1.0/3.0),
-    // Noise is smoothed over the last 3600 frames (~2mins at 30fps).
-    noise_smoothing(1.0/3600.0)
+    // 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))
 {
 }
 
@@ -40,54 +44,39 @@ void FTNoIR_Filter::FilterHeadPoseData(const double *target_camera_position,
                                        double *new_camera_position)
 {
     double new_delta, new_noise, norm_noise;
-    double alpha;
+    double smoothing, RC, alpha;
 
-    //On the first run, initialize to output=target and return.
+    //On the first run, initialize filter states to target intput.
     if (first_run==true) {
         for (int i=0;i<6;i++) {
-            new_camera_position[i] = target_camera_position[i];
-            current_camera_position[i] = target_camera_position[i];
+            output[i] = target_camera_position[i];
             delta[i] = 0.0;
             noise[i] = 0.0;
         }
         first_run=false;
-        return;
-    }
-
-    bool new_frame = false;
-
-    for (int i = 0; i < 6; i++)
-    {
-        if (target_camera_position[i] != current_camera_position[i])
-        {
-            new_frame = true;
-            break;
-        }
-    }
-
-    if (!new_frame)
-    {
-        for (int i = 0; i < 6; i++)
-            new_camera_position[i] = current_camera_position[i];
-        return;
     }
 
     // Calculate the new camera position.
     for (int i=0;i<6;i++) {
         // Calculate the current and smoothed delta.
-        new_delta = target_camera_position[i]-current_camera_position[i];
-        delta[i] = delta_smoothing*new_delta + (1.0-delta_smoothing)*delta[i];
+        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];
-        noise[i] = noise_smoothing*new_noise + (1.0-noise_smoothing)*noise[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<double>(new_noise/(9.0*noise[i]), 1.0);
-        // Calculate the alpha from the normalized noise.
+        // 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).
-        alpha = 1.0/(s.kMinSmoothing+(1.0-pow(norm_noise,s.kSmoothingScaleCurve/20.0))*(s.kMaxSmoothing-s.kMinSmoothing));
-        // Update the current camera position to the new position.
-        double pos = alpha*target_camera_position[i] + (1.0-alpha)*current_camera_position[i];
-        new_camera_position[i] = current_camera_position[i] = pos;
+        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);
+        // 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 8863348c..aacf4916 100644
--- a/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
+++ b/ftnoir_filter_ewma2/ftnoir_filter_ewma2.h
@@ -24,17 +24,17 @@ class FTNoIR_Filter : public IFilter
 {
 public:
     FTNoIR_Filter();
-    void reset() {}
+    void reset() { first_run=true; }
     void FilterHeadPoseData(const double *target_camera_position,
                             double *new_camera_position);
     void receiveSettings();
 private:
     bool first_run;
-    double delta_smoothing;
-    double noise_smoothing;
+    double delta_alpha;
+    double noise_alpha;
     double delta[6];
     double noise[6];
-    double current_camera_position[6];
+    double output[6];
     settings s;
 };