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 (limited to 'filter-kalman') 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