kalman fixes, still not working well

2 files changed, 7 insertions, 7 deletions

diff --git a/ftnoir_filter_kalman/ftnoir_filter_kalman.h b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
index 1f0b36fd..e84ef347 100755
--- a/ftnoir_filter_kalman/ftnoir_filter_kalman.h
+++ b/ftnoir_filter_kalman/ftnoir_filter_kalman.h
@@ -21,9 +21,9 @@ using namespace options;
 
 struct settings : opts {
     value<int> noise_stddev_slider;
-    // slider goes noise_stdder 0->50
+    // slider for noise_stddev goes 0->(mult_noise_stddev * 100)
     static constexpr double mult_noise_stddev = .5;
-    settings() : opts("kalman-filter"), noise_stddev_slider(b, "noise-stddev", 10)
+    settings() : opts("kalman-filter"), noise_stddev_slider(b, "noise-stddev", 40)
     {}
 };
 
@@ -33,10 +33,10 @@ public:
     FTNoIR_Filter();
     void reset();
     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;
+    // Set accel_stddev assuming moving 0.0->accel in dt_ is 3 stddevs: (accel*4/dt_^2)/3.
+    static constexpr double dt_ = .4;
+    static constexpr double accel = 60.;
+    static constexpr double accel_stddev = (accel*4/(dt_*dt_))/3.0;
     cv::KalmanFilter kalman;
     double last_input[6];
     QElapsedTimer timer;
diff --git a/ftnoir_filter_kalman/kalman.cpp b/ftnoir_filter_kalman/kalman.cpp
index 4939610b..08b46f0f 100644
--- a/ftnoir_filter_kalman/kalman.cpp
+++ b/ftnoir_filter_kalman/kalman.cpp
@@ -57,7 +57,7 @@ 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);
-    const double noise_stddev = s.noise_stddev_slider * s.mult_noise_stddev;
+    const double noise_stddev = (1+s.noise_stddev_slider) * s.mult_noise_stddev;
     const 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));