diff options
| -rw-r--r-- | filter-kalman/kalman.cpp | 7 | ||||
| -rw-r--r-- | filter-kalman/kalman.h | 3 |
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]; |