kalman filter revised: noise model change, switch to Eigen3, implemented

process noise covariance scaling adaptive KF, augmented akf with deadzone filter as second filter stage

4 files changed, 402 insertions, 122 deletions

diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt
index f84cb0a9..12de11ec 100644
--- a/filter-kalman/CMakeLists.txt
+++ b/filter-kalman/CMakeLists.txt
@@ -1,6 +1,3 @@
-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()
\ No newline at end of file
+opentrack_boilerplate(opentrack-filter-kalman)
+target_link_libraries(opentrack-filter-kalman opentrack-spline-widget)
+target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC)
\ No newline at end of file
diff --git a/filter-kalman/ftnoir_filter_kalman.h b/filter-kalman/ftnoir_filter_kalman.h
index aa6c978c..623fa7d9 100644
--- a/filter-kalman/ftnoir_filter_kalman.h
+++ b/filter-kalman/ftnoir_filter_kalman.h
@@ -10,40 +10,115 @@
 
 #include "ui_ftnoir_kalman_filtercontrols.h"
 #include "opentrack/plugin-api.hpp"
-#include <opencv2/core.hpp>
-#include <opencv2/video/tracking.hpp>
-#include <vector>
+
+#include <atomic>
+#include <Eigen/Core>
+#include <Eigen/LU>
+
 #include <QString>
-#include <QElapsedTimer>
 #include <QWidget>
+
 #include "opentrack-compat/options.hpp"
 using namespace options;
 #include "opentrack-compat/timer.hpp"
 
+
+static constexpr int NUM_STATE_DOF = 12;
+static constexpr int NUM_MEASUREMENT_DOF = 6;
+using Matrix = Eigen::MatrixXd; // variable size, heap allocated, double valued
+// These vectors are compile time fixed size, stack allocated
+using StateVector = Eigen::Matrix<double, NUM_STATE_DOF, 1>;
+using PoseVector = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, 1>;
+
+struct KalmanFilter
+{
+    Matrix
+        measurement_noise_cov,
+        process_noise_cov,
+        state_cov,
+        state_cov_prior,
+        kalman_gain,
+        transition_matrix,
+        measurement_matrix;
+    StateVector
+        state,
+        state_prior;
+    PoseVector
+        innovation;
+    void init();
+    void time_update();
+    void measurement_update(const PoseVector &measurement);
+};
+
+struct KalmanProcessNoiseScaler
+{
+    Matrix
+        innovation_cov_estimate,
+        base_cov; // baseline (unscaled) process noise covariance matrix
+    void init();
+    void update(KalmanFilter &kf, double dt);
+};
+
+
+struct DeadzoneFilter
+{
+    PoseVector
+        last_output,
+        dz_size;
+    DeadzoneFilter() :
+        last_output(PoseVector::Zero()),
+        dz_size(PoseVector::Zero())
+    {}
+    void reset() {
+        last_output = PoseVector::Zero();
+    }
+    PoseVector filter(const PoseVector &input);
+};
+
+
 struct settings : opts {
-    value<int> noise_stddev_slider;
-    // 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", 40)
+    value<int> noise_rot_slider_value;
+    value<int> noise_pos_slider_value;
+
+    static constexpr double adaptivity_window_length = 0.5; // seconds
+    static constexpr double deadzone_scale = 2.;
+    static constexpr double deadzone_exponent = 4.0;
+    // these values worked best for me (MW) when taken with acompanying measured noise stddev of ca 0.1 (rot) and 0.01 (pos). 
+    static constexpr double process_sigma_pos = 0.05;
+    static constexpr double process_simga_rot = 0.5;
+
+    static double map_slider_value(int v)
+    {
+        return std::pow(10., v * 0.04 - 3.);
+    }
+
+    settings() :  
+        opts("kalman-filter"), 
+        noise_rot_slider_value(b, "noise-rotation-slider", 40), 
+        noise_pos_slider_value(b, "noise-position-slider", 40)
     {}
+
 };
 
 class FTNoIR_Filter : public IFilter
 {
+    PoseVector do_kalman_filter(const PoseVector &input, double dt, bool new_input);
+    void fill_transition_matrix(double dt);
+    void fill_process_noise_cov_matrix(Matrix &target, double dt) const;
 public:
     FTNoIR_Filter();
     void reset();
     void filter(const double *input, double *output);
-    // 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];
+    PoseVector last_input;
     Timer timer;
     bool first_run;
+    double dt_since_last_input;
     settings s;
-    int prev_slider_pos;
+    KalmanFilter kf;
+    KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
+    PoseVector minimal_state_var;
+    DeadzoneFilter dz_filter;
+    int prev_slider_pos[2];
 };
 
 class FTNoIR_FilterDll : public Metadata
@@ -57,16 +132,12 @@ class FilterControls: public IFilterDialog
 {
     Q_OBJECT
 public:
-    FilterControls() {
-        ui.setupUi(this);
-        connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
-        connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
-        tie_setting(s.noise_stddev_slider, ui.noise_slider);
-    }
+    FilterControls();
     Ui::KalmanUICFilterControls ui;
     void register_filter(IFilter*) override {}
     void unregister_filter() override {}
     settings s;
+    FTNoIR_Filter *filter;
 public slots:
     void doOK();
     void doCancel();
diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui
index df213815..7533beed 100644
--- a/filter-kalman/ftnoir_kalman_filtercontrols.ui
+++ b/filter-kalman/ftnoir_kalman_filtercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>288</width>
-    <height>132</height>
+    <width>442</width>
+    <height>153</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -37,26 +37,55 @@
   </property>
   <layout class="QVBoxLayout" name="verticalLayout">
    <item>
-    <widget class="QFrame" name="frame">
-     <property name="frameShape">
-      <enum>QFrame::NoFrame</enum>
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="title">
+      <string>Measurement Noise</string>
      </property>
-     <property name="frameShadow">
-      <enum>QFrame::Raised</enum>
-     </property>
-     <layout class="QGridLayout" name="gridLayout">
+     <layout class="QGridLayout" name="gridLayout_2">
       <item row="0" column="0">
        <widget class="QLabel" name="label">
         <property name="text">
-         <string>Noise standard deviation</string>
+         <string>Rotation</string>
         </property>
        </widget>
       </item>
       <item row="0" column="1">
-       <widget class="QSlider" name="noise_slider">
+       <widget class="QSlider" name="noiseRotSlider">
+        <property name="maximum">
+         <number>100</number>
+        </property>
         <property name="singleStep">
          <number>1</number>
         </property>
+        <property name="tracking">
+         <bool>true</bool>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+        <property name="invertedAppearance">
+         <bool>false</bool>
+        </property>
+        <property name="tickPosition">
+         <enum>QSlider::TicksAbove</enum>
+        </property>
+        <property name="tickInterval">
+         <number>10</number>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="0">
+       <widget class="QLabel" name="label_4">
+        <property name="text">
+         <string>Position</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="1">
+       <widget class="QSlider" name="noisePosSlider">
+        <property name="maximum">
+         <number>100</number>
+        </property>
         <property name="orientation">
          <enum>Qt::Horizontal</enum>
         </property>
@@ -68,6 +97,32 @@
         </property>
        </widget>
       </item>
+      <item row="0" column="2">
+       <widget class="QLabel" name="noiseRotLabel">
+        <property name="minimumSize">
+         <size>
+          <width>60</width>
+          <height>0</height>
+         </size>
+        </property>
+        <property name="text">
+         <string>°</string>
+        </property>
+       </widget>
+      </item>
+      <item row="1" column="2">
+       <widget class="QLabel" name="noisePosLabel">
+        <property name="minimumSize">
+         <size>
+          <width>60</width>
+          <height>0</height>
+         </size>
+        </property>
+        <property name="text">
+         <string>-</string>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 1f23ed90..3f117399 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013 Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2013 Stanislaw Halik <sthalik@misaki.pl>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -9,73 +9,212 @@
 #include <QDebug>
 #include <cmath>
 
-constexpr double settings::mult_noise_stddev;
+
+void KalmanFilter::init()
+{
+    static constexpr int NS = NUM_STATE_DOF;
+    static constexpr int NZ = NUM_MEASUREMENT_DOF;
+    // allocate and initialize matrices
+    measurement_noise_cov = Matrix::Zero(NZ, NZ);
+    process_noise_cov = Matrix::Zero(NS, NS);
+    kalman_gain = Matrix::Zero(NS, NZ);
+    measurement_matrix = Matrix::Zero(NZ, NS);
+    state_cov = Matrix::Zero(NS, NS);
+    state_cov_prior = Matrix::Zero(NS, NS);
+    transition_matrix = Matrix::Zero(NS, NS);
+    // initialize state variables
+    state = StateVector::Zero();
+    state_prior = StateVector::Zero();
+    innovation = PoseVector::Zero();
+}
+
+
+void KalmanFilter::time_update()
+{
+    state_prior     = transition_matrix * state;
+    state_cov_prior = transition_matrix * state_cov * transition_matrix.transpose() + process_noise_cov;
+}
+
+
+void KalmanFilter::measurement_update(const PoseVector &measurement)
+{
+    Matrix tmp     = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov;
+    Matrix tmp_inv = tmp.inverse();
+    kalman_gain = state_cov_prior * measurement_matrix.transpose() * tmp_inv;
+    innovation = measurement - measurement_matrix * state_prior;
+    state     = state_prior + kalman_gain * innovation;
+    state_cov = state_cov_prior - kalman_gain * measurement_matrix * state_cov_prior;
+}
+
+
+
+void KalmanProcessNoiseScaler::init()
+{
+    base_cov = Matrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF);
+    innovation_cov_estimate = Matrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF);
+}
+
+
+/* Uses 
+    innovation, measurement_matrix, measurement_noise_cov, and state_cov_prior
+   found in KalmanFilter. It sets 
+    process_noise_cov
+*/
+void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt)
+{
+    Matrix ddT = kf.innovation * kf.innovation.transpose();
+    double f = dt / (dt + settings::adaptivity_window_length);
+    innovation_cov_estimate =
+        f * ddT + (1. - f) * innovation_cov_estimate;
+
+    double T1 = (innovation_cov_estimate - kf.measurement_noise_cov).trace();
+    double T2 = (kf.measurement_matrix * kf.state_cov_prior * kf.measurement_matrix.transpose()).trace();
+    double alpha = 0.001;
+    if (T2 > 0. && T1 > 0.)
+    {
+        alpha = T1 / T2;
+        alpha = std::sqrt(alpha);
+        alpha = std::min(1000., std::max(0.001, alpha));
+    }
+    kf.process_noise_cov = alpha * base_cov;
+    //qDebug() << "alpha = " << alpha;
+}
+
+
+PoseVector DeadzoneFilter::filter(const PoseVector &input)
+{
+    PoseVector out;
+    for (int i = 0; i < input.rows(); ++i)
+    {
+        const double dz = dz_size[i];
+        if (dz > 0.)
+        {
+            const double delta = input[i] - last_output[i];
+            const double f = pow(abs(delta) / dz, settings::deadzone_exponent);
+            const double response = f / (f + 1.) * delta;
+            out[i] = last_output[i] + response;
+        }
+        else
+            out[i] = input[i];
+        last_output[i] = out[i];
+    }
+    return out;
+}
+
+
+void FTNoIR_Filter::fill_transition_matrix(double dt)
+{
+    for (int i = 0; i < 6; ++i)
+    {
+        kf.transition_matrix(i, i + 6) = dt;
+    }
+}
+
+void FTNoIR_Filter::fill_process_noise_cov_matrix(Matrix &target, double dt) const
+{
+    // This model is like movement at fixed velocity plus superimposed
+    // brownian motion. Unlike standard models for tracking of objects
+    // with a very well predictable trajectory (e.g.
+    // https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical)
+    double sigma_pos = s.process_sigma_pos;
+    double sigma_angle = s.process_simga_rot;
+    double a_pos = sigma_pos * sigma_pos * dt;
+    double a_ang = sigma_angle * sigma_angle * dt;
+    static constexpr double b = 20;
+    static constexpr double c = 1.;
+    for (int i = 0; i < 3; ++i)
+    {
+        target(i, i) = a_pos;
+        target(i, i + 6) = a_pos * c;
+        target(i + 6, i) = a_pos * c;
+        target(i + 6, i + 6) = a_pos * b;
+    }
+    for (int i = 3; i < 6; ++i)
+    {
+        target(i, i) = a_ang;
+        target(i, i + 6) = a_ang * c;
+        target(i + 6, i) = a_ang * c;
+        target(i + 6, i + 6) = a_ang * b;
+    }
+}
+
+
+PoseVector FTNoIR_Filter::do_kalman_filter(const PoseVector &input, double dt, bool new_input)
+{
+    if (new_input)
+    {
+        dt = dt_since_last_input;
+        fill_transition_matrix(dt);
+        fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, dt);
+        kf_adaptive_process_noise_cov.update(kf, dt);
+        kf.time_update();
+        kf.measurement_update(input);
+    }
+    return kf.state.head(6);
+}
+
+
 
 FTNoIR_Filter::FTNoIR_Filter() {
     reset();
-    prev_slider_pos = s.noise_stddev_slider;
 }
 
-// the following was written by Donovan Baarda <abo@minkirri.apana.org.au>
+// The original code 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() {
-    // Setup kalman with state (x) is the 6 tracker outputs then
-    // their 6 corresponding velocities, and the measurement (z) is
-    // the 6 tracker outputs.
-    kalman.init(12, 6, 0, CV_64F);
-    // Initialize the transitionMatrix and processNoiseCov for
-    // dt=0.1. This needs to be updated each frame for the real dt
-    // value, but this hows you what they should look like. See
-    // http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
-    double dt = 0.1;
-    kalman.transitionMatrix = (cv::Mat_<double>(12, 12) <<
-    1, 0, 0, 0, 0, 0, dt, 0, 0, 0, 0, 0,
-    0, 1, 0, 0, 0, 0, 0, dt, 0, 0, 0, 0,
-    0, 0, 1, 0, 0, 0, 0, 0, dt, 0, 0, 0,
-    0, 0, 0, 1, 0, 0, 0, 0, 0, dt, 0, 0,
-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dt, 0,
-    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, dt,
-    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, 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, 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.
-    kalman.processNoiseCov = (cv::Mat_<double>(12, 12) <<
-    c, 0, 0, 0, 0, 0, b, 0, 0, 0, 0, 0,
-    0, c, 0, 0, 0, 0, 0, b, 0, 0, 0, 0,
-    0, 0, c, 0, 0, 0, 0, 0, b, 0, 0, 0,
-    0, 0, 0, c, 0, 0, 0, 0, 0, b, 0, 0,
-    0, 0, 0, 0, c, 0, 0, 0, 0, 0, b, 0,
-    0, 0, 0, 0, 0, c, 0, 0, 0, 0, 0, b,
-    b, 0, 0, 0, 0, 0, a, 0, 0, 0, 0, 0,
-    0, b, 0, 0, 0, 0, 0, a, 0, 0, 0, 0,
-    0, 0, b, 0, 0, 0, 0, 0, a, 0, 0, 0,
-    0, 0, 0, b, 0, 0, 0, 0, 0, a, 0, 0,
-    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 = (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));
+void FTNoIR_Filter::reset() 
+{
+    kf.init();
+    kf_adaptive_process_noise_cov.init();
+    for (int i = 0; i < 6; ++i)
+    {
+        // initialize part of the transition matrix that do not change.
+        kf.transition_matrix(i, i) = 1.;
+        kf.transition_matrix(i + 6, i + 6) = 1.;
+        // "extract" positions, i.e. the first 6 state dof.
+        kf.measurement_matrix(i, i) = 1.;
+    }
+
+    double noise_variance_position = settings::map_slider_value(s.noise_pos_slider_value);
+    double noise_variance_angle = settings::map_slider_value(s.noise_rot_slider_value);
+    for (int i = 0; i < 3; ++i)
+    { 
+        kf.measurement_noise_cov(i    , i    ) = noise_variance_position;
+        kf.measurement_noise_cov(i + 3, i + 3) = noise_variance_angle;
+    }
+
+    fill_transition_matrix(0.03);
+    fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, 0.03);
+
+    kf.process_noise_cov = kf_adaptive_process_noise_cov.base_cov;
+    kf.state_cov = kf.process_noise_cov;
+
     for (int i = 0; i < 6; i++) {
         last_input[i] = 0;
     }
     first_run = true;
+    dt_since_last_input = 0;
+
+    prev_slider_pos[0] = s.noise_pos_slider_value;
+    prev_slider_pos[1] = s.noise_rot_slider_value;
+
+    minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
+
+    dz_filter.reset();
 }
 
-void FTNoIR_Filter::filter(const double* input, double *output)
+
+void FTNoIR_Filter::filter(const double* input_, double *output_)
 {
-    if (prev_slider_pos != s.noise_stddev_slider)
+    // almost non-existent cost, so might as well ...
+    Eigen::Map<const PoseVector> input(input_, PoseVector::RowsAtCompileTime, 1);
+    Eigen::Map<PoseVector> output(output_, PoseVector::RowsAtCompileTime, 1);
+
+    if (prev_slider_pos[0] != s.noise_pos_slider_value ||
+        prev_slider_pos[1] != s.noise_rot_slider_value)
     {
         reset();
-        prev_slider_pos = s.noise_stddev_slider;
     }
+
     // Start the timer on first filter evaluation.
     if (first_run)
     {
@@ -84,54 +223,72 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         return;
     }
 
+    // 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 = input.cwiseNotEqual(last_input).any();
+
     // Get the time in seconds since last run and restart the timer.
     const double dt = timer.elapsed_seconds();
+    dt_since_last_input += dt;
     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.
-    double b = 0.5 * a * dt;  // (dt^3)/2 * accel_variance.
-    double c = 0.5 * b * dt;  // (dt^4)/4 * accel_variance.
-    for (int i = 0; i < 6; i++) {
-        kalman.transitionMatrix.at<double>(i,i+6) = dt;
-        kalman.processNoiseCov.at<double>(i,i) = c;
-        kalman.processNoiseCov.at<double>(i+6,i+6) = a;
-        kalman.processNoiseCov.at<double>(i,i+6) = b;
-        kalman.processNoiseCov.at<double>(i+6,i) = b;
-    }
-    // Get the updated predicted position.
-    cv::Mat next_output = kalman.predict();
-    // If we have new tracker input, get the corrected position.
-    if (new_input) {
-        cv::Mat measurement(6, 1, CV_64F);
-        for (int i = 0; i < 6; i++) {
-            measurement.at<double>(i) = input[i];
-            // Save last_input for detecting new tracker input.
-            last_input[i] = input[i];
-        }
-        next_output = kalman.correct(measurement);
+    output = do_kalman_filter(input, dt, new_input);
+    
+    {
+        // Compute deadzone size base on estimated state variance.
+        // Given a constant input plus noise, KF should converge to the true (constant) input.
+        // This works indeed. That is the output pose becomes very still afte some time.
+        // At this point the estimated cov should be minimal. We can use this to
+        // calculate the size of the deadzone, so that in the stationary state the
+        // deadzone size is zero. Thus the tracking error due to the dz-filter 
+        // becomes zero.
+        PoseVector variance = kf.state_cov.diagonal().head(6);
+        minimal_state_var = minimal_state_var.cwiseMin(variance);
+        dz_filter.dz_size = (variance - minimal_state_var).cwiseSqrt() * s.deadzone_scale;
     }
-    // Set output to the next_output.
-    for (int i = 0; i < 6; i++) {
-        output[i] = next_output.at<double>(i);
+    output = dz_filter.filter(output);
+
+    if (new_input)
+    {
+        dt_since_last_input = 0;
+        last_input = input;
     }
 }
 
+
+
+FilterControls::FilterControls()
+    : filter(nullptr)
+{
+    ui.setupUi(this);
+    connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+    connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+    connect(ui.noiseRotSlider, &QSlider::valueChanged, [=](int value) {
+        this->ui.noiseRotLabel->setText(
+            // M$ hates unicode! (M$ autoconverts source code of one kind of utf-8 format, the one without BOM, to another kind that QT does not like)
+            // We could use QChar(0x00b0). It should be totally backward compatible.
+            // u8"°" is c++11. u8 means that the string is encoded in utf8. It happens to be compatible with QT.
+            QString::number(settings::map_slider_value(value), 'f', 3) + u8" °"); 
+    });
+    connect(ui.noisePosSlider, &QSlider::valueChanged, [=](int value) {
+        this->ui.noisePosLabel->setText(
+            QString::number(settings::map_slider_value(value), 'f', 3) + " cm");
+    });
+    tie_setting(s.noise_rot_slider_value, ui.noiseRotSlider);
+    tie_setting(s.noise_pos_slider_value, ui.noisePosSlider);
+}
+
+
 void FilterControls::doOK() {
     s.b->save();
     close();
 }
 
+
 void FilterControls::doCancel()
 {
     close();
 }
 
+
 OPENTRACK_DECLARE_FILTER(FTNoIR_Filter, FilterControls, FTNoIR_FilterDll)