summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--filter-kalman/kalman.cpp7
-rw-r--r--filter-kalman/kalman.h3
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<slider_value>(s.noise_pos_slider_value);
prev_slider_pos[1] = static_cast<slider_value>(s.noise_rot_slider_value);
- minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::max());
+ //minimal_state_var = PoseVector::Constant(std::numeric_limits<double>::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];