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