diff options
| -rw-r--r-- | filter-kalman/CMakeLists.txt | 9 | ||||
| -rw-r--r-- | filter-kalman/ftnoir_filter_kalman.h | 113 | ||||
| -rw-r--r-- | filter-kalman/ftnoir_kalman_filtercontrols.ui | 77 | ||||
| -rw-r--r-- | filter-kalman/kalman.cpp | 325 |
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) |