From e24ba82d73cb565a9bac07592840b16389fbb0e6 Mon Sep 17 00:00:00 2001 From: DaMichel Date: Fri, 29 Jul 2016 10:38:22 +0200 Subject: 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 --- filter-kalman/CMakeLists.txt | 9 +- filter-kalman/ftnoir_filter_kalman.h | 113 +++++++-- filter-kalman/ftnoir_kalman_filtercontrols.ui | 77 +++++- 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 -#include -#include + +#include +#include +#include + #include -#include #include + #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; +using PoseVector = Eigen::Matrix; + +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 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 noise_rot_slider_value; + 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_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 @@ 0 0 - 288 - 132 + 442 + 153 @@ -37,26 +37,55 @@ - - - QFrame::NoFrame + + + Measurement Noise - - QFrame::Raised - - + - Noise standard deviation + Rotation - + + + 100 + 1 + + true + + + Qt::Horizontal + + + false + + + QSlider::TicksAbove + + + 10 + + + + + + + Position + + + + + + + 100 + Qt::Horizontal @@ -68,6 +97,32 @@ + + + + + 60 + 0 + + + + ° + + + + + + + + 60 + 0 + + + + - + + + 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 +/* Copyright (c) 2013 Stanislaw Halik * * 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 #include -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 +// The original code was written by Donovan Baarda // 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_(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_(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::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 input(input_, PoseVector::RowsAtCompileTime, 1); + Eigen::Map 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(i,i+6) = dt; - kalman.processNoiseCov.at(i,i) = c; - kalman.processNoiseCov.at(i+6,i+6) = a; - kalman.processNoiseCov.at(i,i+6) = b; - kalman.processNoiseCov.at(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(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(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) -- cgit v1.2.3 From 9c37cf39a9c186c2fd98db798c0a5f96ac975c5d Mon Sep 17 00:00:00 2001 From: Stanislaw Halik 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/ftnoir_filter_kalman.h | 146 ----------------------------------- filter-kalman/kalman.h | 146 +++++++++++++++++++++++++++++++++++ 2 files changed, 146 insertions(+), 146 deletions(-) delete mode 100644 filter-kalman/ftnoir_filter_kalman.h create mode 100644 filter-kalman/kalman.h diff --git a/filter-kalman/ftnoir_filter_kalman.h b/filter-kalman/ftnoir_filter_kalman.h deleted file mode 100644 index 623fa7d9..00000000 --- a/filter-kalman/ftnoir_filter_kalman.h +++ /dev/null @@ -1,146 +0,0 @@ -#pragma once -/* Copyright (c) 2013 Stanislaw Halik - * - * 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 -#include -#include - -#include -#include - -#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; -using PoseVector = Eigen::Matrix; - -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 noise_rot_slider_value; - 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_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 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 + * + * 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 +#include +#include + +#include +#include + +#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; +using PoseVector = Eigen::Matrix; + +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 noise_rot_slider_value; + 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_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 f786a5c3e89f52a457744c966cc28f5fe99ad9b4 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 31 Jul 2016 17:52:31 +0200 Subject: filter/kalman: find eigen in non-standard locations --- cmake/FindEigen3.cmake | 81 ++++++++++++++++++++++++++++++++++++++++++++ filter-kalman/CMakeLists.txt | 8 +++-- 2 files changed, 86 insertions(+), 3 deletions(-) create mode 100644 cmake/FindEigen3.cmake diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake new file mode 100644 index 00000000..fc29db7e --- /dev/null +++ b/cmake/FindEigen3.cmake @@ -0,0 +1,81 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + #include(FindPackageHandleStandardArgs) + #find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) + diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt index 12de11ec..1d1c6412 100644 --- a/filter-kalman/CMakeLists.txt +++ b/filter-kalman/CMakeLists.txt @@ -1,3 +1,5 @@ -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 +find_package(Eigen3 QUIET) +if(EIGEN3_FOUND) + opentrack_boilerplate(opentrack-filter-kalman) + target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR}) +endif() -- cgit v1.2.3 From 4a6c5ebbe11df575dd1fb29d7beae387dab05b3c Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 31 Jul 2016 17:53:20 +0200 Subject: filter/kalman: remove @dbaarda's copyright from dialog @DaMichel still needs to change to his copyright in file headers. --- filter-kalman/ftnoir_kalman_filtercontrols.ui | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui index 7533beed..848e5424 100644 --- a/filter-kalman/ftnoir_kalman_filtercontrols.ui +++ b/filter-kalman/ftnoir_kalman_filtercontrols.ui @@ -9,8 +9,8 @@ 0 0 - 442 - 153 + 431 + 141 @@ -126,13 +126,6 @@ - - - - Written by Donovan Baarda, 2014 - - - -- cgit v1.2.3 From 10ea343b3fea8a156f761deadbb58342746e32bf Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 31 Jul 2016 17:54:09 +0200 Subject: filter/kalman: fix "static constexpr double" ODR --- filter-kalman/kalman.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp index 3f117399..660bc0cc 100644 --- a/filter-kalman/kalman.cpp +++ b/filter-kalman/kalman.cpp @@ -1,14 +1,19 @@ -/* Copyright (c) 2013 Stanislaw Halik +/* Copyright (c) 2013 Stanislaw Halik * * 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. */ -#include "ftnoir_filter_kalman.h" +#include "kalman.h" #include "opentrack/plugin-api.hpp" #include #include +constexpr double settings::adaptivity_window_length; +constexpr double settings::deadzone_scale; +constexpr double settings::deadzone_exponent; +constexpr double settings::process_sigma_pos; +constexpr double settings::process_simga_rot; void KalmanFilter::init() { @@ -55,9 +60,9 @@ void KalmanProcessNoiseScaler::init() } -/* Uses +/* Uses innovation, measurement_matrix, measurement_noise_cov, and state_cov_prior - found in KalmanFilter. It sets + found in KalmanFilter. It sets process_noise_cov */ void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt) @@ -161,7 +166,7 @@ FTNoIR_Filter::FTNoIR_Filter() { // The original code was written by Donovan Baarda // https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b -void FTNoIR_Filter::reset() +void FTNoIR_Filter::reset() { kf.init(); kf_adaptive_process_noise_cov.init(); @@ -177,7 +182,7 @@ void FTNoIR_Filter::reset() 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; } @@ -233,14 +238,14 @@ void FTNoIR_Filter::filter(const double* input_, double *output_) timer.start(); 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 + // 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); @@ -268,7 +273,7 @@ FilterControls::FilterControls() // 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" °"); + QString::number(settings::map_slider_value(value), 'f', 3) + u8" °"); }); connect(ui.noisePosSlider, &QSlider::valueChanged, [=](int value) { this->ui.noisePosLabel->setText( -- cgit v1.2.3 From b811a94194990b004196f0dbf0741079da6d34fb Mon Sep 17 00:00:00 2001 From: Stanislaw Halik 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(-) 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 #include #include #include #include -#include "opentrack-compat/options.hpp" -using namespace options; -#include "opentrack-compat/timer.hpp" - +#include 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 7dfed29631b5568a203fb1c5052748ab831476ac Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sun, 31 Jul 2016 18:00:53 +0200 Subject: filter/kalman: fix typo The "abs" in global namespace is an integer-only version. Use "fabs" from cmath instead. --- filter-kalman/kalman.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp index 660bc0cc..7e2050b0 100644 --- a/filter-kalman/kalman.cpp +++ b/filter-kalman/kalman.cpp @@ -5,7 +5,6 @@ * copyright notice and this permission notice appear in all copies. */ #include "kalman.h" -#include "opentrack/plugin-api.hpp" #include #include @@ -95,7 +94,7 @@ PoseVector DeadzoneFilter::filter(const PoseVector &input) if (dz > 0.) { const double delta = input[i] - last_output[i]; - const double f = pow(abs(delta) / dz, settings::deadzone_exponent); + const double f = std::pow(std::fabs(delta) / dz, settings::deadzone_exponent); const double response = f / (f + 1.) * delta; out[i] = last_output[i] + response; } -- cgit v1.2.3 From 4c0f65a9ae72fe0b2277ff704cc7f46c2467d97f Mon Sep 17 00:00:00 2001 From: DaMichel 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(-) 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; +using StateMatrix = Eigen::Matrix; +using MeasureToStateMatrix = Eigen::Matrix; +using MeasureMatrix = Eigen::Matrix; using StateVector = Eigen::Matrix; using PoseVector = Eigen::Matrix; 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 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(-) 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 @@ - 100 + 10000 1 @@ -67,7 +67,7 @@ false - QSlider::TicksAbove + QSlider::NoTicks 10 @@ -84,13 +84,13 @@ - 100 + 10000 Qt::Horizontal - QSlider::TicksAbove + QSlider::NoTicks 10 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(s.noise_pos_slider_value); + prev_slider_pos[1] = static_cast(s.noise_rot_slider_value); minimal_state_var = PoseVector::Constant(std::numeric_limits::max()); @@ -211,8 +211,8 @@ void FTNoIR_Filter::filter(const double* input_, double *output_) Eigen::Map input(input_, PoseVector::RowsAtCompileTime, 1); Eigen::Map 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 noise_rot_slider_value; - value noise_pos_slider_value; + value noise_rot_slider_value; + 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 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(-) 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(s.noise_pos_slider_value); prev_slider_pos[1] = static_cast(s.noise_rot_slider_value); - minimal_state_var = PoseVector::Constant(std::numeric_limits::max()); + //minimal_state_var = PoseVector::Constant(std::numeric_limits::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 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(-) 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 +/* Copyright (c) 2016 Michael Welter * * 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 +/* Copyright (c) 2016 Michael Welter * * 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 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(-) 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 @@ 0 0 - 431 + 438 141 @@ -52,11 +52,14 @@ - 10000 + 400 1 + + 100 + true @@ -67,10 +70,10 @@ false - QSlider::NoTicks + QSlider::TicksBelow - 10 + 100 @@ -84,16 +87,19 @@ - 10000 + 400 + + + 100 Qt::Horizontal - QSlider::NoTicks + QSlider::TicksBelow - 10 + 100 @@ -101,7 +107,7 @@ - 60 + 65 0 @@ -114,7 +120,7 @@ - 60 + 65 0 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 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(-) 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(s.noise_pos_slider_value); prev_slider_pos[1] = static_cast(s.noise_rot_slider_value); - //minimal_state_var = PoseVector::Constant(std::numeric_limits::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 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