Merge pull request #383 from DaMichel/kalman

Kalman filter

3 files changed, 16 insertions, 8 deletions

diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt
index a1eec8b1..f84cb0a9 100644
--- a/filter-kalman/CMakeLists.txt
+++ b/filter-kalman/CMakeLists.txt
@@ -1,8 +1,6 @@
-if(FALSE)
 find_package(OpenCV 3.0 QUIET)
 if(OpenCV_FOUND)
     opentrack_boilerplate(opentrack-filter-kalman)
     target_link_libraries(opentrack-filter-kalman ${OpenCV_LIBS})
     target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
-endif()
-endif()
+endif()
\ No newline at end of file
diff --git a/filter-kalman/ftnoir_filter_kalman.h b/filter-kalman/ftnoir_filter_kalman.h
index 096cc3f9..a468bd5d 100644
--- a/filter-kalman/ftnoir_filter_kalman.h
+++ b/filter-kalman/ftnoir_filter_kalman.h
@@ -18,6 +18,7 @@
 #include <QWidget>
 #include "opentrack-compat/options.hpp"
 using namespace options;
+#include "opentrack-compat/timer.hpp"
 
 struct settings : opts {
     value<int> noise_stddev_slider;
@@ -39,7 +40,8 @@ public:
     static constexpr double accel_stddev = (accel*4/(dt_*dt_))/3.0;
     cv::KalmanFilter kalman;
     double last_input[6];
-    QElapsedTimer timer;
+    Timer timer;
+    bool first_run;
     settings s;
     int prev_slider_pos;
 };
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 05731cb6..deaf6321 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -66,7 +66,7 @@ void FTNoIR_Filter::reset() {
     for (int i = 0; i < 6; i++) {
         last_input[i] = 0;
     }
-    timer.invalidate();
+    first_run = true;
 }
 
 void FTNoIR_Filter::filter(const double* input, double *output)
@@ -76,16 +76,24 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         reset();
         prev_slider_pos = s.noise_stddev_slider;
     }
-    // Start the timer if it's not running.
-    if (!timer.isValid())
+    // Start the timer on first filter evaluation.
+    if (first_run)
+    {
         timer.start();
+        first_run = false;
+        return;
+    }
+
     // Get the time in seconds since last run and restart the timer.
-    const double dt = timer.restart() / 1000.;
+    const double dt = timer.elapsed_seconds();
+    timer.start();
+    
     // Note this is a terrible way to detect when there is a new
     // frame of tracker input, but it is the best we have.
     bool new_input = false;
     for (int i = 0; i < 6 && !new_input; i++)
         new_input = (input[i] != last_input[i]);
+
     // Update the transitionMatrix and processNoiseCov for dt.
     double accel_variance = accel_stddev * accel_stddev;
     double a = dt * dt * accel_variance;  // dt^2 * accel_variance.