summaryrefslogtreecommitdiffhomepage
path: root/filter-kalman
diff options
context:
space:
mode:
Diffstat (limited to 'filter-kalman')
-rw-r--r--filter-kalman/kalman.cpp3
-rw-r--r--filter-kalman/kalman.h28
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