diff options
Diffstat (limited to 'filter-kalman')
| -rw-r--r-- | filter-kalman/kalman.cpp | 3 | ||||
| -rw-r--r-- | filter-kalman/kalman.h | 28 |
2 files changed, 17 insertions, 14 deletions
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp index f88d7f90..1960e0f6 100644 --- a/filter-kalman/kalman.cpp +++ b/filter-kalman/kalman.cpp @@ -192,7 +192,6 @@ void kalman::reset() 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; @@ -311,7 +310,7 @@ double settings::map_slider_value(const slider_value& v_) | left_side_log10 */ - const int k = v * num_divisions; // in which division are we?! + const int k = int(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.; diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h index 30f9e90f..2d8db86e 100644 --- a/filter-kalman/kalman.h +++ b/filter-kalman/kalman.h @@ -26,8 +26,8 @@ using namespace options; #include <QString> #include <QWidget> -static constexpr inline int NUM_STATE_DOF = 12; -static constexpr inline int NUM_MEASUREMENT_DOF = 6; +static constexpr int NUM_STATE_DOF = 12; +static constexpr int NUM_MEASUREMENT_DOF = 6; // 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>; @@ -86,11 +86,11 @@ struct settings : opts { value<slider_value> noise_rot_slider_value { b, "noise-rotation-slider", { .5, 0, 1 } }; value<slider_value> noise_pos_slider_value { b, "noise-position-slider", { .5, 0, 1 } }; - static constexpr inline double adaptivity_window_length = 0.25; // seconds - static constexpr inline double deadzone_scale = 8; - static constexpr inline double deadzone_exponent = 2.0; - static constexpr inline double process_sigma_pos = 0.5; - static constexpr inline double process_sigma_rot = 0.5; + static constexpr double adaptivity_window_length = 0.25; // seconds + static constexpr double deadzone_scale = 8; + static constexpr double deadzone_exponent = 2.0; + static constexpr double process_sigma_pos = 0.5; + static constexpr double process_sigma_rot = 0.5; static double map_slider_value(const slider_value &v); @@ -109,15 +109,19 @@ public: void center() override { reset(); } module_status initialize() override { return status_ok(); } - PoseVector last_input; - Timer timer; - bool first_run; double dt_since_last_input; - settings s; + PoseVector last_input; KalmanFilter kf; KalmanProcessNoiseScaler kf_adaptive_process_noise_cov; DeadzoneFilter dz_filter; - slider_value prev_slider_pos[2]; + settings s; + slider_value prev_slider_pos[2] { + *s.noise_pos_slider_value, + *s.noise_rot_slider_value, + }; + Timer timer; + + bool first_run = true; }; class kalmanDll : public Metadata |
