accela: rework to remove deadzone need

Tested-by: @KyokushinPL

filter gain adjusted by a slider. frames smoothed by ewma for sudden
noise spike reduction.

likely need further work for arbitrary magic constants embedded in
tracker gain.

4 files changed, 99 insertions, 330 deletions

diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
index 310b08cc..4c836ac9 100644
--- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
+++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>288</width>
-    <height>432</height>
+    <width>424</width>
+    <height>268</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -30,36 +30,7 @@
       <enum>QFrame::Raised</enum>
      </property>
      <layout class="QGridLayout" name="gridLayout_3">
-      <item row="0" column="0">
-       <widget class="QLabel" name="label_2">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="text">
-         <string>Fast rotation speed</string>
-        </property>
-        <property name="alignment">
-         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_3">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="text">
-         <string>Translation</string>
-        </property>
-       </widget>
-      </item>
-      <item row="3" column="0">
+      <item row="1" column="0">
        <widget class="QLabel" name="lblSensYaw_6">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
@@ -72,33 +43,40 @@
 background:none;</string>
         </property>
         <property name="text">
-         <string>Rotation deadband</string>
+         <string>Rotation smoothing</string>
         </property>
        </widget>
       </item>
       <item row="3" column="1">
-       <widget class="QDoubleSpinBox" name="rot_deadzone">
-        <property name="alignment">
-         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-        </property>
-        <property name="decimals">
-         <number>3</number>
-        </property>
-        <property name="minimum">
-         <double>0.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>100.000000000000000</double>
-        </property>
-        <property name="singleStep">
-         <double>0.050000000000000</double>
-        </property>
-        <property name="value">
-         <double>0.000000000000000</double>
-        </property>
+       <widget class="QComboBox" name="ewma">
+        <item>
+         <property name="text">
+          <string>None</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>Low</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>Normal</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>High</string>
+         </property>
+        </item>
+        <item>
+         <property name="text">
+          <string>Extreme</string>
+         </property>
+        </item>
        </widget>
       </item>
-      <item row="4" column="0">
+      <item row="2" column="0">
        <widget class="QLabel" name="label">
         <property name="sizePolicy">
          <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
@@ -107,37 +85,12 @@ background:none;</string>
          </sizepolicy>
         </property>
         <property name="text">
-         <string>Translation deadband</string>
-        </property>
-       </widget>
-      </item>
-      <item row="4" column="1">
-       <widget class="QDoubleSpinBox" name="trans_deadzone">
-        <property name="alignment">
-         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-        </property>
-        <property name="decimals">
-         <number>3</number>
-        </property>
-        <property name="minimum">
-         <double>0.000000000000000</double>
-        </property>
-        <property name="maximum">
-         <double>100.000000000000000</double>
-        </property>
-        <property name="singleStep">
-         <double>0.050000000000000</double>
-        </property>
-        <property name="value">
-         <double>0.000000000000000</double>
+         <string>Translation smoothing</string>
         </property>
        </widget>
       </item>
       <item row="2" column="1">
-       <widget class="QSlider" name="trans_smoothing">
-        <property name="minimum">
-         <number>1</number>
-        </property>
+       <widget class="QSlider" name="translation_slider">
         <property name="maximum">
          <number>100</number>
         </property>
@@ -155,49 +108,8 @@ background:none;</string>
         </property>
        </widget>
       </item>
-      <item row="0" column="1">
-       <widget class="QSlider" name="rot_plus">
-        <property name="minimum">
-         <number>1</number>
-        </property>
-        <property name="maximum">
-         <number>100</number>
-        </property>
-        <property name="pageStep">
-         <number>5</number>
-        </property>
-        <property name="orientation">
-         <enum>Qt::Horizontal</enum>
-        </property>
-        <property name="tickPosition">
-         <enum>QSlider::TicksAbove</enum>
-        </property>
-        <property name="tickInterval">
-         <number>25</number>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_4">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="text">
-         <string>Slow rotation speed</string>
-        </property>
-        <property name="alignment">
-         <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
-        </property>
-       </widget>
-      </item>
       <item row="1" column="1">
-       <widget class="QSlider" name="rot_minus">
-        <property name="minimum">
-         <number>1</number>
-        </property>
+       <widget class="QSlider" name="rotation_slider">
         <property name="maximum">
          <number>100</number>
         </property>
@@ -215,126 +127,10 @@ background:none;</string>
         </property>
        </widget>
       </item>
-     </layout>
-    </widget>
-   </item>
-   <item>
-    <widget class="QGroupBox" name="groupBox">
-     <property name="title">
-      <string>Debug information</string>
-     </property>
-     <layout class="QGridLayout" name="gridLayout_2">
-      <item row="0" column="0">
-       <widget class="QLabel" name="label_7">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="text">
-         <string>Yaw</string>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="0">
-       <widget class="QLabel" name="label_6">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="text">
-         <string>Pitch</string>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="0">
-       <widget class="QLabel" name="label_5">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
+      <item row="3" column="0">
+       <widget class="QLabel" name="label_2">
         <property name="text">
-         <string>Roll</string>
-        </property>
-       </widget>
-      </item>
-      <item row="0" column="1">
-       <widget class="QDoubleSpinBox" name="debug_y">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="readOnly">
-         <bool>true</bool>
-        </property>
-        <property name="buttonSymbols">
-         <enum>QAbstractSpinBox::PlusMinus</enum>
-        </property>
-        <property name="decimals">
-         <number>8</number>
-        </property>
-        <property name="maximum">
-         <double>99.000000000000000</double>
-        </property>
-        <property name="singleStep">
-         <double>0.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="1" column="1">
-       <widget class="QDoubleSpinBox" name="debug_p">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="readOnly">
-         <bool>true</bool>
-        </property>
-        <property name="buttonSymbols">
-         <enum>QAbstractSpinBox::PlusMinus</enum>
-        </property>
-        <property name="decimals">
-         <number>8</number>
-        </property>
-        <property name="maximum">
-         <double>99.000000000000000</double>
-        </property>
-        <property name="singleStep">
-         <double>0.000000000000000</double>
-        </property>
-       </widget>
-      </item>
-      <item row="2" column="1">
-       <widget class="QDoubleSpinBox" name="debug_r">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="readOnly">
-         <bool>true</bool>
-        </property>
-        <property name="buttonSymbols">
-         <enum>QAbstractSpinBox::PlusMinus</enum>
-        </property>
-        <property name="decimals">
-         <number>8</number>
-        </property>
-        <property name="maximum">
-         <double>99.000000000000000</double>
-        </property>
-        <property name="singleStep">
-         <double>0.000000000000000</double>
+         <string>Spiking noise reduction</string>
         </property>
        </widget>
       </item>
@@ -358,7 +154,7 @@ background:none;</string>
       <enum>QFrame::NoFrame</enum>
      </property>
      <property name="text">
-      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;justify&quot;&gt;&lt;br/&gt;&lt;/p&gt;&lt;p align=&quot;justify&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;Accela by &lt;/span&gt;&lt;a href=&quot;https://github.com/sthalik&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Stanisław Halik&lt;/span&gt;&lt;/a&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;&lt;br/&gt;Thanks to &lt;/span&gt;&lt;a href=&quot;https://github.com/dbaarda&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Donovan Baarda&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;2012-2014&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;justify&quot;&gt;&lt;br/&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;Accela by &lt;/span&gt;&lt;a href=&quot;https://github.com/sthalik&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Stanisław Halik&lt;/span&gt;&lt;/a&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;&lt;br/&gt;Thanks to &lt;/span&gt;&lt;a href=&quot;https://github.com/dbaarda&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Donovan Baarda&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;2012-2014&lt;/span&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;br/&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
      </property>
      <property name="textFormat">
       <enum>Qt::RichText</enum>
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 42478963..d5cd4080 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -12,74 +12,83 @@
 #include "opentrack/plugin-api.hpp"
 using namespace std;
 
-FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0 }
+FTNoIR_Filter::FTNoIR_Filter() : first_run(true)
 {
 }
 
-static inline double dz(double x, double dz)
+static double f(double vec, double thres)
 {
-    return std::max(0., fabs(x) - dz) * (x < 0. ? -1. : 1.);
+    if (vec > thres*4)
+        return (vec - thres*4) * 600 + thres*4;
+    if (vec > thres)
+        return (vec - thres) * 150 + thres;
+    return vec;
 }
 
 void FTNoIR_Filter::filter(const double* input, double *output)
 {
     if (first_run)
     {
-        for (int i = 0; i < 3; i++)
-            fast_state[i] = 0;
-
         for (int i = 0; i < 6; i++)
         {
             output[i] = input[i];
-            for (int j = 0; j < 3; j++)
-                last_output[i] = input[i];
+            last_output[i] = input[i];
+            smoothed_input[i] = input[i];
         }
         first_run = false;
-        timer.start();
+        t.start();
         return;
     }
 
-    const double rot_dz = s.rot_deadzone;
-    const double trans_dz = s.trans_deadzone;
-
-    const int s_rot_plus = s.rot_plus, s_rot_minus = s.rot_minus;
+    const double rot_t = 10. * s.rot_threshold / 100.;
+    const double trans_t = 10. * s.trans_threshold / 100.;
 
-    const double a_rot_plus = s_rot_plus/100.;
-    const double a_rot_minus = s_rot_minus/100. * a_rot_plus;
-    const double a_trans = s.trans_smoothing/100.;
+    const double dt = t.elapsed() * 1e-9;
+    t.start();
 
-    const double Hz = timer.elapsed() * 1e-9;
-    timer.start();
-    double fast_alpha = Hz/(Hz + fast_alpha_seconds);
+    double RC;
 
+    switch (s.ewma)
+    {
+    default:
+    case 0: // none
+        RC = 0;
+        break;
+    case 1: // low
+        RC = 0.07;
+        break;
+    case 2: // normal
+        RC = 0.10;
+        break;
+    case 3: // high
+        RC = 0.14;
+        break;
+    case 4: // extreme
+        RC = 0.16;
+        break;
+    }
+    
     for (int i = 0; i < 6; i++)
     {
-        const double vec = input[i] - last_output[i];
-        double datum = Hz * 16;
-
-        if (i >= 3)
-        {
-            int k = i - 3;
-            const double vec_ = dz(vec, rot_dz);
-            const double cur_fast = fabs(vec_) * fast_alpha + fast_state[k]*(1. - fast_alpha);
-            fast_state[k] = cur_fast;
-            const double c = cur_fast > max_slow_delta ? a_rot_plus : a_rot_minus;
-            datum *= vec_ * c;
-        }
-        else
-            datum *= dz(vec, trans_dz) * a_trans;
-
-        const double result = last_output[i] + datum;
+        const double alpha = dt/(dt+RC);
+        
+        smoothed_input[i] = smoothed_input[i] * (1.-alpha) + input[i] * alpha;
+        
+        const double in = smoothed_input[i];
+        
+        const double vec = in - last_output[i];
+        const double vec_ = fabs(vec);
+        const double t = i >= 3 ? rot_t : trans_t;
+        const double val = f(vec_, t);
+        const double result = last_output[i] + (vec < 0 ? -1 : 1) * dt * val;
         const bool negp = vec < 0.;
-        const bool done = negp ? result <= input[i] : result >= input[i];
-
-        const double ret = done ? input[i] : result;
+        const bool done = negp
+            ? result <= in
+            : result >= in;
+        const double ret = done ? in : result;
+        
         last_output[i] = output[i] = ret;
     }
-
-    state.y = fast_state[0];
-    state.p = fast_state[1];
-    state.r = fast_state[2];
 }
 
 extern "C" OPENTRACK_EXPORT IFilter* GetConstructor()
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index 9052f49b..3abd2cf0 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -11,40 +11,26 @@ using namespace options;
 
 struct settings {
     pbundle b;
-    value<double> rot_deadzone, trans_deadzone;
-    value<int> rot_plus, rot_minus, trans_smoothing;
+    value<int> rot_threshold, trans_threshold, ewma;
     settings() :
         b(bundle("Accela")),
-        rot_deadzone(b, "rotation-deadband", 0),
-        trans_deadzone(b, "translation-deadband", 0),
-        rot_plus(b, "rotation-increase", 30),
-        rot_minus(b, "rotation-decrease", 25),
-        trans_smoothing(b, "translation-smoothing", 50)
+        rot_threshold(b, "rotation-threshold", 30),
+        trans_threshold(b, "translation-threshold", 50),
+        ewma(b, "ewma", 2)
     {}
 };
 
-struct state_display
-{
-    double y, p, r;
-    state_display() : y(0), p(0), r(0) {}
-};
-
 class FTNoIR_Filter : public IFilter
 {
 public:
     FTNoIR_Filter();
-    void filter(const double* target_camera_position, double *new_camera_position);
-    state_display state;
+    void filter(const double* input, double *output);
 private:
-    // moving average history amount in seconds
-    static constexpr double fast_alpha_seconds = 0.3;
-    // max degrees considered "slow" after moving average
-    static constexpr double max_slow_delta = 0.9;
     settings s;
     bool first_run;
     double last_output[6];
-    double fast_state[3];
-    Timer timer;
+    double smoothed_input[6];
+    Timer t;
 };
 
 class FilterControls: public IFilterDialog
@@ -60,11 +46,9 @@ private:
     void save();
     FTNoIR_Filter* accela_filter;
     settings s;
-    QTimer t;
 private slots:
     void doOK();
     void doCancel();
-    void timer_fired();
 };
 
 class FTNoIR_FilterDll : public Metadata
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
index cbfe650f..4fbc482d 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
@@ -12,38 +12,18 @@ FilterControls::FilterControls() :
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
 
-    tie_setting(s.rot_deadzone, ui.rot_deadzone);
-    tie_setting(s.trans_deadzone, ui.trans_deadzone);
-
-    tie_setting(s.rot_plus , ui.rot_plus);
-    tie_setting(s.rot_minus , ui.rot_minus);
-    tie_setting(s.trans_smoothing, ui.trans_smoothing);
-
-    connect(&t, SIGNAL(timeout()), this, SLOT(timer_fired()));
-
-    t.setInterval(250);
-}
-
-void FilterControls::timer_fired()
-{
-    if (accela_filter)
-    {
-        state_display st = accela_filter->state;
-        ui.debug_y->setValue(st.y);
-        ui.debug_p->setValue(st.p);
-        ui.debug_r->setValue(st.r);
-    }
+    tie_setting(s.rot_threshold, ui.rotation_slider);
+    tie_setting(s.trans_threshold, ui.translation_slider);
+    tie_setting(s.ewma, ui.ewma);
 }
 
 void FilterControls::register_filter(IFilter* filter)
 {
     accela_filter = static_cast<FTNoIR_Filter*>(filter);
-    t.start();
 }
 
 void FilterControls::unregister_filter()
 {
-    t.stop();
     accela_filter = nullptr;
 }