Merge pull request #88 from dbaarda/dev/kalman

Dev/kalman

2 files changed, 22 insertions, 24 deletions

diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
index c2947516..f6224530 100755
--- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -24,12 +24,13 @@ class OPENTRACK_EXPORT FTNoIR_Filter : public IFilter
 public:
     FTNoIR_Filter();
     void reset();
-    void filter(const double *target_camera_position,
-                            double *new_camera_position);
-    double accel_variance;
-    double noise_variance;
+    void filter(const double *input, double *output);
+    // Set accel_stddev assuming moving 0.0->100.0 in dt=0.2 is 3 stddevs: (100.0*4/dt^2)/3.
+    const double accel_stddev = (100.0*4/(0.2*0.2))/3.0;
+    // TODO(abo): make noise_stddev a UI setting 0.0->10.0 with 0.1 resolution.
+    const double noise_stddev = 1.0;
     cv::KalmanFilter kalman;
-    double prev_position[6];
+    double last_input[6];
     QElapsedTimer timer;
 };
 
diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp
index ab352faa..c5a600af 100644
--- a/ftnoir_filter_kalman/kalman.cpp
+++ b/ftnoir_filter_kalman/kalman.cpp
@@ -16,10 +16,6 @@ FTNoIR_Filter::FTNoIR_Filter() {
 // the following was written by Donovan Baarda <abo@minkirri.apana.org.au>
 // https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b
 void FTNoIR_Filter::reset() {
-    // Set accel_variance for moving 0.0->1.0 in dt=0.1.
-    accel_variance = 400.0f;
-    // TODO(abo): make noise_variance a UI setting 0.0->1.0.
-    noise_variance = 0.1;
     // Setup kalman with state (x) is the 6 tracker outputs then
     // their 6 corresponding velocities, and the measurement (z) is
     // the 6 tracker outputs.
@@ -42,6 +38,7 @@ void FTNoIR_Filter::reset() {
     0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
+    double accel_variance = accel_stddev * accel_stddev;
     double a = dt * dt * accel_variance;  // dt^2 * accel_variance.
     double b = 0.5 * a * dt;  // (dt^3)/2 * accel_variance.
     double c = 0.5 * b * dt;  // (dt^4)/4 * accel_variance.
@@ -59,16 +56,16 @@ void FTNoIR_Filter::reset() {
     0, 0, 0, 0, b, 0, 0, 0, 0, 0, a, 0,
     0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0, a);
     cv::setIdentity(kalman.measurementMatrix);
+    double noise_variance = noise_stddev * noise_stddev;
     cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(noise_variance));
     cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(accel_variance * 1e4));
     for (int i = 0; i < 6; i++) {
-        prev_position[i] = 0;
+        last_input[i] = 0;
     }
     timer.invalidate();
 }
 
-void FTNoIR_Filter::filter(const double* target_camera_position,
-                                       double *new_camera_position)
+void FTNoIR_Filter::filter(const double* input, double *output)
 {
     // Start the timer if it's not running.
     if (!timer.isValid())
@@ -77,11 +74,11 @@ void FTNoIR_Filter::filter(const double* target_camera_position,
     auto dt = timer.restart() / 1000.0f;
     // 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_target = false;
-    for (int i = 0; i < 6 && !new_target; i++)
-        new_target = (prev_position[i] != target_camera_position[i]);
-
+    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.
     double b = 0.5 * a * dt;  // (dt^3)/2 * accel_variance.
     double c = 0.5 * b * dt;  // (dt^4)/4 * accel_variance.
@@ -93,20 +90,20 @@ void FTNoIR_Filter::filter(const double* target_camera_position,
         kalman.processNoiseCov.at<double>(i+6,i) = b;
     }
     // Get the updated predicted position.
-    cv::Mat output = kalman.predict();
+    cv::Mat next_output = kalman.predict();
     // If we have new tracker input, get the corrected position.
-    if (new_target) {
+    if (new_input) {
         cv::Mat measurement(6, 1, CV_64F);
         for (int i = 0; i < 6; i++) {
-            measurement.at<double>(i) = target_camera_position[i];
-            // Save prev_position for detecting new tracker input.
-            prev_position[i] = target_camera_position[i];
+            measurement.at<double>(i) = input[i];
+            // Save last_input for detecting new tracker input.
+            last_input[i] = input[i];
         }
-        output = kalman.correct(measurement);
+        next_output = kalman.correct(measurement);
     }
-    // Set new_camera_position to the output.
+    // Set output to the next_output.
     for (int i = 0; i < 6; i++) {
-        new_camera_position[i] = output.at<double>(i);
+        output[i] = next_output.at<double>(i);
     }
 }