From 9c37cf39a9c186c2fd98db798c0a5f96ac975c5d Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sun, 31 Jul 2016 17:50:54 +0200
Subject: filter/kalman: rename header only

In Qt Creator I can jump between header and source if only they have the
same base name.
---
 filter-kalman/kalman.h | 146 +++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 146 insertions(+)
 create mode 100644 filter-kalman/kalman.h

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
new file mode 100644
index 00000000..623fa7d9
--- /dev/null
+++ b/filter-kalman/kalman.h
@@ -0,0 +1,146 @@
+#pragma once
+/* 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
+ * copyright notice and this permission notice appear in all copies.
+ */
+#ifndef INCLUDED_FTN_FILTER_H
+#define INCLUDED_FTN_FILTER_H
+
+#include "ui_ftnoir_kalman_filtercontrols.h"
+#include "opentrack/plugin-api.hpp"
+
+#include <atomic>
+#include <Eigen/Core>
+#include <Eigen/LU>
+
+#include <QString>
+#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_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);
+    PoseVector last_input;
+    Timer timer;
+    bool first_run;
+    double dt_since_last_input;
+    settings s;
+    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
+{
+public:
+    QString name() { return QString("Kalman"); }
+    QIcon icon() { return QIcon(":/images/filter-16.png"); }
+};
+
+class FilterControls: public IFilterDialog
+{
+    Q_OBJECT
+public:
+    FilterControls();
+    Ui::KalmanUICFilterControls ui;
+    void register_filter(IFilter*) override {}
+    void unregister_filter() override {}
+    settings s;
+    FTNoIR_Filter *filter;
+public slots:
+    void doOK();
+    void doCancel();
+};
+
+#endif
-- 
cgit v1.2.3


From b811a94194990b004196f0dbf0741079da6d34fb Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sun, 31 Jul 2016 17:54:38 +0200
Subject: filter/kalman: ensure data members are aligned

What a PITA.
---
 filter-kalman/kalman.h | 29 ++++++++++++++++-------------
 1 file changed, 16 insertions(+), 13 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 623fa7d9..3a5488f8 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -5,23 +5,20 @@
  * purpose with or without fee is hereby granted, provided that the above
  * copyright notice and this permission notice appear in all copies.
  */
-#ifndef INCLUDED_FTN_FILTER_H
-#define INCLUDED_FTN_FILTER_H
 
 #include "ui_ftnoir_kalman_filtercontrols.h"
 #include "opentrack/plugin-api.hpp"
+#include "opentrack-compat/options.hpp"
+using namespace options;
+#include "opentrack-compat/timer.hpp"
 
-#include <atomic>
 #include <Eigen/Core>
 #include <Eigen/LU>
 
 #include <QString>
 #include <QWidget>
 
-#include "opentrack-compat/options.hpp"
-using namespace options;
-#include "opentrack-compat/timer.hpp"
-
+#include <atomic>
 
 static constexpr int NUM_STATE_DOF = 12;
 static constexpr int NUM_MEASUREMENT_DOF = 6;
@@ -48,6 +45,8 @@ struct KalmanFilter
     void init();
     void time_update();
     void measurement_update(const PoseVector &measurement);
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 struct KalmanProcessNoiseScaler
@@ -57,6 +56,8 @@ struct KalmanProcessNoiseScaler
         base_cov; // baseline (unscaled) process noise covariance matrix
     void init();
     void update(KalmanFilter &kf, double dt);
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 
@@ -73,6 +74,8 @@ struct DeadzoneFilter
         last_output = PoseVector::Zero();
     }
     PoseVector filter(const PoseVector &input);
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 
@@ -83,7 +86,7 @@ struct settings : opts {
     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). 
+    // 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;
 
@@ -92,9 +95,9 @@ struct settings : opts {
         return std::pow(10., v * 0.04 - 3.);
     }
 
-    settings() :  
-        opts("kalman-filter"), 
-        noise_rot_slider_value(b, "noise-rotation-slider", 40), 
+    settings() :
+        opts("kalman-filter"),
+        noise_rot_slider_value(b, "noise-rotation-slider", 40),
         noise_pos_slider_value(b, "noise-position-slider", 40)
     {}
 
@@ -119,6 +122,8 @@ public:
     PoseVector minimal_state_var;
     DeadzoneFilter dz_filter;
     int prev_slider_pos[2];
+
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 class FTNoIR_FilterDll : public Metadata
@@ -142,5 +147,3 @@ public slots:
     void doOK();
     void doCancel();
 };
-
-#endif
-- 
cgit v1.2.3


From 4c0f65a9ae72fe0b2277ff704cc7f46c2467d97f Mon Sep 17 00:00:00 2001
From: DaMichel <mw.pub@welter-4d.de>
Date: Tue, 2 Aug 2016 10:58:27 +0200
Subject: filter/kalman: use fixed size matrices

---
 filter-kalman/kalman.cpp | 28 +++++++++++++---------------
 filter-kalman/kalman.h   | 23 +++++++++++++++--------
 2 files changed, 28 insertions(+), 23 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 7e2050b0..804d15da 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -16,16 +16,14 @@ constexpr double settings::process_simga_rot;
 
 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);
+    measurement_noise_cov = MeasureMatrix::Zero();
+    process_noise_cov = StateMatrix::Zero();
+    state_cov = StateMatrix::Zero();
+    state_cov_prior = StateMatrix::Zero();
+    transition_matrix = StateMatrix::Zero();
+    measurement_matrix = StateToMeasureMatrix::Zero();
+    kalman_gain = MeasureToStateMatrix::Zero();
     // initialize state variables
     state = StateVector::Zero();
     state_prior = StateVector::Zero();
@@ -42,8 +40,8 @@ void KalmanFilter::time_update()
 
 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();
+    MeasureMatrix tmp     = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov;
+    MeasureMatrix 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;
@@ -54,8 +52,8 @@ void KalmanFilter::measurement_update(const PoseVector &measurement)
 
 void KalmanProcessNoiseScaler::init()
 {
-    base_cov = Matrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF);
-    innovation_cov_estimate = Matrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF);
+    base_cov = StateMatrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF);
+    innovation_cov_estimate = MeasureMatrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF);
 }
 
 
@@ -66,7 +64,7 @@ void KalmanProcessNoiseScaler::init()
 */
 void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt)
 {
-    Matrix ddT = kf.innovation * kf.innovation.transpose();
+    MeasureMatrix ddT = kf.innovation * kf.innovation.transpose();
     double f = dt / (dt + settings::adaptivity_window_length);
     innovation_cov_estimate =
         f * ddT + (1. - f) * innovation_cov_estimate;
@@ -114,7 +112,7 @@ void FTNoIR_Filter::fill_transition_matrix(double dt)
     }
 }
 
-void FTNoIR_Filter::fill_process_noise_cov_matrix(Matrix &target, double dt) const
+void FTNoIR_Filter::fill_process_noise_cov_matrix(StateMatrix &target, double dt) const
 {
     // This model is like movement at fixed velocity plus superimposed
     // brownian motion. Unlike standard models for tracking of objects
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 3a5488f8..a9771b8e 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -22,20 +22,26 @@ using namespace options;
 
 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 StateToMeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_STATE_DOF>;
+using StateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_STATE_DOF>;
+using MeasureToStateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_MEASUREMENT_DOF>;
+using MeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF>;
 using StateVector = Eigen::Matrix<double, NUM_STATE_DOF, 1>;
 using PoseVector = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, 1>;
 
 struct KalmanFilter
 {
-    Matrix
-        measurement_noise_cov,
+    MeasureMatrix
+        measurement_noise_cov;
+    StateMatrix
         process_noise_cov,
         state_cov,
         state_cov_prior,
-        kalman_gain,
-        transition_matrix,
+        transition_matrix;
+    MeasureToStateMatrix
+        kalman_gain;
+    StateToMeasureMatrix
         measurement_matrix;
     StateVector
         state,
@@ -51,8 +57,9 @@ struct KalmanFilter
 
 struct KalmanProcessNoiseScaler
 {
-    Matrix
-        innovation_cov_estimate,
+    MeasureMatrix
+        innovation_cov_estimate;
+    StateMatrix
         base_cov; // baseline (unscaled) process noise covariance matrix
     void init();
     void update(KalmanFilter &kf, double dt);
@@ -107,7 +114,7 @@ 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;
+    void fill_process_noise_cov_matrix(StateMatrix &target, double dt) const;
 public:
     FTNoIR_Filter();
     void reset();
-- 
cgit v1.2.3


From 36266cacc451d468f450fd42e4a4e7de403f6144 Mon Sep 17 00:00:00 2001
From: DaMichel <mw.pub@welter-4d.de>
Date: Tue, 2 Aug 2016 12:55:58 +0200
Subject: filter/kalman: slider resolution shall be limited by pixels on
 screen, not by quantization of slider values.

---
 filter-kalman/ftnoir_kalman_filtercontrols.ui |  8 +++---
 filter-kalman/kalman.cpp                      | 39 ++++++++++++++++-----------
 filter-kalman/kalman.h                        | 18 +++++++------
 3 files changed, 38 insertions(+), 27 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui
index 848e5424..2e9bcae7 100644
--- a/filter-kalman/ftnoir_kalman_filtercontrols.ui
+++ b/filter-kalman/ftnoir_kalman_filtercontrols.ui
@@ -52,7 +52,7 @@
       <item row="0" column="1">
        <widget class="QSlider" name="noiseRotSlider">
         <property name="maximum">
-         <number>100</number>
+         <number>10000</number>
         </property>
         <property name="singleStep">
          <number>1</number>
@@ -67,7 +67,7 @@
          <bool>false</bool>
         </property>
         <property name="tickPosition">
-         <enum>QSlider::TicksAbove</enum>
+         <enum>QSlider::NoTicks</enum>
         </property>
         <property name="tickInterval">
          <number>10</number>
@@ -84,13 +84,13 @@
       <item row="1" column="1">
        <widget class="QSlider" name="noisePosSlider">
         <property name="maximum">
-         <number>100</number>
+         <number>10000</number>
         </property>
         <property name="orientation">
          <enum>Qt::Horizontal</enum>
         </property>
         <property name="tickPosition">
-         <enum>QSlider::TicksAbove</enum>
+         <enum>QSlider::NoTicks</enum>
         </property>
         <property name="tickInterval">
          <number>10</number>
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 804d15da..513ebf51 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -196,8 +196,8 @@ void FTNoIR_Filter::reset()
     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;
+    prev_slider_pos[0] = static_cast<slider_value>(s.noise_pos_slider_value);
+    prev_slider_pos[1] = static_cast<slider_value>(s.noise_rot_slider_value);
 
     minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
 
@@ -211,8 +211,8 @@ void FTNoIR_Filter::filter(const double* input_, double *output_)
     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)
+    if (!(prev_slider_pos[0] == s.noise_pos_slider_value &&
+          prev_slider_pos[1] == s.noise_rot_slider_value))
     {
         reset();
     }
@@ -265,19 +265,28 @@ FilterControls::FilterControls()
     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);
+
+    connect(&s.noise_rot_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&)));
+    connect(&s.noise_pos_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&)));
+
+    updateLabels(slider_value());
+}
+
+
+void FilterControls::updateLabels(const slider_value&)
+{
+    // 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)
+    // Previous attempt to use c++11 utf8 strings like u8" °" now failed for unkown 
+    // reasons where it worked before. Hence fallback to QChar(0x00b0).
+    this->ui.noiseRotLabel->setText(
+        QString::number(settings::map_slider_value(s.noise_rot_slider_value), 'f', 3) + " " + QChar(0x00b0));
+
+    this->ui.noisePosLabel->setText(
+        QString::number(settings::map_slider_value(s.noise_pos_slider_value), 'f', 3) + " cm");
 }
 
 
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index a9771b8e..cb3041ea 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -87,25 +87,26 @@ struct DeadzoneFilter
 
 
 struct settings : opts {
-    value<int> noise_rot_slider_value;
-    value<int> noise_pos_slider_value;
+    value<slider_value> noise_rot_slider_value;
+    value<slider_value> 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_sigma_pos = 0.5;
     static constexpr double process_simga_rot = 0.5;
 
-    static double map_slider_value(int v)
+    static double map_slider_value(const slider_value &v)
     {
-        return std::pow(10., v * 0.04 - 3.);
+        //return std::pow(4., v * 5. - 4.) / 4. * 10.; // not so much difference, except that it is harder to adjust the min-max range.
+        return std::pow(10., v * 4. - 3.);
     }
 
     settings() :
         opts("kalman-filter"),
-        noise_rot_slider_value(b, "noise-rotation-slider", 40),
-        noise_pos_slider_value(b, "noise-position-slider", 40)
+        noise_rot_slider_value(b, "noise-rotation-slider", slider_value(0.5, 0., 1.)),
+        noise_pos_slider_value(b, "noise-position-slider", slider_value(0.5, 0., 1.))
     {}
 
 };
@@ -128,7 +129,7 @@ public:
     KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
     PoseVector minimal_state_var;
     DeadzoneFilter dz_filter;
-    int prev_slider_pos[2];
+    slider_value prev_slider_pos[2];
 
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
@@ -153,4 +154,5 @@ public:
 public slots:
     void doOK();
     void doCancel();
+    void updateLabels(const slider_value&);
 };
-- 
cgit v1.2.3


From 9f7dc5c005d7acbb0dcbf748787d29dac203e825 Mon Sep 17 00:00:00 2001
From: DaMichel <mw.pub@welter-4d.de>
Date: Tue, 2 Aug 2016 13:11:05 +0200
Subject: filter/kalman: deadzone adjustment and parameter tuning

---
 filter-kalman/kalman.cpp | 7 ++++---
 filter-kalman/kalman.h   | 3 +--
 2 files changed, 5 insertions(+), 5 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 513ebf51..d885960d 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -199,7 +199,7 @@ void FTNoIR_Filter::reset()
     prev_slider_pos[0] = static_cast<slider_value>(s.noise_pos_slider_value);
     prev_slider_pos[1] = static_cast<slider_value>(s.noise_rot_slider_value);
 
-    minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
+    //minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
 
     dz_filter.reset();
 }
@@ -245,8 +245,9 @@ void FTNoIR_Filter::filter(const double* input_, double *output_)
         // 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;
+        //minimal_state_var = minimal_state_var.cwiseMin(variance);
+        //dz_filter.dz_size = (variance - minimal_state_var).cwiseSqrt() * s.deadzone_scale;
+        dz_filter.dz_size = variance.cwiseSqrt() * s.deadzone_scale;
     }
     output = dz_filter.filter(output);
 
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index cb3041ea..57eb8e97 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -93,7 +93,6 @@ struct settings : opts {
     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.5;
     static constexpr double process_simga_rot = 0.5;
 
@@ -127,7 +126,7 @@ public:
     settings s;
     KalmanFilter kf;
     KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
-    PoseVector minimal_state_var;
+    //PoseVector minimal_state_var;
     DeadzoneFilter dz_filter;
     slider_value prev_slider_pos[2];
 
-- 
cgit v1.2.3


From 561c0021630481d9dfba3987a47c1a142be9a2ba Mon Sep 17 00:00:00 2001
From: DaMichel <mw.pub@welter-4d.de>
Date: Tue, 2 Aug 2016 13:31:53 +0200
Subject: filter/kalman: changed copyright notice

---
 filter-kalman/kalman.cpp | 2 +-
 filter-kalman/kalman.h   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index d885960d..1d71e3f6 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) 2016 Michael Welter <mw.pub@welter-4d.de>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 57eb8e97..3bb5aad2 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -1,5 +1,5 @@
 #pragma once
-/* Copyright (c) 2013 Stanislaw Halik <sthalik@misaki.pl>
+/* Copyright (c) 2016 Michael Welter <mw.pub@welter-4d.de>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
-- 
cgit v1.2.3


From e1658b7ffaae49085ea8ef501b24b8645b071b85 Mon Sep 17 00:00:00 2001
From: DaMichel <mw.pub@welter-4d.de>
Date: Thu, 4 Aug 2016 10:06:36 +0200
Subject: filter/kalman: log sliders that only change a single decimal digit at
 a time when moved

---
 filter-kalman/ftnoir_kalman_filtercontrols.ui | 24 +++++++++++++++---------
 filter-kalman/kalman.h                        | 27 ++++++++++++++++++++++++---
 2 files changed, 39 insertions(+), 12 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui
index 2e9bcae7..6ec18ba8 100644
--- a/filter-kalman/ftnoir_kalman_filtercontrols.ui
+++ b/filter-kalman/ftnoir_kalman_filtercontrols.ui
@@ -9,7 +9,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>431</width>
+    <width>438</width>
     <height>141</height>
    </rect>
   </property>
@@ -52,11 +52,14 @@
       <item row="0" column="1">
        <widget class="QSlider" name="noiseRotSlider">
         <property name="maximum">
-         <number>10000</number>
+         <number>400</number>
         </property>
         <property name="singleStep">
          <number>1</number>
         </property>
+        <property name="pageStep">
+         <number>100</number>
+        </property>
         <property name="tracking">
          <bool>true</bool>
         </property>
@@ -67,10 +70,10 @@
          <bool>false</bool>
         </property>
         <property name="tickPosition">
-         <enum>QSlider::NoTicks</enum>
+         <enum>QSlider::TicksBelow</enum>
         </property>
         <property name="tickInterval">
-         <number>10</number>
+         <number>100</number>
         </property>
        </widget>
       </item>
@@ -84,16 +87,19 @@
       <item row="1" column="1">
        <widget class="QSlider" name="noisePosSlider">
         <property name="maximum">
-         <number>10000</number>
+         <number>400</number>
+        </property>
+        <property name="pageStep">
+         <number>100</number>
         </property>
         <property name="orientation">
          <enum>Qt::Horizontal</enum>
         </property>
         <property name="tickPosition">
-         <enum>QSlider::NoTicks</enum>
+         <enum>QSlider::TicksBelow</enum>
         </property>
         <property name="tickInterval">
-         <number>10</number>
+         <number>100</number>
         </property>
        </widget>
       </item>
@@ -101,7 +107,7 @@
        <widget class="QLabel" name="noiseRotLabel">
         <property name="minimumSize">
          <size>
-          <width>60</width>
+          <width>65</width>
           <height>0</height>
          </size>
         </property>
@@ -114,7 +120,7 @@
        <widget class="QLabel" name="noisePosLabel">
         <property name="minimumSize">
          <size>
-          <width>60</width>
+          <width>65</width>
           <height>0</height>
          </size>
         </property>
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 3bb5aad2..3c9466a5 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -96,10 +96,31 @@ struct settings : opts {
     static constexpr double process_sigma_pos = 0.5;
     static constexpr double process_simga_rot = 0.5;
 
-    static double map_slider_value(const slider_value &v)
+    static double map_slider_value(const slider_value &v_)
     {
-        //return std::pow(4., v * 5. - 4.) / 4. * 10.; // not so much difference, except that it is harder to adjust the min-max range.
-        return std::pow(10., v * 4. - 3.);
+        const double v = v_;
+#if 0
+        //return std::pow(10., v * 4. - 3.);
+#else
+        constexpr int min_log10 = -3;
+        constexpr int max_log10 = 1;
+        constexpr int num_divisions = max_log10 - min_log10;
+        /* ascii art representation of slider
+          // ----- //  ------//  ------// ------- //   4 divisions
+         -3    -   2         -1        0           1    power of 10
+                   |      |
+                   |      f + left_side_log10
+                   |
+                   left_side_log10
+        */
+        const int k = v * num_divisions; // in which division are we?!
+        const double f = v * num_divisions - k; // where in the division are we?!
+        const double ff = f * 9. + 1.;
+        const double multiplier = int(ff * 10.) / 10.;
+        const int left_side_log10 = min_log10 + k;
+        const double val = std::pow(10., left_side_log10) * multiplier;
+        return val;
+#endif
     }
 
     settings() :
-- 
cgit v1.2.3


From b798b0ae5a971bbb43c6f47b667f108a747bfffe Mon Sep 17 00:00:00 2001
From: DaMichel <mw.pub@welter-4d.de>
Date: Thu, 4 Aug 2016 13:31:23 +0200
Subject: filter/kalman: deadzone tweaks

---
 filter-kalman/kalman.cpp | 17 +++++++----------
 filter-kalman/kalman.h   |  5 ++---
 2 files changed, 9 insertions(+), 13 deletions(-)

(limited to 'filter-kalman/kalman.h')

diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
index 1d71e3f6..6ed5ca91 100644
--- a/filter-kalman/kalman.cpp
+++ b/filter-kalman/kalman.cpp
@@ -199,8 +199,6 @@ void FTNoIR_Filter::reset()
     prev_slider_pos[0] = static_cast<slider_value>(s.noise_pos_slider_value);
     prev_slider_pos[1] = static_cast<slider_value>(s.noise_rot_slider_value);
 
-    //minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
-
     dz_filter.reset();
 }
 
@@ -238,15 +236,14 @@ void FTNoIR_Filter::filter(const double* input_, double *output_)
 
     {
         // 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.
+        // Given a constant input plus measurement noise, KF should converge to the true input.
+        // This works well. That is the output pose becomes very still afte some time.
+        // The QScaling adaptive filter makes the state cov vary depending on the estimated noise
+        // and the measured noise of the innovation sequence. After a sudden movement it peaks
+        // and then decays asymptotically to some constant value taken in stationary state. 
+        // We can use this to calculate the size of the deadzone, so that in the stationary state the
+        // deadzone size is small. Thus the tracking error due to the dz-filter becomes also small.
         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;
         dz_filter.dz_size = variance.cwiseSqrt() * s.deadzone_scale;
     }
     output = dz_filter.filter(output);
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
index 3c9466a5..aaed3bc1 100644
--- a/filter-kalman/kalman.h
+++ b/filter-kalman/kalman.h
@@ -91,8 +91,8 @@ struct settings : opts {
     value<slider_value> 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;
+    static constexpr double deadzone_scale = 2;
+    static constexpr double deadzone_exponent = 2.0;
     static constexpr double process_sigma_pos = 0.5;
     static constexpr double process_simga_rot = 0.5;
 
@@ -147,7 +147,6 @@ public:
     settings s;
     KalmanFilter kf;
     KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
-    //PoseVector minimal_state_var;
     DeadzoneFilter dz_filter;
     slider_value prev_slider_pos[2];
 
-- 
cgit v1.2.3